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