diff options
Diffstat (limited to 'intern/iksolver/intern/IK_QChain.cpp')
| -rw-r--r-- | intern/iksolver/intern/IK_QChain.cpp | 280 |
1 files changed, 280 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_QChain.cpp b/intern/iksolver/intern/IK_QChain.cpp new file mode 100644 index 00000000000..438729003a9 --- /dev/null +++ b/intern/iksolver/intern/IK_QChain.cpp @@ -0,0 +1,280 @@ +/** + * $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 ***** + */ + +/** + + * $Id$ + * Copyright (C) 2001 NaN Technologies B.V. + * + * @author Laurence + */ + +#include "IK_QChain.h" + +using namespace std; + +IK_QChain:: +IK_QChain( +) +{ + // nothing to do; +}; + +const + vector<IK_QSegment> & +IK_QChain:: +Segments( +) const { + return m_segments; +}; + + vector<IK_QSegment> & +IK_QChain:: +Segments( +){ + return m_segments; +}; + + void +IK_QChain:: +UpdateGlobalTransformations( +){ + + // now iterate through the segment list + // compute their local transformations if needed + + // assign their global transformations + // (relative to chain origin) + + vector<IK_QSegment>::const_iterator s_end = m_segments.end(); + vector<IK_QSegment>::iterator s_it = m_segments.begin(); + + MT_Transform global; + global.setIdentity(); + + for (; s_it != s_end; ++s_it) { + global = s_it->UpdateGlobal(global); + } + + // we also need to compute the accumulated local transforms + // for each segment + + MT_Transform acc_local; + acc_local.setIdentity(); + + vector<IK_QSegment>::reverse_iterator s_rit = m_segments.rbegin(); + vector<IK_QSegment>::reverse_iterator s_rend = m_segments.rend(); + + for (; s_rit != s_rend; ++s_rit) { + acc_local = s_rit->UpdateAccumulatedLocal(acc_local); + } + + // 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(); + +#if 0 + + // The end pose is not currently used. + + MT_Matrix3x3 last_basis = last_t.getBasis(); + last_basis.transpose(); + MT_Vector3 m_end_pose = last_basis[1]; + +#endif + +}; + +const + TNT::Matrix<MT_Scalar> & +IK_QChain:: +Jacobian( +) const { + return m_jacobian; +} ; + + +const + TNT::Matrix<MT_Scalar> & +IK_QChain:: +TransposedJacobian( +) const { + return m_t_jacobian; +}; + + void +IK_QChain:: +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_QSegment>::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++) { +#if 0 + + // For euler angle computation we can use a slightly quicker method. + + const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis(); + const MT_Vector3 &origin = segs[n].GlobalSegmentStart(); + + const MT_Vector3 p = origin-m_end_effector; + + const MT_Vector3 x_axis(1,0,0); + const MT_Vector3 y_axis(0,1,0); + const MT_Vector3 z_axis(0,0,1); + + MT_Vector3 a = basis * x_axis; + MT_Vector3 pca = p.cross(a); + + m_t_jacobian(n*3 + 1,1) = pca.x(); + m_t_jacobian(n*3 + 1,2) = pca.y(); + m_t_jacobian(n*3 + 1,3) = pca.z(); + + a = basis * y_axis; + pca = p.cross(a); + + m_t_jacobian(n*3 + 2,1) = pca.x(); + m_t_jacobian(n*3 + 2,2) = pca.y(); + m_t_jacobian(n*3 + 2,3) = pca.z(); + + a = basis * z_axis; + pca = p.cross(a); + + m_t_jacobian(n*3 + 3,1) = pca.x(); + m_t_jacobian(n*3 + 3,2) = pca.y(); + m_t_jacobian(n*3 + 3,3) = pca.z(); +#else + // user slower general jacobian computation method + + MT_Vector3 j1 = segs[n].ComputeJacobianColumn(0); + + m_t_jacobian(n*3 + 1,1) = j1.x(); + m_t_jacobian(n*3 + 1,2) = j1.y(); + m_t_jacobian(n*3 + 1,3) = j1.z(); + + j1 = segs[n].ComputeJacobianColumn(1); + + m_t_jacobian(n*3 + 2,1) = j1.x(); + m_t_jacobian(n*3 + 2,2) = j1.y(); + m_t_jacobian(n*3 + 2,3) = j1.z(); + + j1 = segs[n].ComputeJacobianColumn(2); + + m_t_jacobian(n*3 + 3,1) = j1.x(); + m_t_jacobian(n*3 + 3,2) = j1.y(); + m_t_jacobian(n*3 + 3,3) = j1.z(); +#endif + + + + } + + + // get the origina1 jacobain + + TNT::transpose(m_t_jacobian,m_jacobian); +}; + + MT_Vector3 +IK_QChain:: +EndEffector( +) const { + return m_end_effector; +}; + + MT_Vector3 +IK_QChain:: +EndPose( +) const { + return m_end_pose; +}; + + + int +IK_QChain:: +DoF( +) const { + return 3 * m_segments.size(); +} + +const + MT_Scalar +IK_QChain:: +MaxExtension( +) const { + + vector<IK_QSegment>::const_iterator s_end = m_segments.end(); + vector<IK_QSegment>::const_iterator s_it = m_segments.begin(); + + if (m_segments.size() == 0) return MT_Scalar(0); + + MT_Scalar output = s_it->Length(); + + ++s_it ; + for (; s_it != s_end; ++s_it) { + output += s_it->MaxExtension(); + } + return output; +} + + + + + + + + + + + + + + + + + + + + |