Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / eigen_types.hpp
1 /* $Id: eigen_types.hpp 19905 2009-04-23 13:29:54Z ben2610 $
2  * eigen_types.hpp
3  *
4  *  Created on: March 6, 2009
5  *      Author: benoit bolsee
6  */
7
8 #ifndef EIGEN_TYPES_HPP_
9 #define EIGEN_TYPES_HPP_
10
11 #include <Eigen/Core>
12 #include "kdl/frames.hpp"
13 #include "kdl/tree.hpp"
14 #include "kdl/chain.hpp"
15 #include "kdl/jacobian.hpp"
16 #include "kdl/jntarray.hpp"
17
18
19 namespace iTaSC{
20
21 using KDL::Twist;
22 using KDL::Frame;
23 using KDL::Joint;
24 using KDL::Inertia;
25 using KDL::SegmentMap;
26 using KDL::Tree;
27 using KDL::JntArray;
28 using KDL::Jacobian;
29 using KDL::Segment;
30 using KDL::Rotation;
31 using KDL::Vector;
32 using KDL::Vector2;
33 using KDL::Chain;
34
35 extern const Frame F_identity;
36
37 #define e_scalar double
38 #define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
39 #define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
40 #define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
41 #define e_matrix6 Eigen::Matrix<e_scalar,6,6>
42 #define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
43 #define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
44 #define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
45 #define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
46 #define e_vector6 Eigen::Matrix<e_scalar,6,1>
47 #define e_vector3 Eigen::Matrix<e_scalar,3,1>
48
49 class Range {
50 public:
51         int start;
52         int count;
53         Range(int _start, int _count) { start = _start; count=_count; }
54         Range(const Range& other) { start=other.start; count=other.count; }
55 };
56
57 template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
58 {
59         return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
60 }
61
62 template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
63 {
64         return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
65 }
66
67 template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
68
69     if (J.rows() != 6)
70         return -1;
71     for (int j = 0; j < J.cols(); ++j) {
72                 typename Derived::ColXpr Jj = J.col(j);
73                 Twist arg;
74         for(unsigned int i=0;i<6;++i)
75             arg(i)=Jj[i];
76                 Twist tmp(T*arg);
77         for(unsigned int i=0;i<6;++i)
78             Jj[i]=e_scalar(tmp(i));
79     }
80     return 0;
81 }
82
83 }
84 #endif /* UBLAS_TYPES_HPP_ */