Logo Search packages:      
Sourcecode: blender version File versions

IK_QSegment.cpp

/**
 * $Id: IK_QSegment.cpp,v 1.5 2003/05/01 19:52:20 sirdude Exp $
 * ***** 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: IK_QSegment.cpp,v 1.5 2003/05/01 19:52:20 sirdude Exp $
 * Copyright (C) 2001 NaN Technologies B.V.
 *
 * @author Laurence
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "IK_QSegment.h"
#include <iostream>

IK_QSegment::
00048 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::
00068 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::
00087 Length(
) const {
      return m_length;
}

const 
      MT_Scalar
IK_QSegment::
00095 MaxExtension(
) const {
      return m_max_extension;
}
      
// This is the transform from adjacent
// coordinate systems in the chain.

const 
      MT_Transform &
IK_QSegment::
00106 LocalTransform(
) const {
      return m_local_transform;
}

const
      MT_ExpMap &
IK_QSegment::
LocalJointParameter(
) const {
      return m_q;
}

      MT_Transform  
IK_QSegment::
00121 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::
00139 GlobalTransform(
) const {
      return m_global_transform * m_local_transform;
}
      

      MT_Transform
IK_QSegment::
00147 UpdateAccumulatedLocal(
      const MT_Transform & acc_local
){
      m_accum_local = acc_local;
      return m_local_transform * m_accum_local;
}           

const 
      MT_Transform &
IK_QSegment::
00157 AccumulatedLocal(
) const {
      return m_accum_local;
}

      MT_Vector3  
IK_QSegment::
00164 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::
00291 GlobalSegmentStart(
) const{
      return m_seg_start;
}

const       
      MT_Vector3 &
IK_QSegment::
00299 GlobalSegmentEnd(
) const {
      return m_seg_end;
}


      void
IK_QSegment::
00307 IncrementAngle(
      const MT_Vector3 &dq
){
      m_q.vector() += dq;
      MT_Scalar theta(0);
      m_q.reParameterize(theta);

      UpdateLocalTransform();
}
 

      void
IK_QSegment::
00320 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;
};







Generated by  Doxygen 1.6.0   Back to index