diff options
Diffstat (limited to 'intern/iksolver/intern/IK_Chain.cpp')
| -rw-r--r-- | intern/iksolver/intern/IK_Chain.cpp | 208 |
1 files changed, 208 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_Chain.cpp b/intern/iksolver/intern/IK_Chain.cpp new file mode 100644 index 00000000000..875b8da1405 --- /dev/null +++ b/intern/iksolver/intern/IK_Chain.cpp @@ -0,0 +1,208 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "IK_Chain.h" + + +using namespace std; + +IK_Chain:: +IK_Chain( +) +{ + // nothing to do; +}; + +const + vector<IK_Segment> & +IK_Chain:: +Segments( +) const { + return m_segments; +}; + + vector<IK_Segment> & +IK_Chain:: +Segments( +){ + return m_segments; +}; + + void +IK_Chain:: +UpdateGlobalTransformations( +){ + + // now iterate through the segment list + // compute their local transformations if needed + + // assign their global transformations + // (relative to chain origin) + + vector<IK_Segment>::const_iterator s_end = m_segments.end(); + vector<IK_Segment>::iterator s_it = m_segments.begin(); + + MT_Transform global; + + global.setIdentity(); + + for (; s_it != s_end; ++s_it) { + s_it->UpdateGlobal(global); + global = s_it->GlobalTransform(); + } + // compute the position of the end effector and it's pose + + const MT_Transform &last_t = m_segments.back().GlobalTransform(); + m_end_effector = last_t.getOrigin(); + + MT_Matrix3x3 last_basis = last_t.getBasis(); + last_basis.transpose(); + MT_Vector3 m_end_pose = last_basis[2]; + + +}; + +const + TNT::Matrix<MT_Scalar> & +IK_Chain:: +Jacobian( +) const { + return m_jacobian; +} ; + + +const + TNT::Matrix<MT_Scalar> & +IK_Chain:: +TransposedJacobian( +) const { + return m_t_jacobian; +}; + + void +IK_Chain:: +ComputeJacobian( +){ + // let's assume that the chain's global transfomations + // have already been computed. + + int dof = DoF(); + + int num_segs = m_segments.size(); + vector<IK_Segment>::const_iterator segs = m_segments.begin(); + + m_t_jacobian.newsize(dof,3); + m_jacobian.newsize(3,dof); + + // compute the transposed jacobian first + + int n; + int i = 0; + + + for (n= 0; n < num_segs; n++) { + + const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis(); + const MT_Vector3 &origin = segs[n].GlobalSegmentStart(); + + MT_Vector3 p = origin-m_end_effector; + + // iterate through the active angle vectors of this segment + + int angle_ind =0; + int seg_dof = segs[n].DoF(); + + const std::vector<MT_Vector3> & angle_vectors = segs[n].AngleVectors(); + + for (angle_ind = 0;angle_ind <seg_dof; angle_ind++,i++) { + + MT_Vector3 angle_axis = angle_vectors[angle_ind]; + + MT_Vector3 a = basis * angle_axis; + MT_Vector3 pca = p.cross(a); + + m_t_jacobian(i + 1,1) = pca.x(); + m_t_jacobian(i + 1,2) = pca.y(); + m_t_jacobian(i + 1,3) = pca.z(); + + } + } + + // get the origina1 jacobain + + TNT::transpose(m_t_jacobian,m_jacobian); +}; + + MT_Vector3 +IK_Chain:: +EndEffector( +) const { + return m_end_effector; +}; + + MT_Vector3 +IK_Chain:: +EndPose( +) const { + return m_end_pose; +}; + + + int +IK_Chain:: +DoF( +) const { + + // iterate through the segs and compute the DOF + + vector<IK_Segment>::const_iterator s_end = m_segments.end(); + vector<IK_Segment>::const_iterator s_it = m_segments.begin(); + + int dof = 0; + + for (;s_it != s_end; ++s_it) { + dof += s_it->DoF(); + } + + return dof; +} + + + + + + + + + + + + |