diff options
Diffstat (limited to 'intern/iksolver/intern/IK_QSegment.cpp')
| -rw-r--r-- | intern/iksolver/intern/IK_QSegment.cpp | 363 |
1 files changed, 363 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp new file mode 100644 index 00000000000..2956dd815e2 --- /dev/null +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -0,0 +1,363 @@ +/** + * $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_QSegment.h" +#include <iostream.h> + +IK_QSegment:: +IK_QSegment ( + const MT_Point3 tr1, + const MT_Matrix3x3 A, + const MT_Scalar length, + const MT_ExpMap q +) : + m_length (length), + m_q (q) +{ + + m_max_extension = tr1.length() + length; + + m_transform.setOrigin(tr1); + m_transform.setBasis(A); + + UpdateLocalTransform(); +}; + + +IK_QSegment:: +IK_QSegment ( +) : + m_length(0), + m_q (0,0,0), + m_max_extension(0) +{ + // Intentionally empty +} + + + +// accessors +//////////// + +// The length of the segment + +const + MT_Scalar +IK_QSegment:: +Length( +) const { + return m_length; +} + +const + MT_Scalar +IK_QSegment:: +MaxExtension( +) const { + return m_max_extension; +} + +// This is the transform from adjacent +// coordinate systems in the chain. + +const + MT_Transform & +IK_QSegment:: +LocalTransform( +) const { + return m_local_transform; +} + +const + MT_ExpMap & +IK_QSegment:: +LocalJointParameter( +) const { + return m_q; +} + + MT_Transform +IK_QSegment:: +UpdateGlobal( + const MT_Transform & global +){ + // compute the global transform + // and the start of the segment in global coordinates. + + m_seg_start = global * m_transform.getOrigin(); + m_global_transform = global; + + const MT_Transform new_global = GlobalTransform(); + + m_seg_end = new_global.getOrigin(); + + return new_global; +} + + MT_Transform +IK_QSegment:: +GlobalTransform( +) const { + return m_global_transform * m_local_transform; +} + + + MT_Transform +IK_QSegment:: +UpdateAccumulatedLocal( + const MT_Transform & acc_local +){ + m_accum_local = acc_local; + return m_local_transform * m_accum_local; +} + +const + MT_Transform & +IK_QSegment:: +AccumulatedLocal( +) const { + return m_accum_local; +} + + MT_Vector3 +IK_QSegment:: +ComputeJacobianColumn( + int angle +) const { + + + MT_Transform translation; + translation.setIdentity(); + translation.translate(MT_Vector3(0,m_length,0)); + + + // we can compute the jacobian for one of the + // angles of this joint by first computing + // the partial derivative of the local transform dR/da + // and then computing + // dG/da = m_global_transform * dR/da * m_accum_local (0,0,0) + +#if 0 + + // use euler angles as a test of the matrices and this method. + + MT_Matrix3x3 dRda; + + MT_Quaternion rotx,roty,rotz; + + rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); + roty.setRotation(MT_Vector3(0,1,0),m_q[1]); + rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); + + MT_Matrix3x3 rotx_m(rotx); + MT_Matrix3x3 roty_m(roty); + MT_Matrix3x3 rotz_m(rotz); + + if (angle == 0) { + + MT_Scalar ci = cos(m_q[0]); + MT_Scalar si = sin(m_q[1]); + + dRda = MT_Matrix3x3( + 0, 0, 0, + 0,-si,-ci, + 0, ci,-si + ); + + dRda = rotz_m * roty_m * dRda; + } else + + if (angle == 1) { + + MT_Scalar cj = cos(m_q[1]); + MT_Scalar sj = sin(m_q[1]); + + dRda = MT_Matrix3x3( + -sj, 0, cj, + 0, 0, 0, + -cj, 0,-sj + ); + + dRda = rotz_m * dRda * rotx_m; + } else + + if (angle == 2) { + + MT_Scalar ck = cos(m_q[2]); + MT_Scalar sk = sin(m_q[2]); + + dRda = MT_Matrix3x3( + -sk,-ck, 0, + ck,-sk, 0, + 0, 0, 0 + ); + + dRda = dRda * roty_m * rotx_m; + } + + MT_Transform dRda_t(MT_Point3(0,0,0),dRda); + + // convert to 4x4 matrices coz dRda is singular. + MT_Matrix4x4 dRda_m(dRda_t); + dRda_m[3][3] = 0; + +#else + + // use exponential map derivatives + MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle); + +#endif + + + // Once we have computed the local derivatives we can compute + // derivatives of the end effector. + + // Imagine a chain consisting of 5 segments each with local + // transformation Li + // Then the global transformation G is L1 *L2 *L3 *L4 *L5 + // If we want to compute the partial derivatives of this expression + // w.r.t one of the angles x of L3 we should then compute + // dG/dx = d(L1 *L2 *L3 *L4 *L5)/dx + // = L1 *L2 * dL3/dx *L4 *L5 + // but L1 *L2 is the global transformation of the parent of this + // bone and L4 *L5 is the accumulated local transform of the children + // of this bone (m_accum_local) + // so dG/dx = m_global_transform * dL3/dx * m_accum_local + // + // so now we need to compute dL3/dx + // L3 = m_transform * rotation(m_q) * translate(0,m_length,0) + // do using the same procedure we get + // dL3/dx = m_transform * dR/dx * translate(0,m_length,0) + // dR/dx is the partial derivative of the exponential map w.r.t + // one of it's parameters. This is computed in MT_ExpMap + + MT_Matrix4x4 translation_m (translation); + MT_Matrix4x4 global_m(m_global_transform); + MT_Matrix4x4 accum_local_m(m_accum_local); + MT_Matrix4x4 transform_m(m_transform); + + + MT_Matrix4x4 dFda_m = global_m * transform_m * dRda_m * translation_m * accum_local_m; + + MT_Vector4 result = dFda_m * MT_Vector4(0,0,0,1); + return MT_Vector3(result[0],result[1],result[2]); +}; + + + +const + MT_Vector3 & +IK_QSegment:: +GlobalSegmentStart( +) const{ + return m_seg_start; +} + +const + MT_Vector3 & +IK_QSegment:: +GlobalSegmentEnd( +) const { + return m_seg_end; +} + + + void +IK_QSegment:: +IncrementAngle( + const MT_Vector3 &dq +){ + m_q.vector() += dq; + MT_Scalar theta(0); + m_q.reParameterize(theta); + + UpdateLocalTransform(); +} + + + void +IK_QSegment:: +SetAngle( + const MT_ExpMap &dq +){ + m_q = dq; + UpdateLocalTransform(); +} + + + void +IK_QSegment:: +UpdateLocalTransform( +){ +#if 0 + + //use euler angles - test + MT_Quaternion rotx,roty,rotz; + + rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); + roty.setRotation(MT_Vector3(0,1,0),m_q[1]); + rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); + + MT_Matrix3x3 rotx_m(rotx); + MT_Matrix3x3 roty_m(roty); + MT_Matrix3x3 rotz_m(rotz); + + MT_Matrix3x3 rot = rotz_m * roty_m * rotx_m; +#else + + //use exponential map + MT_Matrix3x3 rot = m_q.getMatrix(); + + +#endif + + MT_Transform rx(MT_Point3(0,0,0),rot); + + MT_Transform translation; + translation.setIdentity(); + translation.translate(MT_Vector3(0,m_length,0)); + + m_local_transform = m_transform * rx * translation; +}; + + + + + + |