Logo Search packages:      
Sourcecode: blender version File versions

IK_Solver.cpp

/**
 * $Id: IK_Solver.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 *****
 */

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

#define IK_USE_EXPMAP



#ifdef IK_USE_EXPMAP 
#     include "IK_QSolver_Class.h"
#else 
#     include "IK_Solver_Class.h"
#endif
#include "../extern/IK_solver.h"

#include <iostream>

      IK_Chain_ExternPtr  
IK_CreateChain(
      void
) {

      MEM_SmartPtr<IK_Chain_Extern> output (new IK_Chain_Extern);
      MEM_SmartPtr<IK_QSolver_Class> output_void (IK_QSolver_Class::New());   
      

      if (output == NULL || output_void == NULL) {
            return NULL;
      }

      output->chain_dof = 0;
      output->num_segments = 0;
      output->segments = NULL;
      output->intern = output_void.Release();
      return output.Release();
};


      int 
IK_LoadChain(
      IK_Chain_ExternPtr chain,
      IK_Segment_ExternPtr segments,
      int num_segs
) {

      if (chain == NULL || segments == NULL) return 0;

      IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
      if (intern_cpp == NULL) return 0;

      std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments(); 
      if (segs.size() != num_segs) {
            segs = std::vector<IK_QSegment>(num_segs);
      }

      std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
      std::vector<IK_QSegment>::iterator seg_begin= segs.begin();

      IK_Segment_ExternPtr extern_seg_it = segments;
      

      for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) {
      
            MT_Point3 tr1(extern_seg_it->seg_start);

            MT_Matrix3x3 A(
                  extern_seg_it->basis[0],extern_seg_it->basis[1],extern_seg_it->basis[2],
                  extern_seg_it->basis[3],extern_seg_it->basis[4],extern_seg_it->basis[5],
                  extern_seg_it->basis[6],extern_seg_it->basis[7],extern_seg_it->basis[8]
            );

            MT_Scalar length(extern_seg_it->length);


            *seg_begin = IK_QSegment( 
                  tr1,A,length,MT_Vector3(0,0,0)
            );

      }


      intern_cpp->Chain().UpdateGlobalTransformations();
      intern_cpp->Chain().ComputeJacobian();
      
      chain->num_segments = num_segs;
      chain->chain_dof = intern_cpp->Chain().DoF();
      chain->segments = segments;

      return 1;
};          
            
      int 
IK_SolveChain(
      IK_Chain_ExternPtr chain,
      float goal[3],
      float tolerance,
      int max_iterations,
      float max_angle_change, 
      IK_Segment_ExternPtr output
){
      if (chain == NULL || output == NULL) return 0;
      if (chain->intern == NULL) return 0;

      IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;

      IK_QJacobianSolver & solver = intern_cpp->Solver();

      bool solve_result = solver.Solve(
            intern_cpp->Chain(),
            MT_Vector3(goal),
            MT_Vector3(0,0,0),
            MT_Scalar(tolerance),
            max_iterations,
            MT_Scalar(max_angle_change)   
      );
      
      // turn the computed role->pitch->yaw into a quaternion and 
      // return the result in output
      
      std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments(); 
      std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
      std::vector<IK_QSegment>::iterator seg_begin= segs.begin();

      for (;seg_begin != seg_end; ++seg_begin, ++output) {
            MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix();

            // don't forget to transpose this qrot for use by blender!

            qrot.transpose(); // blender uses transpose here ????

            output->basis_change[0] = float(qrot[0][0]);
            output->basis_change[1] = float(qrot[0][1]);
            output->basis_change[2] = float(qrot[0][2]);
            output->basis_change[3] = float(qrot[1][0]);
            output->basis_change[4] = float(qrot[1][1]);
            output->basis_change[5] = float(qrot[1][2]);
            output->basis_change[6] = float(qrot[2][0]);
            output->basis_change[7] = float(qrot[2][1]);
            output->basis_change[8] = float(qrot[2][2]);

      }


      return solve_result ? 1 : 0;
}

      void 
IK_FreeChain(
      IK_Chain_ExternPtr chain
){
      IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;

      delete(intern_cpp);
      delete(chain);

}     





      
      


      




Generated by  Doxygen 1.6.0   Back to index