Logo Search packages:      
Sourcecode: blender version File versions

IK_QChain.cpp

/**
 * $Id: IK_QChain.cpp,v 1.4 2002/11/25 09:53:01 mein 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_QChain.cpp,v 1.4 2002/11/25 09:53:01 mein Exp $
 * Copyright (C) 2001 NaN Technologies B.V.
 *
 * @author Laurence
 */

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

#include "IK_QChain.h"

using namespace std;

IK_QChain::
00049 IK_QChain(
)
{
      // nothing to do;
};

const 
      vector<IK_QSegment> &
IK_QChain::
00058 Segments(
) const {
      return m_segments;
};

      vector<IK_QSegment> &
IK_QChain::
00065 Segments(
){
      return m_segments;
};    

      void
IK_QChain::
00072 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::
00124 Jacobian(
) const  {
      return m_jacobian;
} ;
 

const 
      TNT::Matrix<MT_Scalar> &
IK_QChain::
00133 TransposedJacobian(
) const {
      return m_t_jacobian;
};

      void
IK_QChain::
00140 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;

      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::
00225 EndEffector(
) const {
      return m_end_effector;
};

      MT_Vector3 
IK_QChain::
00232 EndPose(
) const {
      return m_end_pose;
};
            

      int
IK_QChain::
00240 DoF(
) const {
      return 3 * m_segments.size();
}

const 
      MT_Scalar
IK_QChain::
00248 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;
}


Generated by  Doxygen 1.6.0   Back to index