1 // This file is part of libigl, a simple c++ geometry processing library.
2 //
3 // Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public License
6 // v. 2.0. If a copy of the MPL was not distributed with this file, You can
7 // obtain one at http://mozilla.org/MPL/2.0/.
8 #include "forward_kinematics.h"
9 #include <functional>
10 #include <iostream>
11
forward_kinematics(const Eigen::MatrixXd & C,const Eigen::MatrixXi & BE,const Eigen::VectorXi & P,const std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & dQ,const std::vector<Eigen::Vector3d> & dT,std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & vQ,std::vector<Eigen::Vector3d> & vT)12 IGL_INLINE void igl::forward_kinematics(
13 const Eigen::MatrixXd & C,
14 const Eigen::MatrixXi & BE,
15 const Eigen::VectorXi & P,
16 const std::vector<
17 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
18 const std::vector<Eigen::Vector3d> & dT,
19 std::vector<
20 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
21 std::vector<Eigen::Vector3d> & vT)
22 {
23 using namespace std;
24 using namespace Eigen;
25 const int m = BE.rows();
26 assert(m == P.rows());
27 assert(m == (int)dQ.size());
28 assert(m == (int)dT.size());
29 vector<bool> computed(m,false);
30 vQ.resize(m);
31 vT.resize(m);
32 // Dynamic programming
33 function<void (int) > fk_helper = [&] (int b)
34 {
35 if(!computed[b])
36 {
37 if(P(b) < 0)
38 {
39 // base case for roots
40 vQ[b] = dQ[b];
41 const Vector3d r = C.row(BE(b,0)).transpose();
42 vT[b] = r-dQ[b]*r + dT[b];
43 }else
44 {
45 // Otherwise first compute parent's
46 const int p = P(b);
47 fk_helper(p);
48 vQ[b] = vQ[p] * dQ[b];
49 const Vector3d r = C.row(BE(b,0)).transpose();
50 vT[b] = vT[p] - vQ[b]*r + vQ[p]*(r + dT[b]);
51 }
52 computed[b] = true;
53 }
54 };
55 for(int b = 0;b<m;b++)
56 {
57 fk_helper(b);
58 }
59 }
60
forward_kinematics(const Eigen::MatrixXd & C,const Eigen::MatrixXi & BE,const Eigen::VectorXi & P,const std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & dQ,std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & vQ,std::vector<Eigen::Vector3d> & vT)61 IGL_INLINE void igl::forward_kinematics(
62 const Eigen::MatrixXd & C,
63 const Eigen::MatrixXi & BE,
64 const Eigen::VectorXi & P,
65 const std::vector<
66 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
67 std::vector<
68 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
69 std::vector<Eigen::Vector3d> & vT)
70 {
71 std::vector<Eigen::Vector3d> dT(BE.rows(),Eigen::Vector3d(0,0,0));
72 return forward_kinematics(C,BE,P,dQ,dT,vQ,vT);
73 }
74
forward_kinematics(const Eigen::MatrixXd & C,const Eigen::MatrixXi & BE,const Eigen::VectorXi & P,const std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & dQ,const std::vector<Eigen::Vector3d> & dT,Eigen::MatrixXd & T)75 IGL_INLINE void igl::forward_kinematics(
76 const Eigen::MatrixXd & C,
77 const Eigen::MatrixXi & BE,
78 const Eigen::VectorXi & P,
79 const std::vector<
80 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
81 const std::vector<Eigen::Vector3d> & dT,
82 Eigen::MatrixXd & T)
83 {
84 using namespace Eigen;
85 using namespace std;
86 vector< Quaterniond,aligned_allocator<Quaterniond> > vQ;
87 vector< Vector3d> vT;
88 forward_kinematics(C,BE,P,dQ,dT,vQ,vT);
89 const int dim = C.cols();
90 T.resize(BE.rows()*(dim+1),dim);
91 for(int e = 0;e<BE.rows();e++)
92 {
93 Affine3d a = Affine3d::Identity();
94 a.translate(vT[e]);
95 a.rotate(vQ[e]);
96 T.block(e*(dim+1),0,dim+1,dim) =
97 a.matrix().transpose().block(0,0,dim+1,dim);
98 }
99 }
100
forward_kinematics(const Eigen::MatrixXd & C,const Eigen::MatrixXi & BE,const Eigen::VectorXi & P,const std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> & dQ,Eigen::MatrixXd & T)101 IGL_INLINE void igl::forward_kinematics(
102 const Eigen::MatrixXd & C,
103 const Eigen::MatrixXi & BE,
104 const Eigen::VectorXi & P,
105 const std::vector<
106 Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
107 Eigen::MatrixXd & T)
108 {
109 std::vector<Eigen::Vector3d> dT(BE.rows(),Eigen::Vector3d(0,0,0));
110 return forward_kinematics(C,BE,P,dQ,dT,T);
111 }
112
113 #ifdef IGL_STATIC_LIBRARY
114 // Explicit template instantiation
115 #endif
116