First commit of Brecht's new IK work. This only does the IK module.
authorTon Roosendaal <ton@blender.org>
Sat, 27 Aug 2005 12:44:41 +0000 (12:44 +0000)
committerTon Roosendaal <ton@blender.org>
Sat, 27 Aug 2005 12:44:41 +0000 (12:44 +0000)
Don't start compiling yet!

intern/iksolver/intern/IK_QChain.cpp [deleted file]
intern/iksolver/intern/IK_QChain.h [deleted file]
intern/iksolver/intern/IK_QJacobianSolver.cpp
intern/iksolver/intern/IK_QJacobianSolver.h
intern/iksolver/intern/IK_QSegment.cpp
intern/iksolver/intern/IK_QSegment.h
intern/iksolver/intern/IK_QSolver_Class.h [deleted file]
intern/iksolver/intern/IK_Solver.cpp
intern/iksolver/intern/MT_ExpMap.cpp
intern/iksolver/intern/MT_ExpMap.h
intern/iksolver/intern/Makefile

diff --git a/intern/iksolver/intern/IK_QChain.cpp b/intern/iksolver/intern/IK_QChain.cpp
deleted file mode 100644 (file)
index de62195..0000000
+++ /dev/null
@@ -1,264 +0,0 @@
-/**
- * $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
- */
-
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
-
-#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;
-
-       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;
-}
-
diff --git a/intern/iksolver/intern/IK_QChain.h b/intern/iksolver/intern/IK_QChain.h
deleted file mode 100644 (file)
index df249fe..0000000
+++ /dev/null
@@ -1,211 +0,0 @@
-/**
- * $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
- */
-
-#ifndef NAN_INCLUDED_IK_QChain_h
-#define NAN_INCLUDED_IK_QChain_h
-
-
-#include "IK_QSegment.h"
-#include <vector>
-#include "MT_Scalar.h"
-#include "TNT/cmat.h"
-
-/**
- * This class is a collection of ordered segments that are used
- * in an Inverse Kinematic solving routine. An IK solver operating
- * on the chain, will in general manipulate all the segments of the
- * chain in order to solve the IK problem.
- * 
- * To build a chain use the default constructor. Once built it's
- * then possible to add IK_Segments to the chain by inserting
- * them into the vector of IK_Segments. Note that segments will be
- * copied into the chain so chains cannot share instances of 
- * IK_Segments. 
- * 
- * You have full control of which segments form the chain via the 
- * the std::vector routines. 
- */
-
-class IK_QChain{
-
-public :
-
-       /**
-        * Construct a IK_QChain with no segments.
-        */
-
-       IK_QChain(
-       );
-
-       // IK_QChains also have the default copy constructors
-       // available. 
-
-       /** 
-     * Const access to the array of segments
-        * comprising the IK_QChain. Used for rendering
-        * etc
-     * @return a const reference to a vector of segments
-        */
-
-       const 
-               std::vector<IK_QSegment> &
-       Segments(
-       ) const ;
-
-
-       /**
-        * Full access to segments used to initialize
-        * the IK_QChain and manipulate the segments.
-        * Use the push_back() method of std::vector to add
-     * segments in order to the chain
-     * @return a reference to a vector of segments
-        */
-
-               std::vector<IK_QSegment> &
-       Segments(
-       );
-
-
-       /** 
-        * Force the IK_QChain to recompute all the local 
-        * segment transformations and composite them
-        * to calculate the global transformation for 
-        * each segment. Must be called before 
-        * ComputeJacobian()
-     */
-
-               void
-       UpdateGlobalTransformations(
-       );
-
-       /** 
-        * Return the global position of the end
-        * of the last segment.
-        */
-
-               MT_Vector3 
-       EndEffector(
-       ) const;
-
-
-       /** 
-        * Return the global pose of the end
-        * of the last segment.
-        */
-
-               MT_Vector3
-       EndPose(
-       ) const;
-
-
-       /** 
-        * Calculate the jacobian matrix for
-        * the current end effector position.
-        * A jacobian is the set of column vectors
-        * of partial derivatives for each active angle.
-        * This method also computes the transposed jacobian.
-        * @pre You must have updated the global transformations
-        * of the chain's segments before a call to this method. Do this
-        * with UpdateGlobalTransformation()
-        */
-
-               void
-       ComputeJacobian(
-       );
-       
-
-       /** 
-        * @return A const reference to the last computed jacobian matrix
-        */
-
-       const 
-               TNT::Matrix<MT_Scalar> &
-       Jacobian(
-       ) const ;
-
-       /** 
-        * @return A const reference to the last computed transposed jacobian matrix
-        */
-
-       const 
-               TNT::Matrix<MT_Scalar> &
-       TransposedJacobian(
-       ) const ;
-
-       /** 
-        * Count the degrees of freedom in the IK_QChain
-        * @warning store this value rather than using this function
-        * as the termination value of a for loop etc.
-        */
-
-               int
-       DoF(
-       ) const;
-
-       /** 
-        * Compute the maximum extension of the chain from the
-        * root segment. This is the length of the root segments
-        * + the max extensions of all the other segments
-        */
-       
-       const 
-               MT_Scalar
-       MaxExtension(
-       ) const;
-
-
-private :
-
-       /// The vector of segments comprising the chain
-       std::vector<IK_QSegment> m_segments;
-
-       /// The jacobain of the IK_QChain
-       TNT::Matrix<MT_Scalar> m_jacobian;
-
-       /// It's transpose
-       TNT::Matrix<MT_Scalar> m_t_jacobian;
-
-       MT_Vector3 m_end_effector;              
-       MT_Vector3 m_end_pose;
-
-
-};
-
-#endif
-
index 450baf8fa9e1021c5ae4e6ec79a5b6f92c26613e..4bd3d28ce69aef7163ec36b41faf7d0d069f4cde 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original Author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
 
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
-
 #include "IK_QJacobianSolver.h"
 
-#include "TNT/svd.h"
+//#include "analyze.h"
 
+#include <iostream>
 using namespace std;
 
-       IK_QJacobianSolver *
-IK_QJacobianSolver::
-New(
-){
-       return new IK_QJacobianSolver();
-}
+void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
+{
+       m_segments.push_back(seg);
 
-       bool
-IK_QJacobianSolver::
-Solve(
-       IK_QChain &chain,
-       const MT_Vector3 &g_position,
-       const MT_Vector3 &g_pose,
-       const MT_Scalar tolerance,
-       const int max_iterations,
-       const MT_Scalar max_angle_change
-){
-
-       const vector<IK_QSegment> & segs = chain.Segments();
-       if (segs.size() == 0) return false;
-       
-       // compute the goal direction from the base of the chain
-       // in global coordinates
-
-       MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart();
-       
+       IK_QSegment *child;
+       for (child = seg->Child(); child; child = child->Sibling())
+               AddSegmentList(child);
+}
 
-       const MT_Scalar chain_max_extension = chain.MaxExtension();
+bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
+{
+       m_segments.clear();
+       AddSegmentList(root);
 
-       bool do_parallel_check(false);
+       // assing each segment a unique id for the jacobian
+       std::vector<IK_QSegment*>::iterator seg;
+       int num_dof = 0;
 
-       if (chain_max_extension < goal_dir.length()) {
-               do_parallel_check = true;
+       for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+               (*seg)->SetDoFId(num_dof);
+               num_dof += (*seg)->NumberOfDoF();
        }
 
-       goal_dir.normalize();
-
+       // compute task id's and assing weights to task
+       int primary_size = 0, primary = 0;
+       int secondary_size = 0, secondary = 0;
+       MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
+       std::list<IK_QTask*>::iterator task;
 
-       ArmMatrices(chain.DoF());
-
-       for (int iterations = 0; iterations < max_iterations; iterations++) {
-               
-               // check to see if the chain is pointing in the right direction
+       for (task = tasks.begin(); task != tasks.end(); task++) {
+               IK_QTask *qtask = *task;
 
-               if (iterations%32 && do_parallel_check && ParallelCheck(chain,goal_dir)) {
-
-                       return false;
+               if (qtask->Primary()) {
+                       qtask->SetId(primary_size);
+                       primary_size += qtask->Size();
+                       primary_weight += qtask->Weight();
+                       primary++;
+               }
+               else {
+                       qtask->SetId(secondary_size);
+                       secondary_size += qtask->Size();
+                       secondary_weight += qtask->Weight();
+                       secondary++;
                }
-               
-               MT_Vector3 end_effector = chain.EndEffector();
-               MT_Vector3 d_pos = g_position - end_effector;
-               const MT_Scalar x_length = d_pos.length();
-               
-               if (x_length < tolerance) {
-                       return true;
-               }       
-
-               chain.ComputeJacobian();
-
-        try {
-                   ComputeInverseJacobian(chain,x_length,max_angle_change);
-        }
-        catch(...) {
-            return false;
-        }
-               
-               ComputeBetas(chain,d_pos);
-               UpdateChain(chain);
-               chain.UpdateGlobalTransformations();
        }
 
+       if (primary_size == 0 || MT_fuzzyZero(primary_weight))
+               return false;
 
-       return false;
-};
-
-IK_QJacobianSolver::
-~IK_QJacobianSolver(
-){
-       // nothing to do
-}
-
-
-IK_QJacobianSolver::
-IK_QJacobianSolver(
-){
-       // nothing to do
-};
-       void
-IK_QJacobianSolver::
-ComputeBetas(
-       IK_QChain &chain,
-       const MT_Vector3 d_pos
-){     
-
-       m_beta = 0;
-
-       m_beta[0] = d_pos.x();
-       m_beta[1] = d_pos.y();
-       m_beta[2] = d_pos.z();
+       m_secondary_enabled = (secondary > 0);
        
-       TNT::matmult(m_d_theta,m_svd_inverse,m_beta);
-
-};     
-
-
-       int
-IK_QJacobianSolver::
-ComputeInverseJacobian(
-       IK_QChain &chain,
-       const MT_Scalar x_length,
-       const MT_Scalar max_angle_change
-) {
-
-       int dimension = 0;
-
-       m_svd_u = MT_Scalar(0);
-
-       // copy first 3 rows of jacobian into m_svd_u
-
-       int row, column;
+       // rescale weights of tasks to sum up to 1
+       MT_Scalar primary_rescale = 1.0/primary_weight;
+       MT_Scalar secondary_rescale;
+       if (MT_fuzzyZero(secondary_weight))
+               secondary_rescale = 0.0;
+       else
+               secondary_rescale = 1.0/secondary_weight;
+       
+       for (task = tasks.begin(); task != tasks.end(); task++) {
+               IK_QTask *qtask = *task;
 
-       for (row = 0; row < 3; row ++) {
-               for (column = 0; column < chain.Jacobian().num_cols(); column ++) {
-                       m_svd_u[row][column] = chain.Jacobian()[row][column];
-               }
+               if (qtask->Primary())
+                       qtask->SetWeight(qtask->Weight()*primary_rescale);
+               else
+                       qtask->SetWeight(qtask->Weight()*secondary_rescale);
        }
 
-       m_svd_w = MT_Scalar(0);
-       m_svd_v = MT_Scalar(0);
-
-    m_svd_work_space = MT_Scalar(0);
-
-       TNT::SVD(m_svd_u,m_svd_w,m_svd_v,m_svd_work_space);
-
-       // invert the SVD and compute inverse
-
-       TNT::transpose(m_svd_v,m_svd_v_t);
-       TNT::transpose(m_svd_u,m_svd_u_t);
-
-       // Compute damped least squares inverse of pseudo inverse
-       // Compute damping term lambda
-
-       // Note when lambda is zero this is equivalent to the
-       // least squares solution. This is fine when the m_jjt is
-       // of full rank. When m_jjt is near to singular the least squares
-       // inverse tries to minimize |J(dtheta) - dX)| and doesn't 
-       // try to minimize  dTheta. This results in eratic changes in angle.
-       // Damped least squares minimizes |dtheta| to try and reduce this
-       // erratic behaviour.
-
-       // We don't want to use the damped solution everywhere so we
-       // only increase lamda from zero as we approach a singularity.
+       // set matrix sizes
+       m_jacobian.ArmMatrices(num_dof, primary_size, primary);
+       if (secondary > 0)
+               m_jacobian_sub.ArmMatrices(num_dof, secondary_size, secondary);
 
-       // find the smallest non-zero m_svd_w value
-
-       int i = 0;
-
-       MT_Scalar w_min = MT_INFINITY;
-
-       // anything below epsilon is treated as zero
+       // set dof weights
+       int i;
 
-       MT_Scalar epsilon = MT_Scalar(1e-10);
+       for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
+               for (i = 0; i < (*seg)->NumberOfDoF(); i++)
+                       m_jacobian.SetDoFWeight((*seg)->DoFId()+i, (*seg)->Weight(i));
 
-       for ( i = 0; i <m_svd_w.size() ; i++) {
+       return true;
+}
 
-               if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) {
-                       w_min = m_svd_w[i];
+bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
+{
+       // assing each segment a unique id for the jacobian
+       std::vector<IK_QSegment*>::iterator seg;
+       IK_QSegment *qseg, *minseg = NULL;
+       MT_Scalar minabsdelta = 1e10, absdelta;
+       MT_Vector3 delta, mindelta;
+       bool locked = false, clamp[3];
+       int i, mindof = 0;
+
+       for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+               qseg = *seg;
+               if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
+                       for (i = 0; i < qseg->NumberOfDoF(); i++) {
+                               if (clamp[i] && !qseg->Locked(i)) {
+                                       absdelta = MT_abs(delta[i]);
+
+                                       if (absdelta < MT_EPSILON) {
+                                               qseg->Lock(i, m_jacobian, delta);
+                                               locked = true;
+                                       }
+                                       else if (absdelta < minabsdelta) {
+                                               minabsdelta = absdelta;
+                                               mindelta = delta;
+                                               minseg = qseg;
+                                               mindof = i;
+                                       }
+                               }
+                       }
                }
        }
 
-       MT_Scalar lambda(0);
+       if (minseg) {
+               minseg->Lock(mindof, m_jacobian, mindelta);
+               locked = true;
 
-       MT_Scalar d = x_length/max_angle_change;
-
-       if (w_min <= d/2) {
-               lambda = d/2;
-       } else
-       if (w_min < d) {
-               lambda = sqrt(w_min*(d - w_min));
-       } else {
-               lambda = MT_Scalar(0);
+               if (minabsdelta > norm)
+                       norm = minabsdelta;
        }
 
-
-       lambda *= lambda;
-
-       for (i= 0; i < m_svd_w.size(); i++) {
-               if (m_svd_w[i] < epsilon) {
-                       m_svd_w[i] = MT_Scalar(0);
-               } else {
-                       m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
+       if (locked == false)
+               for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+                       (*seg)->UnLock();
+                       (*seg)->UpdateAngleApply();
                }
-       }
-
-
-       TNT::matmultdiag(m_svd_temp1,m_svd_w,m_svd_u_t);
-       TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
-
-       return dimension;
-
+       
+       return locked;
 }
 
-       void
-IK_QJacobianSolver::
-UpdateChain(
-       IK_QChain &chain
-){
-
-       // iterate through the set of angles and 
-       // update their values from the d_thetas
+bool IK_QJacobianSolver::Solve(
+       IK_QSegment *root,
+       std::list<IK_QTask*> tasks,
+       MT_Scalar,
+       int max_iterations
+)
+{
+       //double dt = analyze_time();
 
-       vector<IK_QSegment> &segs = chain.Segments(); 
-       
-       unsigned int seg_ind = 0;
-       for (seg_ind = 0;seg_ind < segs.size(); seg_ind++) {
-       
-               MT_Vector3 dq;
-               dq[0] = m_d_theta[3*seg_ind];
-               dq[1] = m_d_theta[3*seg_ind + 1];
-               dq[2] = m_d_theta[3*seg_ind + 2];
-               segs[seg_ind].IncrementAngle(dq);       
-       }
+       if (!Setup(root, tasks))
+               return false;
 
-};
-       
-       void
-IK_QJacobianSolver::
-ArmMatrices(
-       int dof
-){
-
-       m_beta.newsize(dof);
-       m_d_theta.newsize(dof);
-
-       m_svd_u.newsize(dof,dof);
-       m_svd_v.newsize(dof,dof);
-       m_svd_w.newsize(dof);
-
-    m_svd_work_space.newsize(dof);
-
-       m_svd_u = MT_Scalar(0);
-       m_svd_v = MT_Scalar(0);
-       m_svd_w = MT_Scalar(0);
-
-       m_svd_u_t.newsize(dof,dof);
-       m_svd_v_t.newsize(dof,dof);
-       m_svd_w_diag.newsize(dof,dof);
-       m_svd_inverse.newsize(dof,dof);
-       m_svd_temp1.newsize(dof,dof);
-
-};
-
-       bool
-IK_QJacobianSolver::
-ParallelCheck(
-       const IK_QChain &chain,
-       const MT_Vector3 goal_dir
-) const {
-       
-       // compute the start of the chain in global coordinates
-       const vector<IK_QSegment> &segs = chain.Segments(); 
+       // iterate
+       for (int iterations = 0; iterations < max_iterations; iterations++) {
+               // update transform
+               root->UpdateTransform(MT_Transform::Identity());
 
-       int num_segs = segs.size();
-       
-       if (num_segs == 0) {
-               return false;
-       }
+               std::list<IK_QTask*>::iterator task;
 
-       MT_Scalar crossp_sum = 0;
+               //bool done = true;
 
-       int i;
-       for (i = 0; i < num_segs; i++) {
-               MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() - 
-                       segs[i].GlobalSegmentStart();
+               // compute jacobian
+               for (task = tasks.begin(); task != tasks.end(); task++) {
+                       if ((*task)->Primary())
+                               (*task)->ComputeJacobian(m_jacobian);
+                       else
+                               (*task)->ComputeJacobian(m_jacobian_sub);
 
-               global_seg_direction.normalize();       
+                       //printf("#> %f\n", (*task)->Distance());
+                       //if ((*task)->Distance() > 1e-4)
+                       //      done = false;
+               }
 
-               MT_Scalar crossp = (global_seg_direction.cross(goal_dir)).length();
-               crossp_sum += MT_Scalar(fabs(crossp));
+               /*if (done) {
+                       //analyze_add_run(iterations, analyze_time()-dt);
+                       return true;
+               }*/
+
+               MT_Scalar norm = 0.0;
+
+               do {
+                       // invert jacobian
+                       try {
+                               m_jacobian.Invert();
+                               if (m_secondary_enabled)
+                                       m_jacobian.SubTask(m_jacobian_sub);
+                       }
+                       catch (...) {
+                               printf("IK Exception\n");
+                               return false;
+                       }
+
+                       // update angles and check limits
+               } while (UpdateAngles(norm));
+
+               // unlock segments again after locking in clamping loop
+               std::vector<IK_QSegment*>::iterator seg;
+               for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
+                       (*seg)->UnLock();
+
+               // compute angle update norm
+               MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
+               if (maxnorm > norm)
+                       norm = maxnorm;
+
+               // check for convergence
+               if (norm < 1e-3) {
+                       //analyze_add_run(iterations, analyze_time()-dt);
+                       return true;
+               }
        }
 
-       if (crossp_sum < MT_Scalar(0.01)) {
-               return true;
-       } else {
-               return false;
-       }
+       //analyze_add_run(max_iterations, analyze_time()-dt);
+       return false;
 }
 
-
index 2a01fd05f83e81d6d4270d8d999ccf948f26e810..caa47b775396b8a7cbc777827123c3962bcab08a 100644 (file)
@@ -24,7 +24,8 @@
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original Author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
  * @date 28/6/2001
  */
 
-#include "TNT/cmat.h"
 #include <vector>
-#include "MT_Vector3.h"
-#include "IK_QChain.h"
+#include <list>
 
-class IK_QJacobianSolver {
+#include "MT_Vector3.h"
+#include "IK_QJacobian.h"
+#include "IK_QSegment.h"
+#include "IK_QTask.h"
 
-public :
-
-       /**
-        * Create a new IK_QJacobianSolver on the heap
-        * @return A newly created IK_QJacobianSolver you take ownership of the object
-     * and responsibility for deleting it
-        */
-
-
-       static 
-               IK_QJacobianSolver *
-       New(
-       );
+class IK_QJacobianSolver
+{
+public:
+       IK_QJacobianSolver() {};
+       ~IK_QJacobianSolver() {};
 
        /**
         * Compute a solution for a chain.
-     * @param chain Reference to the chain to modify
-        * @param g_position Reference to the goal position.
-        * @param g_pose -not used- Reference to the goal pose. 
+     * @param root Pointer to root segment.
+        * @param tasks List of tasks.
         * @param tolerance The maximum allowed distance between solution
      * and goal for termination.
         * @param max_iterations should be in the range (50 - 500) 
-     * @param max_angle_change The maximum change in the angle vector 
-     * of the chain (0.1 is a good value)
         *
      * @return True iff goal position reached.
      */
 
-               bool
-       Solve(
-               IK_QChain &chain,
-               const MT_Vector3 &g_position,
-               const MT_Vector3 &g_pose,
+       bool Solve(
+               IK_QSegment *root,
+               std::list<IK_QTask*> tasks,
                const MT_Scalar tolerance,
-               const int max_iterations,
-               const MT_Scalar max_angle_change
-       );
-
-       ~IK_QJacobianSolver(
-       );
-
-
-private :
-
-       /**
-        * Private constructor to force use of New()
-        */
-
-       IK_QJacobianSolver(
-       );
-
-
-       /** 
-        * Compute the inverse jacobian matrix of the chain.
-        * Uses a damped least squares solution when the matrix is 
-        * is approaching singularity
-     */
-
-               int
-       ComputeInverseJacobian(
-               IK_QChain &chain,
-               const MT_Scalar x_length,
-               const MT_Scalar max_angle_change
-       );
-
-               void
-       ComputeBetas(
-               IK_QChain &chain,
-               const MT_Vector3 d_pos
-       );
-
-       /** 
-        * Updates the angles of the chain with the newly
-        * computed values
-     */
-
-               void
-       UpdateChain(
-               IK_QChain &chain
+               const int max_iterations
        );
-       
-       /**
-        * Make sure all the matrices are of the correct size
-     */
-
-               void
-       ArmMatrices(
-               int dof
-       );
-
-       /**
-        * Quick check to see if all the segments are parallel
-        * to the goal direction.
-        */
-
-               bool
-       ParallelCheck(
-               const IK_QChain &chain,
-               const MT_Vector3 goal
-       ) const;
-       
-
-
-private :
-
-       /// the vector of intermediate betas
-       TNT::Vector<MT_Scalar> m_beta;
-
-       /// the vector of computed angle changes
-       TNT::Vector<MT_Scalar> m_d_theta;
-
-       /// the constraint gradients for each angle.
-       TNT::Vector<MT_Scalar> m_dh;
-
-       /// Space required for SVD computation
-
-       TNT::Vector<MT_Scalar> m_svd_w;
-    TNT::Vector<MT_Scalar> m_svd_work_space;        
-       TNT::Matrix<MT_Scalar> m_svd_v;
-       TNT::Matrix<MT_Scalar> m_svd_u;
-
-       TNT::Matrix<MT_Scalar> m_svd_w_diag;
-       TNT::Matrix<MT_Scalar> m_svd_v_t;
-       TNT::Matrix<MT_Scalar> m_svd_u_t;
-       TNT::Matrix<MT_Scalar> m_svd_inverse;
-       TNT::Matrix<MT_Scalar> m_svd_temp1;
-
 
+private:
+       void AddSegmentList(IK_QSegment *seg);
+       bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks);
+       bool UpdateAngles(MT_Scalar& norm);
+
+private:
+       // the jacobian matrix
+       IK_QJacobian m_jacobian;
+       IK_QJacobian m_jacobian_sub;
+       bool m_secondary_enabled;
+       std::vector<IK_QSegment*> m_segments;
 };
 
 #endif
index 2b7c126f9734558778dad9d2edfd9d62483da0bd..a380e29e6fa7d61821cbd7612cac36100543bd57 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original Author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
 
-/**
+#include "IK_QSegment.h"
 
- * $Id$
- * Copyright (C) 2001 NaN Technologies B.V.
- *
- * @author Laurence
- */
+#include <iostream>
+using namespace std;
 
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
+// Utility functions
 
-#include "IK_QSegment.h"
-#include <iostream>
+static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis)
+{
+       if (axis == 0)
+               return MT_Matrix3x3(1.0, 0.0, 0.0,
+                                   0.0, cosine, -sine,
+                                   0.0, sine, cosine);
+       else if (axis == 1)
+               return MT_Matrix3x3(cosine, 0.0, sine,
+                                   0.0, 1.0, 0.0,
+                                   -sine, 0.0, cosine);
+       else
+               return MT_Matrix3x3(cosine, -sine, 0.0,
+                                   sine, cosine, 0.0,
+                                   0.0, 0.0, 1.0);
+}
 
-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) 
-{      
+static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis)
+{
+       return RotationMatrix(sin(angle), cos(angle), axis);
+}
 
-       m_max_extension = tr1.length() + length;        
+static MT_Scalar safe_acos(MT_Scalar f)
+{
+       if (f <= -1.0f)
+               return MT_PI;
+       else if (f >= 1.0f)
+               return 0.0;
+       else
+               return acos(f);
+}
 
-       m_transform.setOrigin(tr1);
-       m_transform.setBasis(A);
+static MT_Scalar ComputeTwist(const MT_Matrix3x3& R)
+{
+       // qy and qw are the y and w components of the quaternion from R
+       MT_Scalar qy = R[0][2] - R[2][0];
+       MT_Scalar qw = R[0][0] + R[1][1] + R[2][2] + 1;
+
+       MT_Scalar tau = 2*atan2(qy, qw);
+
+       return tau;
+}
+
+static MT_Matrix3x3 ComputeTwistMatrix(MT_Scalar tau)
+{
+       return RotationMatrix(tau, 1);
+}
 
-       UpdateLocalTransform();
-};
+static void RemoveTwist(MT_Matrix3x3& R)
+{
+       // compute twist parameter
+       MT_Scalar tau = ComputeTwist(R);
+
+       // compute twist matrix
+       MT_Matrix3x3 T = ComputeTwistMatrix(tau);
+
+       // remove twist
+       R = R*T.transposed();
+}
+
+static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
+{
+       // compute twist parameter
+       MT_Scalar tau = ComputeTwist(R);
+
+       // compute swing parameters
+       MT_Scalar num = 2.0*(1.0 + R[1][1]);
+
+       // singularity at pi
+       if (MT_abs(num) < MT_EPSILON)
+               // TODO: this does now rotation of size pi over z axis, but could
+               // be any axis, how to deal with this i'm not sure, maybe don't
+               // enforce limits at all then
+               return MT_Vector3(0.0, tau, 1.0);
+
+       num = 1.0/sqrt(num);
+       MT_Scalar ax = -R[2][1]*num;
+       MT_Scalar az = R[0][1]*num;
+
+       return MT_Vector3(ax, tau, az);
+}
+
+static MT_Matrix3x3 ComputeSwingMatrix(MT_Scalar ax, MT_Scalar az)
+{
+       // length of (ax, 0, az) = sin(theta/2)
+       MT_Scalar sine2 = ax*ax + az*az;
+       MT_Scalar cosine2 = sqrt((sine2 >= 1.0)? 0.0: 1.0-sine2);
+
+       // compute swing matrix
+       MT_Matrix3x3 S(MT_Quaternion(ax, 0.0, az, -cosine2));
+
+       return S;
+}
+
+static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R)
+{
+       MT_Vector3 delta = MT_Vector3(R[2][1] - R[1][2],
+                                     R[0][2] - R[2][0],
+                                     R[1][0] - R[0][1]);
+
+       MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1)/2);
+       MT_Scalar l = delta.length();
        
+       if (!MT_fuzzyZero(l))
+               delta *= c/l;
+       
+       return delta;
+}
 
-IK_QSegment::
-IK_QSegment (
-) :
-       m_length(0),
-       m_q (0,0,0),
-       m_max_extension(0)
+// IK_QSegment
+
+IK_QSegment::IK_QSegment(int num_DoF, bool translational)
+: m_parent(NULL), m_child(NULL), m_sibling(NULL), m_num_DoF(num_DoF),
+  m_translational(translational)
 {
-       // Intentionally empty
+       m_locked[0] = m_locked[1] = m_locked[2] = false;
+       m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
+
+       m_max_extension = 0.0;
+
+       m_start = MT_Vector3(0, 0, 0);
+       m_rest_basis.setIdentity();
+       m_basis.setIdentity();
+       m_translation = MT_Vector3(0, 0, 0);
+
+       m_orig_basis = m_basis;
+       m_orig_translation = m_translation;
 }
 
+void IK_QSegment::SetTransform(
+       const MT_Vector3& start,
+       const MT_Matrix3x3& rest_basis,
+       const MT_Matrix3x3& basis,
+       const MT_Scalar length
+)
+{
+       m_max_extension = start.length() + length;      
 
+       m_start = start;
+       m_rest_basis = rest_basis;
+       m_basis = basis;
+       m_translation = MT_Vector3(0, length, 0);
 
-// accessors
-////////////
+       m_orig_basis = m_basis;
+       m_orig_translation = m_translation;
+}
 
-// The length of the segment 
+MT_Matrix3x3 IK_QSegment::BasisChange() const
+{
+       return m_orig_basis.transposed()*m_basis;
+}
 
-const
-       MT_Scalar
-IK_QSegment::
-Length(
-) const {
-       return m_length;
+MT_Vector3 IK_QSegment::TranslationChange() const
+{
+       return m_translation - m_orig_translation;
 }
 
-const 
-       MT_Scalar
-IK_QSegment::
-MaxExtension(
-) const {
-       return m_max_extension;
+IK_QSegment::~IK_QSegment()
+{
+       if (m_parent)
+               m_parent->RemoveChild(this);
+
+       for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
+               seg->m_parent = NULL;
 }
+
+void IK_QSegment::SetParent(IK_QSegment *parent)
+{
+       if (m_parent == parent)
+               return;
        
-// 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.
+       if (m_parent)
+               m_parent->RemoveChild(this);
        
-       m_seg_start = global * m_transform.getOrigin();
-       m_global_transform = global;
+       if (parent) {
+               m_sibling = parent->m_child;
+               parent->m_child = this;
+       }
+
+       m_parent = parent;
+}
+
+void IK_QSegment::RemoveChild(IK_QSegment *child)
+{
+       if (m_child == NULL)
+               return;
+       else if (m_child == child)
+               m_child = m_child->m_sibling;
+       else {
+               IK_QSegment *seg = m_child;
+
+               while (seg->m_sibling != child);
+                       seg = seg->m_sibling;
+
+               if (child == seg->m_sibling)
+                       seg->m_sibling = child->m_sibling;
+       }
+}
+
+void IK_QSegment::UpdateTransform(const MT_Transform& global)
+{
+       // compute the global transform at the end of the segment
+       m_global_start = global.getOrigin() + global.getBasis()*m_start;
+
+       m_global_transform.setOrigin(m_global_start);
+       m_global_transform.setBasis(global.getBasis() * m_rest_basis * m_basis);
+       m_global_transform.translate(m_translation);
+
+       // update child transforms
+       for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
+               seg->UpdateTransform(m_global_transform);
+}
 
-       const MT_Transform new_global = GlobalTransform();
-               
-       m_seg_end = new_global.getOrigin();
+// IK_QSphericalSegment
 
-       return new_global;
+IK_QSphericalSegment::IK_QSphericalSegment()
+: IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
+{
 }
 
-       MT_Transform
-IK_QSegment::
-GlobalTransform(
-) const {
-       return m_global_transform * m_local_transform;
+MT_Vector3 IK_QSphericalSegment::Axis(int dof) const
+{
+       return m_global_transform.getBasis().getColumn(dof);
 }
+
+void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+{
+       if (lmin >= lmax)
+               return;
        
+       if (axis == 1) {
+               lmin = MT_clamp(lmin, -180, 180);
+               lmax = MT_clamp(lmax, -180, 180);
 
-       MT_Transform
-IK_QSegment::
-UpdateAccumulatedLocal(
-       const MT_Transform & acc_local
-){
-       m_accum_local = acc_local;
-       return m_local_transform * m_accum_local;
-}              
+               m_min_y = MT_radians(lmin);
+               m_max_y = MT_radians(lmax);
 
-const 
-       MT_Transform &
-IK_QSegment::
-AccumulatedLocal(
-) const {
-       return m_accum_local;
+               m_limit_y = true;
+       }
+       else {
+               // clamp and convert to axis angle parameters
+               lmin = MT_clamp(lmin, -180, 180);
+               lmax = MT_clamp(lmax, -180, 180);
+
+               lmin = sin(MT_radians(lmin)*0.5);
+               lmax = sin(MT_radians(lmax)*0.5);
+
+               // put center of ellispe in the middle between min and max
+               MT_Scalar offset = 0.5*(lmin + lmax);
+               lmax = lmax - offset;
+
+               if (axis == 0) {
+                       m_limit_x = true;
+                       m_offset_x = offset;
+                       m_max_x = lmax;
+               }
+               else if (axis == 2) {
+                       m_limit_z = true;
+                       m_offset_z = offset;
+                       m_max_z = lmax;
+               }
+       }
 }
 
-       MT_Vector3      
-IK_QSegment::
-ComputeJacobianColumn(
-       int angle
-) const {
+void IK_QSphericalSegment::SetWeight(int axis, MT_Scalar weight)
+{
+       m_weight[axis] = weight;
+}
+
+bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+{
+       if (m_locked[0] && m_locked[1] && m_locked[2])
+               return false;
+
+       MT_Vector3 dq;
+       dq.x() = jacobian.AngleUpdate(m_DoF_id);
+       dq.y() = jacobian.AngleUpdate(m_DoF_id+1);
+       dq.z() = jacobian.AngleUpdate(m_DoF_id+2);
+
+       // Directly update the rotation matrix, with Rodrigues' rotation formula,
+       // to avoid singularities and allow smooth integration.
 
+       MT_Scalar theta = dq.length();
 
-       MT_Transform translation;
-       translation.setIdentity();
-       translation.translate(MT_Vector3(0,m_length,0));
+       if (!MT_fuzzyZero(theta)) {
+               MT_Vector3 w = dq*(1.0/theta);
 
+               MT_Scalar sine = sin(theta);
+               MT_Scalar cosine = cos(theta);
+               MT_Scalar cosineInv = 1-cosine;
 
-       // 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)
+               MT_Scalar xsine = w.x()*sine;
+               MT_Scalar ysine = w.y()*sine;
+               MT_Scalar zsine = w.z()*sine;
 
-#if 0
+               MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
+               MT_Scalar xycosine = w.x()*w.y()*cosineInv;
+               MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
+               MT_Scalar yycosine = w.y()*w.y()*cosineInv;
+               MT_Scalar yzcosine = w.y()*w.z()*cosineInv;
+               MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
 
-       // use euler angles as a test of the matrices and this method.
+               MT_Matrix3x3 M(
+                       cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
+                       zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
+                       -ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
 
-       MT_Matrix3x3 dRda;
+               m_new_basis = m_basis*M;
+       }
+       else
+               m_new_basis = m_basis;
+
+       
+       if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
+               return false;
+
+       MT_Vector3 a = SphericalRangeParameters(m_new_basis);
 
-       MT_Quaternion rotx,roty,rotz;
+       if (m_locked[0])
+               a.x() = m_locked_ax;
+       if (m_locked[1])
+               a.y() = m_locked_ay;
+       if (m_locked[2])
+               a.z() = m_locked_az;
 
-       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_Scalar ax = a.x(), ay = a.y(), az = a.z();
 
-       MT_Matrix3x3 rotx_m(rotx);
-       MT_Matrix3x3 roty_m(roty);
-       MT_Matrix3x3 rotz_m(rotz);
-       if (angle == 0) {
+       clamp[0] = clamp[1] = clamp[2] =  false;
        
-               MT_Scalar ci = cos(m_q[0]);
-               MT_Scalar si = sin(m_q[1]);
+       if (m_limit_y) {
+               if (a.y() > m_max_y) {
+                       ay = m_max_y;
+                       clamp[1] = true;
+               }
+               else if (a.y() < m_min_y) {
+                       ay = m_min_y;
+                       clamp[1] = true;
+               }
+       }
 
-               dRda = MT_Matrix3x3(
-                       0,  0,  0,
-                       0,-si,-ci,
-                       0, ci,-si
-               );
+       if (m_limit_x && m_limit_z) {
+               /* check in ellipsoid region */
+               ax = a.x() + m_offset_x;
+               az = a.z() + m_offset_z;
+
+               MT_Scalar invX = 1.0/(m_max_x*m_max_x);
+               MT_Scalar invZ = 1.0/(m_max_z*m_max_z);
+
+               if ((ax*ax*invX + az*az*invZ) > 1.0) {
+                       clamp[0] = clamp[2] = true;
+
+                       if (MT_fuzzyZero(ax)) {
+                               ax = 0.0;
+                               az = (az > 0)? m_max_z: -m_max_z;
+                       }
+                       else {
+                               MT_Scalar rico = az/ax;
+                               MT_Scalar old_ax = ax;
+                               ax = sqrt(1.0/(invX + invZ*rico*rico));
+                               if (old_ax < 0.0)
+                                       ax = -ax;
+                               az = rico*ax;
+                       }
+               }
+
+               ax = ax - m_offset_x;
+               az = az - m_offset_z;
+       }
+       else if (m_limit_x) {
+               ax = a.x() + m_offset_x;
+
+               if (ax < -m_max_x) {
+                       ax = -m_max_x;
+                       clamp[0] = true;
+               }
+               else if (ax > m_max_x) {
+                       ax = m_max_x;
+                       clamp[0] = true;
+               }
+
+               ax = ax - m_offset_x;
+               az = a.z();
+       }
+       else if (m_limit_z) {
+               az = a.z() + m_offset_z;
+
+               if (az < -m_max_z) {
+                       az = -m_max_z;
+                       clamp[2] = true;
+               }
+               else if (az > m_max_z) {
+                       az = m_max_z;
+                       clamp[2] = true;
+               }
+
+               ax = a.x();
+               az = az - m_offset_z;
+       }
 
-               dRda = rotz_m * roty_m * dRda;
-       } else 
+       if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
+               if (m_locked[0] || m_locked[1] || m_locked[2])
+                       m_new_basis = ComputeSwingMatrix(ax, az)*ComputeTwistMatrix(ay);
+               return false;
+       }
        
-       if (angle == 1) {
+       m_new_basis = ComputeSwingMatrix(ax, az)*ComputeTwistMatrix(ay);
+
+       delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
+
+       if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
+               m_locked_ax = ax;
+               m_locked_az = az;
+       }
+
+       if (!m_locked[1] && clamp[1])
+               m_locked_ay = ay;
        
-               MT_Scalar cj = cos(m_q[1]);
-               MT_Scalar sj = sin(m_q[1]);
+       return true;
+}
+
+void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
+{
+       if (dof == 1) {
+               m_locked[1] = true;
+               jacobian.Lock(m_DoF_id+1, delta[1]);
+       }
+       else {
+               m_locked[0] = m_locked[2] = true;
+               jacobian.Lock(m_DoF_id, delta[0]);
+               jacobian.Lock(m_DoF_id+2, delta[2]);
+       }
+}
+
+void IK_QSphericalSegment::UpdateAngleApply()
+{
+       m_basis = m_new_basis;
+}
+
+// IK_QNullSegment
+
+IK_QNullSegment::IK_QNullSegment()
+: IK_QSegment(0, false)
+{
+}
 
-               dRda = MT_Matrix3x3(
-                       -sj,  0, cj,
-                         0,  0,  0,
-                       -cj,  0,-sj
-               );
+// IK_QRevoluteSegment
 
-               dRda = rotz_m * dRda * rotx_m;
-       } else 
+IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
+: IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
+{
+}
+
+MT_Vector3 IK_QRevoluteSegment::Axis(int) const
+{
+       return m_global_transform.getBasis().getColumn(m_axis);
+}
+
+bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+{
+       if (m_locked[0])
+               return false;
+
+       m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
+
+       clamp[0] = false;
+
+       if (m_limit == false)
+               return false;
+
+       if (m_new_angle > m_max)
+               delta[0] = m_max - m_angle;
+       else if (m_new_angle < m_min)
+               delta[0] = m_min - m_angle;
+       else
+               return false;
        
-       if (angle == 2) {
+       clamp[0] = true;
+       m_new_angle = m_angle + delta[0];
+
+       return true;
+}
+
+void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
+{
+       m_locked[0] = true;
+       jacobian.Lock(m_DoF_id, delta[0]);
+}
+
+void IK_QRevoluteSegment::UpdateAngleApply()
+{
+       m_angle = m_new_angle;
+
+       m_basis = m_orig_basis*RotationMatrix(m_angle, m_axis);
+}
+
+void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+{
+       if (lmin >= lmax || m_axis != axis)
+               return;
        
-               MT_Scalar ck = cos(m_q[2]);
-               MT_Scalar sk = sin(m_q[2]);
+       // clamp and convert to axis angle parameters
+       lmin = MT_clamp(lmin, -180, 180);
+       lmax = MT_clamp(lmax, -180, 180);
 
-               dRda = MT_Matrix3x3(
-                       -sk,-ck,  0,
-                        ck,-sk,  0,
-                         0,  0,  0
-               );
+       m_min = MT_radians(lmin);
+       m_max = MT_radians(lmax);
 
-               dRda = dRda * roty_m * rotx_m;
-       }
-               
-       MT_Transform dRda_t(MT_Point3(0,0,0),dRda);
+       m_limit = true;
+}
 
-       // convert to 4x4 matrices coz dRda is singular.
-       MT_Matrix4x4 dRda_m(dRda_t);
-       dRda_m[3][3] = 0;
+void IK_QRevoluteSegment::SetWeight(int axis, MT_Scalar weight)
+{
+       if (axis == m_axis)
+               m_weight[0] = weight;
+}
 
-#else
+// IK_QSwingSegment
 
-       // use exponential map derivatives
-       MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle);
+IK_QSwingSegment::IK_QSwingSegment()
+: IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
+{
+}
+
+MT_Vector3 IK_QSwingSegment::Axis(int dof) const
+{
+       return m_global_transform.getBasis().getColumn((dof==0)? 0: 2);
+}
+
+bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+{
+       if (m_locked[0] && m_locked[1])
+               return false;
+
+       MT_Vector3 dq;
+       dq.x() = jacobian.AngleUpdate(m_DoF_id);
+       dq.y() = 0.0;
+       dq.z() = jacobian.AngleUpdate(m_DoF_id+1);
+
+       // Directly update the rotation matrix, with Rodrigues' rotation formula,
+       // to avoid singularities and allow smooth integration.
+
+       MT_Scalar theta = dq.length();
+
+       if (!MT_fuzzyZero(theta)) {
+               MT_Vector3 w = dq*(1.0/theta);
+
+               MT_Scalar sine = sin(theta);
+               MT_Scalar cosine = cos(theta);
+               MT_Scalar cosineInv = 1-cosine;
+
+               MT_Scalar xsine = w.x()*sine;
+               MT_Scalar zsine = w.z()*sine;
+
+               MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
+               MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
+               MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
 
-#endif 
+               MT_Matrix3x3 M(
+                       cosine + xxcosine, -zsine, xzcosine,
+                       zsine, cosine, -xsine,
+                       xzcosine, xsine, cosine + zzcosine);
 
+               m_new_basis = m_basis*M;
+
+               RemoveTwist(m_new_basis);
+       }
+       else
+               m_new_basis = m_basis;
+
+       if (m_limit_x == false && m_limit_z == false)
+               return false;
+
+       MT_Vector3 a = SphericalRangeParameters(m_new_basis);
+       MT_Scalar ax = 0, az = 0;
+
+       clamp[0] = clamp[1] = false;
        
-       // Once we have computed the local derivatives we can compute 
-       // derivatives of the end effector. 
+       if (m_limit_x && m_limit_z) {
+               /* check in ellipsoid region */
+               ax = a.x() + m_offset_x;
+               az = a.z() + m_offset_z;
+
+               MT_Scalar invX = 1.0/(m_max_x*m_max_x);
+               MT_Scalar invZ = 1.0/(m_max_z*m_max_z);
+
+               if ((ax*ax*invX + az*az*invZ) > 1.0) {
+                       clamp[0] = clamp[1] = true;
+
+                       if (MT_fuzzyZero(ax)) {
+                               ax = 0.0;
+                               az = (az > 0)? m_max_z: -m_max_z;
+                       }
+                       else {
+                               MT_Scalar rico = az/ax;
+                               MT_Scalar old_ax = ax;
+                               ax = sqrt(1.0/(invX + invZ*rico*rico));
+                               if (old_ax < 0.0)
+                                       ax = -ax;
+                               az = rico*ax;
+                       }
+               }
+
+               ax = ax - m_offset_x;
+               az = az - m_offset_z;
+       }
+       else if (m_limit_x) {
+               ax = a.x() + m_offset_x;
+
+               if (ax < -m_max_x) {
+                       ax = -m_max_x;
+                       clamp[0] = true;
+               }
+               else if (ax > m_max_x) {
+                       ax = m_max_x;
+                       clamp[0] = true;
+               }
+
+               ax = ax - m_offset_x;
+               az = a.z();
+       }
+       else if (m_limit_z) {
+               az = a.z() + m_offset_z;
+
+               if (az < -m_max_z) {
+                       az = -m_max_z;
+                       clamp[1] = true;
+               }
+               else if (az > m_max_z) {
+                       az = m_max_z;
+                       clamp[1] = true;
+               }
+
+               ax = a.x();
+               az = az - m_offset_z;
+       }
 
-       // 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
+       if (clamp[0] == false && clamp[1] == false)
+               return false;
 
-       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);
+       m_new_basis = ComputeSwingMatrix(ax, az);
 
-               
-       MT_Matrix4x4 dFda_m = global_m * transform_m * dRda_m * translation_m * accum_local_m;
+       delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
+       delta[1] = delta[2]; delta[2] = 0.0;
 
-       MT_Vector4 result = dFda_m * MT_Vector4(0,0,0,1);
-       return MT_Vector3(result[0],result[1],result[2]);
-};
+       return true;
+}
 
+void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
+{
+       m_locked[0] = m_locked[1] = true;
+       jacobian.Lock(m_DoF_id, delta[0]);
+       jacobian.Lock(m_DoF_id+1, delta[1]);
+}
 
+void IK_QSwingSegment::UpdateAngleApply()
+{
+       m_basis = m_new_basis;
+}
 
-const  
-       MT_Vector3 &
-IK_QSegment::
-GlobalSegmentStart(
-) const{
-       return m_seg_start;
+void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+{
+       if (lmin >= lmax)
+               return;
+       
+       // clamp and convert to axis angle parameters
+       lmin = MT_clamp(lmin, -180, 180);
+       lmax = MT_clamp(lmax, -180, 180);
+
+       lmin = sin(MT_radians(lmin)*0.5);
+       lmax = sin(MT_radians(lmax)*0.5);
+
+       // put center of ellispe in the middle between min and max
+       MT_Scalar offset = 0.5*(lmin + lmax);
+       lmax = lmax - offset;
+
+       if (axis == 0) {
+               m_limit_x = true;
+               m_offset_x = offset;
+               m_max_x = lmax;
+       }
+       else if (axis == 2) {
+               m_limit_z = true;
+               m_offset_z = offset;
+               m_max_z = lmax;
+       }
+}
+
+void IK_QSwingSegment::SetWeight(int axis, MT_Scalar weight)
+{
+       if (axis == 0)
+               m_weight[0] = weight;
+       else if (axis == 2)
+               m_weight[1] = weight;
+}
+
+// IK_QElbowSegment
+
+IK_QElbowSegment::IK_QElbowSegment(int axis)
+: IK_QSegment(2, false), m_axis(axis), m_twist(0.0), m_angle(0.0),
+  m_cos_twist(1.0), m_sin_twist(0.0), m_limit(false), m_limit_twist(false)
+{
 }
 
-const  
-       MT_Vector3 &
-IK_QSegment::
-GlobalSegmentEnd(
-) const {
-       return m_seg_end;
+MT_Vector3 IK_QElbowSegment::Axis(int dof) const
+{
+       if (dof == 0) {
+               MT_Vector3 v;
+               if (m_axis == 0)
+                       v = MT_Vector3(m_cos_twist, 0, m_sin_twist);
+               else
+                       v = MT_Vector3(-m_sin_twist, 0, m_cos_twist);
+
+               return m_global_transform.getBasis() * v;
+       }
+       else
+               return m_global_transform.getBasis().getColumn(1);
 }
 
+bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+{
+       if (m_locked[0] && m_locked[1])
+               return false;
+
+       clamp[0] = clamp[1] = false;
+
+       if (!m_locked[0]) {
+               m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
+
+               if (m_limit) {
+                       if (m_new_angle > m_max) {
+                               delta[0] = m_max - m_angle;
+                               m_new_angle = m_max;
+                               clamp[0] = true;
+                       }
+                       else if (m_new_angle < m_min) {
+                               delta[0] = m_min - m_angle;
+                               m_new_angle = m_min;
+                               clamp[0] = true;
+                       }
+               }
+       }
 
-       void
-IK_QSegment::
-IncrementAngle(
-       const MT_Vector3 &dq
-){
-       m_q.vector() += dq;
-       MT_Scalar theta(0);
-       m_q.reParameterize(theta);
+       if (!m_locked[1]) {
+               m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id+1);
+
+               if (m_limit_twist) {
+                       if (m_new_twist > m_max_twist) {
+                               delta[1] = m_max_twist - m_twist;
+                               m_new_twist = m_max_twist;
+                               clamp[1] = true;
+                       }
+                       else if (m_new_twist < m_min_twist) {
+                               delta[1] = m_min_twist - m_twist;
+                               m_new_twist = m_min_twist;
+                               clamp[1] = true;
+                       }
+               }
+       }
 
-       UpdateLocalTransform();
+       return (clamp[0] || clamp[1]);
 }
 
-       void
-IK_QSegment::
-SetAngle(
-       const MT_ExpMap &dq
-){
-       m_q = dq;
-       UpdateLocalTransform();
+void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
+{
+       if (dof == 0) {
+               m_locked[0] = true;
+               jacobian.Lock(m_DoF_id, delta[0]);
+       }
+       else {
+               m_locked[1] = true;
+               jacobian.Lock(m_DoF_id+1, delta[1]);
+       }
 }
 
+void IK_QElbowSegment::UpdateAngleApply()
+{
+       m_angle = m_new_angle;
+       m_twist = m_new_twist;
 
-       void
-IK_QSegment::
-UpdateLocalTransform(
-){
-#if 0
+       m_sin_twist = sin(m_twist);
+       m_cos_twist = cos(m_twist);
 
-       //use euler angles - test 
-       MT_Quaternion rotx,roty,rotz;
+       MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis);
+       MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
 
-       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]);
+       m_basis = m_orig_basis*A*T;
+}
 
-       MT_Matrix3x3 rotx_m(rotx);
-       MT_Matrix3x3 roty_m(roty);
-       MT_Matrix3x3 rotz_m(rotz);
+void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+{
+       if (lmin >= lmax)
+               return;
 
-       MT_Matrix3x3 rot = rotz_m * roty_m * rotx_m;
-#else
+       // clamp and convert to axis angle parameters
+       lmin = MT_clamp(lmin, -180, 180);
+       lmax = MT_clamp(lmax, -180, 180);
 
-       //use exponential map 
-       MT_Matrix3x3 rot = m_q.getMatrix();     
+       lmin = MT_radians(lmin);
+       lmax = MT_radians(lmax);
 
-       
-#endif
+       if (axis == 1) {
+               m_min_twist = lmin;
+               m_max_twist = lmax;
+               m_limit_twist = true;
+       }
+       else if (axis == m_axis) {
+               m_min = lmin;
+               m_max = lmax;
+               m_limit = true;
+       }
+}
+
+void IK_QElbowSegment::SetWeight(int axis, MT_Scalar weight)
+{
+       if (axis == m_axis)
+               m_weight[0] = weight;
+       else if (axis == 1)
+               m_weight[1] = weight;
+}
+
+// IK_QTranslateSegment
+
+IK_QTranslateSegment::IK_QTranslateSegment(int axis1)
+: IK_QSegment(1, true)
+{
+       m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
+       m_axis_enabled[axis1] = true;
+
+       m_axis[0] = axis1;
+}
+
+IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2)
+: IK_QSegment(2, true)
+{
+       m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
+       m_axis_enabled[axis1] = true;
+       m_axis_enabled[axis2] = true;
 
-       MT_Transform rx(MT_Point3(0,0,0),rot);
+       m_axis[0] = axis1;
+       m_axis[1] = axis2;
+}
 
-       MT_Transform translation;
-       translation.setIdentity();
-       translation.translate(MT_Vector3(0,m_length,0));
+IK_QTranslateSegment::IK_QTranslateSegment()
+: IK_QSegment(3, true)
+{
+       m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
 
-       m_local_transform = m_transform * rx * translation;
-};
+       m_axis[0] = 0;
+       m_axis[1] = 1;
+       m_axis[2] = 2;
+}
 
+MT_Vector3 IK_QTranslateSegment::Axis(int dof) const
+{
+       return m_global_transform.getBasis().getColumn(m_axis[dof]);
+}
 
+bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&, bool*)
+{
+       int dof_id = m_DoF_id;
 
+       MT_Vector3 dx;
+       dx.x() = (m_axis_enabled[0])? jacobian.AngleUpdate(dof_id++): 0.0;
+       dx.y() = (m_axis_enabled[1])? jacobian.AngleUpdate(dof_id++): 0.0;
+       dx.z() = (m_axis_enabled[2])? jacobian.AngleUpdate(dof_id++): 0.0;
 
+       m_new_translation = m_translation + dx;
 
+       return false;
+}
+
+void IK_QTranslateSegment::UpdateAngleApply()
+{
+       m_translation = m_new_translation;
+}
+
+void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight)
+{
+       m_weight[axis] = weight;
+}
 
index 7d4540f05430c9e1f5b03055737f1be2ef01cc3e..c6367dc4e036ef16d607374caa69eb74a1c2311e 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
 
-/**
-
- * $Id$
- * Copyright (C) 2001 NaN Technologies B.V.
- *
- * @author Laurence
- */
 #ifndef NAN_INCLUDED_IK_QSegment_h
 #define NAN_INCLUDED_IK_QSegment_h
 
-
 #include "MT_Vector3.h"
 #include "MT_Transform.h"
 #include "MT_Matrix4x4.h"
-#include "MT_ExpMap.h"
+#include "IK_QJacobian.h"
+#include "MEM_SmartPtr.h"
 
 #include <vector>
 
 /**
  * An IK_Qsegment encodes information about a segments
  * local coordinate system.
- * In these segments exponential maps are used to parameterize
- * the 3 DOF joints. Please see the file MT_ExpMap.h for more
- * information on this parameterization.
  * 
  * These segments always point along the y-axis.
  * 
- * Here wee define the local coordinates of a joint as
+ * Here we define the local coordinates of a joint as
  * local_transform = 
  * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
  * We use the standard moto column ordered matrices. You can read
  * use exactly the same transformations when displaying the segments
  */
 
-class IK_QSegment {
-
-public :
-
-       /**
-        * Constructor.
-        * @param tr1 a user defined translation 
-        * @param a used defined rotation matrix representin
-        * the rest position of the bone.
-        * @param the length of the bone.
-        * @param an exponential map can also be used to 
-        * define the bone rest position.
-        */
-
-       IK_QSegment(
-               const MT_Point3 tr1,
-               const MT_Matrix3x3 A,
-               const MT_Scalar length,
-               const MT_ExpMap q
+class IK_QSegment
+{
+public:
+       virtual ~IK_QSegment();
+
+       // start: a user defined translation
+       // rest_basis: a user defined rotation
+       // basis: a user defined rotation
+       // length: length of this segment
+
+       void SetTransform(
+               const MT_Vector3& start,
+               const MT_Matrix3x3& rest_basis,
+               const MT_Matrix3x3& basis,
+               const MT_Scalar length
        );
 
-       /** 
-        * Default constructor
-        * Defines identity local coordinate system,
-        * with a bone length of 1.
-        */
-       
+       // tree structure access
+       void SetParent(IK_QSegment *parent);
 
-       IK_QSegment(
-       );
+       IK_QSegment *Child() const
+       { return m_child; }
 
+       IK_QSegment *Sibling() const
+       { return m_sibling; }
 
-       /**
-     * @return The length of the segment
-     */
-
-       const
-               MT_Scalar
-       Length(
-       ) const ;
-
-       /**
-        * @return the max distance of the end of this
-        * bone from the local origin.
-        */
-
-       const 
-               MT_Scalar
-       MaxExtension(
-       ) const ;
-
-       /**
-     * @return The transform from adjacent
-        * coordinate systems in the chain.
-     */
-
-       const 
-               MT_Transform &
-       LocalTransform(
-       ) const ;
-
-       const
-               MT_ExpMap &
-       LocalJointParameter(
-       ) const;
-
-       /**
-        * Update the global coordinates of this segment.
-        * @param global the global coordinates of the
-        * previous bone in the chain
-        * @return the global coordinates of this segment.
-        */
-
-               MT_Transform
-       UpdateGlobal(
-               const MT_Transform & global
-       );
+       IK_QSegment *Parent() const
+       { return m_parent; }
 
-       /**
-        * @return The global transformation 
-        */
-
-               MT_Transform
-       GlobalTransform(
-       ) const;
-       
-       /** 
-        * Update the accumulated local transform of this segment
-        * The accumulated local transform is the end effector
-        * transform in the local coordinates of this segment.
-        * @param acc_local the accumulated local transform of 
-        * the child of this bone.
-        * @return the accumulated local transorm of this segment
-        */
-       
-               MT_Transform
-       UpdateAccumulatedLocal(
-               const MT_Transform & acc_local
-       );
+       // number of degrees of freedom
+       int NumberOfDoF() const
+       { return m_num_DoF; }
 
-       /**
-        * @return A const reference to accumulated local 
-        * transform of this segment.
-        */
-
-       const 
-               MT_Transform &
-       AccumulatedLocal(
-       ) const;
-               
-       /**
-        * @return A const Reference to start of segment in global 
-     * coordinates
-        */
-       
-       const   
-               MT_Vector3 &
-       GlobalSegmentStart(
-       ) const;
-
-       /**
-        * @return A const Reference to end of segment in global 
-     * coordinates
-        */
-
-       const 
-               MT_Vector3 &
-       GlobalSegmentEnd(
-       ) const;
-
-
-       /**
-        * @return the partial derivative of the end effector 
-        * with respect to one of the degrees of freedom of this 
-        * segment.
-        * @param angle the angle parameter you want to compute
-        * the partial derivatives for. For these segments this
-        * must be in the range [0,2]
-        */
-
-               MT_Vector3      
-       ComputeJacobianColumn(
-               int angle
-       ) const ;
-               
-       /**
-        * Explicitly set the angle parameterization value. 
-        */
-
-               void
-       SetAngle(
-               const MT_ExpMap &q
-       );
+       // unique id for this segment, for identification in the jacobian
+       int DoFId() const
+       { return m_DoF_id; }
 
-       /**
-        * Increment the angle parameterization value. 
-        */
+       void SetDoFId(int dof_id)
+       { m_DoF_id = dof_id; }
 
-               void
-       IncrementAngle(
-               const MT_Vector3 &dq
-       );      
+       // the max distance of the end of this bone from the local origin.
+       const MT_Scalar MaxExtension() const
+       { return m_max_extension; }
 
+       // the change in rotation and translation w.r.t. the rest pose
+       MT_Matrix3x3 BasisChange() const;
+       MT_Vector3 TranslationChange() const;
 
-       /**
-        * Return the parameterization of this angle
-        */
+       // the start and end of the segment
+       const MT_Point3 &GlobalStart() const
+       { return m_global_start; }
 
-       const 
-               MT_ExpMap &
-       ExpMap(
-       ) const {
-               return m_q;
-       };
+       const MT_Point3 &GlobalEnd() const
+       { return m_global_transform.getOrigin(); }
 
+       // the global transformation at the end of the segment
+       const MT_Transform &GlobalTransform() const
+       { return m_global_transform; }
 
-private :
-       
-               void
-       UpdateLocalTransform(
-       );
+       // is a translational segment?
+       bool Translational() const
+       { return m_translational; }
+
+       // locking (during inner clamping loop)
+       bool Locked(int dof) const
+       { return m_locked[dof]; }
+
+       void UnLock()
+       { m_locked[0] = m_locked[1] = m_locked[2] = false; }
+
+       // per dof joint weighting
+       MT_Scalar Weight(int dof) const
+       { return m_weight[dof]; }
+
+       void ScaleWeight(int dof, MT_Scalar scale)
+       { m_weight[dof] *= scale; }
 
+       // recursively update the global coordinates of this segment, 'global'
+       // is the global transformation from the parent segment
+       void UpdateTransform(const MT_Transform &global);
 
-private :
+       // get axis from rotation matrix for derivative computation
+       virtual MT_Vector3 Axis(int dof) const=0;
 
-       /** 
-        * m_transform The user defined transformation, composition of the 
-        * translation and rotation from constructor.
-     */
-       MT_Transform m_transform;
+       // update the angles using the dTheta's computed using the jacobian matrix
+       virtual bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*)=0;
+       virtual void Lock(int, IK_QJacobian&, MT_Vector3&) {}
+       virtual void UpdateAngleApply()=0;
 
-       /**
-        * The exponential map parameterization of this joint.
-        */
+       // set joint limits
+       virtual void SetLimit(int, MT_Scalar, MT_Scalar) {};
 
-       MT_Scalar m_length;
-       MT_ExpMap m_q;
+       // set joint weights (per axis)
+       virtual void SetWeight(int, MT_Scalar) {};
 
-       /**
-        * The maximum extension of this segment
-        * This is the magnitude of the user offset + the length of the 
-        * chain 
-        */
+protected:
 
+       // num_DoF: number of degrees of freedom
+       IK_QSegment(int num_DoF, bool translational);
+
+       // remove child as a child of this segment
+       void RemoveChild(IK_QSegment *child);
+
+       // tree structure variables
+       IK_QSegment *m_parent;
+       IK_QSegment *m_child;
+       IK_QSegment *m_sibling;
+
+       // full transform = 
+       // start * rest_basis * basis * translation
+       MT_Vector3 m_start;
+       MT_Matrix3x3 m_rest_basis;
+       MT_Matrix3x3 m_basis;
+       MT_Vector3 m_translation;
+
+       // original basis
+       MT_Matrix3x3 m_orig_basis;
+       MT_Vector3 m_orig_translation;
+
+       // maximum extension of this segment
        MT_Scalar m_max_extension;
 
-       MT_Transform m_local_transform;
+       // accumulated transformations starting from root
+       MT_Point3 m_global_start;
        MT_Transform m_global_transform;
-       MT_Transform m_accum_local;
 
-       MT_Vector3 m_seg_start;
-       MT_Vector3 m_seg_end;
+       // number degrees of freedom, (first) id of this segments DOF's
+       int m_num_DoF, m_DoF_id;
+
+       bool m_locked[3];
+       bool m_translational;
+       MT_Scalar m_weight[3];
+};
+
+class IK_QSphericalSegment : public IK_QSegment
+{
+public:
+       IK_QSphericalSegment();
+
+       MT_Vector3 Axis(int dof) const;
+
+       bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
+       void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
+       void UpdateAngleApply();
+
+       bool ComputeClampRotation(MT_Vector3& clamp);
+
+       void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
+       void SetWeight(int axis, MT_Scalar weight);
+
+private:
+       MT_Matrix3x3 m_new_basis;
+       bool m_limit_x, m_limit_y, m_limit_z;
+       MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
+       MT_Scalar m_locked_ax, m_locked_ay, m_locked_az;
+};
+
+class IK_QNullSegment : public IK_QSegment
+{
+public:
+       IK_QNullSegment();
+
+       bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
+       void UpdateAngleApply() {}
+
+       MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
+};
+
+class IK_QRevoluteSegment : public IK_QSegment
+{
+public:
+       // axis: the axis of the DoF, in range 0..2
+       IK_QRevoluteSegment(int axis);
+
+       MT_Vector3 Axis(int dof) const;
+
+       bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
+       void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
+       void UpdateAngleApply();
+
+       void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
+       void SetWeight(int axis, MT_Scalar weight);
+
+private:
+       int m_axis;
+       MT_Scalar m_angle, m_new_angle;
+       MT_Scalar m_limit;
+       MT_Scalar m_min, m_max;
+};
+
+class IK_QSwingSegment : public IK_QSegment
+{
+public:
+       // XZ DOF, uses one direct rotation
+       IK_QSwingSegment();
+
+       MT_Vector3 Axis(int dof) const;
+
+       bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
+       void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
+       void UpdateAngleApply();
+
+       void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
+       void SetWeight(int axis, MT_Scalar weight);
+
+private:
+       MT_Matrix3x3 m_new_basis;
+       bool m_limit_x, m_limit_z;
+       MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
+};
+
+class IK_QElbowSegment : public IK_QSegment
+{
+public:
+       // XY or ZY DOF, uses two sequential rotations: first rotate around
+       // X or Z, then rotate around Y (twist)
+       IK_QElbowSegment(int axis);
+
+       MT_Vector3 Axis(int dof) const;
+
+       bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
+       void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
+       void UpdateAngleApply();
+
+       void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
+       void SetWeight(int axis, MT_Scalar weight);
+
+private:
+       int m_axis;
+
+       MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
+       MT_Scalar m_cos_twist, m_sin_twist;
+
+       bool m_limit, m_limit_twist;
+       MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
+};
+
+class IK_QTranslateSegment : public IK_QSegment
+{
+public:
+       // Revolute, 2DOF or 3DOF translational segments
+       IK_QTranslateSegment(int axis1);
+       IK_QTranslateSegment(int axis1, int axis2);
+       IK_QTranslateSegment();
+
+       MT_Vector3 Axis(int dof) const;
+
+       bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
+       void Lock(int, IK_QJacobian&, MT_Vector3&) {};
+       void UpdateAngleApply();
+
+       void SetWeight(int axis, MT_Scalar weight);
 
+private:
+       int m_axis[3];
+       bool m_axis_enabled[3];
+       MT_Vector3 m_new_translation;
 };
 
 #endif
diff --git a/intern/iksolver/intern/IK_QSolver_Class.h b/intern/iksolver/intern/IK_QSolver_Class.h
deleted file mode 100644 (file)
index 3f69140..0000000
+++ /dev/null
@@ -1,108 +0,0 @@
-/**
- * $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
- */
-
-#ifndef NAN_INCLUDED_IK_Solver_Class
-#define NAN_INCLUDED_IK_Solver_Class 
-
-#include "IK_QChain.h"
-#include "IK_QJacobianSolver.h"
-#include "IK_QSegment.h"
-#include "MEM_SmartPtr.h"
-
-/**
- * This class just contains all instances of internal data 
- * associated with the external chain structure needed for 
- * an ik solve. A pointer to this class gets hidden in the 
- * external structure as a void pointer.
- */
-
-class IK_QSolver_Class {
-
-public :
-
-       static 
-               IK_QSolver_Class *
-       New(
-       ){
-               MEM_SmartPtr<IK_QSolver_Class> output (new IK_QSolver_Class);
-       
-               MEM_SmartPtr<IK_QJacobianSolver> solver (IK_QJacobianSolver::New());
-
-               if (output == NULL ||
-                       solver == NULL
-               ) {
-                       return NULL;
-               }
-
-               output->m_solver = solver.Release();
-       
-               return output.Release();
-       };
-       
-               IK_QChain &     
-       Chain(
-       ) {
-               return m_chain;
-       };
-               
-               IK_QJacobianSolver &
-       Solver(
-       ) {
-               return m_solver.Ref();
-       }
-
-       ~IK_QSolver_Class(
-       ) {
-               // nothing to do
-       }
-
-
-private :
-
-       IK_QSolver_Class(
-       ) {
-       };
-
-       IK_QChain m_chain;
-       MEM_SmartPtr<IK_QJacobianSolver> m_solver;
-
-};     
-
-#endif
-
index d04b0de0daf7530babc5e77d6bab8b0e3ff124b2..a335a94616615a54689634300956142433a4c284 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original Author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** 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());   
-       
+#include "IK_QJacobianSolver.h"
+#include "IK_QSegment.h"
+#include "IK_QTask.h"
 
-       if (output == NULL || output_void == NULL) {
-               return NULL;
+#include <list>
+#include <iostream>
+using namespace std;
+
+typedef struct {
+       IK_QJacobianSolver solver;
+       IK_QSegment *root;
+       std::list<IK_QTask*> tasks;
+} IK_QSolver;
+
+IK_Segment *IK_CreateSegment(int flag)
+{
+       int ndof = 0;
+       ndof += (flag & IK_XDOF)? 1: 0;
+       ndof += (flag & IK_YDOF)? 1: 0;
+       ndof += (flag & IK_ZDOF)? 1: 0;
+
+       IK_QSegment *seg;
+
+       if (ndof == 0)
+               seg = new IK_QNullSegment();
+       else if (ndof == 1) {
+               int axis;
+
+               if (flag & IK_XDOF) axis = 0;
+               else if (flag & IK_YDOF) axis = 1;
+               else axis = 2;
+
+               if (flag & IK_TRANSLATIONAL)
+                       seg = new IK_QTranslateSegment(axis);
+               else
+                       seg = new IK_QRevoluteSegment(axis);
+       }
+       else if (ndof == 2) {
+               int axis1, axis2;
+
+               if (flag & IK_XDOF) {
+                       axis1 = 0;
+                       axis2 = (flag & IK_YDOF)? 1: 2;
+               }
+               else {
+                       axis1 = 1;
+                       axis2 = 2;
+               }
+
+               if (flag & IK_TRANSLATIONAL)
+                       seg = new IK_QTranslateSegment(axis1, axis2);
+               else {
+                       if (axis1 + axis2 == 2)
+                               seg = new IK_QSwingSegment();
+                       else
+                               seg = new IK_QElbowSegment((axis1 == 0)? 0: 2);
+               }
+       }
+       else {
+               if (flag & IK_TRANSLATIONAL)
+                       seg = new IK_QTranslateSegment();
+               else
+                       seg = new IK_QSphericalSegment();
        }
 
-       output->chain_dof = 0;
-       output->num_segments = 0;
-       output->segments = NULL;
-       output->intern = output_void.Release();
-       return output.Release();
-};
-
+       return (IK_Segment*)seg;
+}
 
-       int 
-IK_LoadChain(
-       IK_Chain_ExternPtr chain,
-       IK_Segment_ExternPtr segments,
-       int num_segs
-) {
+void IK_FreeSegment(IK_Segment *seg)
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
 
-       if (chain == NULL || segments == NULL) return 0;
+       delete qseg;
+}
 
-       IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
-       if (intern_cpp == NULL) return 0;
+void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
 
-       std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();       
-       if (segs.size() != (unsigned int) num_segs) {
-               segs = std::vector<IK_QSegment>(num_segs);
-       }
+       qseg->SetParent((IK_QSegment*)parent);
+}
 
-       std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
-       std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
+void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
+
+       MT_Vector3 mstart(start);
+       // convert from blender column major to moto row major
+       MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
+                           basis[0][1], basis[1][1], basis[2][1],
+                           basis[0][2], basis[1][2], basis[2][2]);
+       MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
+                          rest[0][1], rest[1][1], rest[2][1],
+                          rest[0][2], rest[1][2], rest[2][2]);
+       MT_Scalar mlength(length);
+
+       qseg->SetTransform(mstart, mrest, mbasis, mlength);
+}
 
-       IK_Segment_ExternPtr extern_seg_it = segments;
-       
+void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
 
-       for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) {
-       
-               MT_Point3 tr1(extern_seg_it->seg_start);
+       if (axis == IK_X)
+               qseg->SetLimit(0, lmin, lmax);
+       else if (axis == IK_Y)
+               qseg->SetLimit(1, lmin, lmax);
+       else if (axis == IK_Z)
+               qseg->SetLimit(2, lmin, lmax);
+}
 
-               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]
-               );
+void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
+{
+       if (stiffness < 1.0)
+               return;
 
-               MT_Scalar length(extern_seg_it->length);
+       IK_QSegment *qseg = (IK_QSegment*)seg;
+       MT_Scalar weight = 1.0/stiffness;
 
+       if (axis == IK_X)
+               qseg->SetWeight(0, weight);
+       else if (axis == IK_Y)
+               qseg->SetWeight(1, weight);
+       else if (axis == IK_Z)
+               qseg->SetWeight(2, weight);
+}
 
-               *seg_begin = IK_QSegment( 
-                       tr1,A,length,MT_Vector3(0,0,0)
-               );
+void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
+       const MT_Matrix3x3& change = qseg->BasisChange();
+
+       // convert from moto row major to blender column major
+       basis_change[0][0] = (float)change[0][0];
+       basis_change[1][0] = (float)change[0][1];
+       basis_change[2][0] = (float)change[0][2];
+       basis_change[0][1] = (float)change[1][0];
+       basis_change[1][1] = (float)change[1][1];
+       basis_change[2][1] = (float)change[1][2];
+       basis_change[0][2] = (float)change[2][0];
+       basis_change[1][2] = (float)change[2][1];
+       basis_change[2][2] = (float)change[2][2];
+}
 
-       }
+void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
+{
+       IK_QSegment *qseg = (IK_QSegment*)seg;
+       const MT_Vector3& change = qseg->TranslationChange();
 
+       translation_change[0] = (float)change[0];
+       translation_change[1] = (float)change[1];
+       translation_change[2] = (float)change[2];
+}
 
-       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
+IK_Solver *IK_CreateSolver(IK_Segment *root)
+{
+       if (root == NULL)
+               return NULL;
        
-       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();
+       IK_QSolver *solver = new IK_QSolver();
+       solver->root = (IK_QSegment*)root;
+
+       return (IK_Solver*)solver;
+}
 
-       for (;seg_begin != seg_end; ++seg_begin, ++output) {
-               MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix();
+void IK_FreeSolver(IK_Solver *solver)
+{
+       if (solver == NULL)
+               return;
 
-               // don't forget to transpose this qrot for use by blender!
+       IK_QSolver *qsolver = (IK_QSolver*)solver;
+       std::list<IK_QTask*>& tasks = qsolver->tasks;
+       std::list<IK_QTask*>::iterator task;
 
-               qrot.transpose(); // blender uses transpose here ????
+       for (task = tasks.begin(); task != tasks.end(); task++)
+               delete (*task);
+       
+       delete qsolver;
+}
 
-               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]);
+void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
+{
+       if (solver == NULL || tip == NULL)
+               return;
 
-       }
+       IK_QSolver *qsolver = (IK_QSolver*)solver;
+       IK_QSegment *qtip = (IK_QSegment*)tip;
 
+       MT_Vector3 pos(goal);
 
-       return solve_result ? 1 : 0;
+       // qsolver->tasks.empty()
+       IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
+       ee->SetWeight(weight);
+       qsolver->tasks.push_back(ee);
 }
 
-       void 
-IK_FreeChain(
-       IK_Chain_ExternPtr chain
-){
-       IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
+void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
+{
+       if (solver == NULL || tip == NULL)
+               return;
 
-       delete(intern_cpp);
-       delete(chain);
+       IK_QSolver *qsolver = (IK_QSolver*)solver;
+       IK_QSegment *qtip = (IK_QSegment*)tip;
 
-}      
+       // convert from blender column major to moto row major
+       MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
+                        goal[0][1], goal[1][1], goal[2][1],
+                        goal[0][2], goal[1][2], goal[2][2]);
 
+       IK_QTask *orient = new IK_QOrientationTask(false, qtip, rot);
+       orient->SetWeight(weight);
+       qsolver->tasks.push_back(orient);
+}
 
+void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
+{
+       if (solver == NULL || root == NULL)
+               return;
 
+       IK_QSolver *qsolver = (IK_QSolver*)solver;
+       IK_QSegment *qroot = (IK_QSegment*)root;
 
+       // convert from blender column major to moto row major
+       MT_Vector3 center(goal);
 
-       
-       
+       IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
+       com->SetWeight(weight);
+       qsolver->tasks.push_back(com);
+}
 
+int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
+{
+       if (solver == NULL)
+               return 0;
 
-       
+       IK_QSolver *qsolver = (IK_QSolver*)solver;
 
+       IK_QSegment *root = qsolver->root;
+       IK_QJacobianSolver& jacobian = qsolver->solver;
+       std::list<IK_QTask*>& tasks = qsolver->tasks;
+       MT_Scalar tol = tolerance;
 
+       bool result = jacobian.Solve(root, tasks, tol, max_iterations);
+
+       return ((result)? 1: 0);
+}
 
index c936466c8d58c62b2dc6be69cb16207de9e0e85f..9b758103de8af91280ca7d18f88b798bfd917aa3 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original Author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
 
-/**
-
- * $Id$
- * Copyright (C) 2001 NaN Technologies B.V.
- *
- * @author Laurence
- */
-
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
-
 #include "MT_ExpMap.h"
 
 /** 
@@ -52,21 +41,19 @@ MT_ExpMap::
 setRotation(
        const MT_Quaternion &q
 ) {
-       // ok first normailize the quaternion
+       // ok first normalize the quaternion
        // then compute theta the axis-angle and the normalized axis v
        // scale v by theta and that's it hopefully!
 
-       MT_Quaternion qt = q.normalized();
+       m_q = q.normalized();
+       m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z());
 
-       MT_Vector3 axis(qt.x(),qt.y(),qt.z());
-       MT_Scalar cosp = qt.w();
-       MT_Scalar sinp = axis.length();
-       axis /= sinp;
+       MT_Scalar cosp = m_q.w();
+       m_sinp = m_v.length();
+       m_v /= m_sinp;
 
-       MT_Scalar theta = atan2(double(sinp),double(cosp));
-
-       axis *= theta;  
-       m_v = axis;
+       m_theta = atan2(double(m_sinp),double(cosp));
+       m_v *= m_theta;
 }
        
 /** 
@@ -74,35 +61,11 @@ setRotation(
  * representation
  */    
 
-       MT_Quaternion 
+       const MT_Quaternion&
 MT_ExpMap::
 getRotation(
 ) const {
-    MT_Scalar  cosp, sinp, theta;
-
-       MT_Quaternion q;
-
-       theta = m_v.length();
-    
-    cosp = MT_Scalar(cos(.5*theta));
-    sinp = MT_Scalar(sin(.5*theta));
-
-    q.w() = cosp;
-
-    if (theta < MT_EXPMAP_MINANGLE) {
-
-               MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - theta*theta/MT_Scalar(48.0)); /* Taylor Series for sinc */
-               q.x() = temp.x();
-               q.y() = temp.y();
-               q.z() = temp.z();
-       } else {
-               MT_Vector3 temp = m_v * (sinp/theta); /* Taylor Series for sinc */
-               q.x() = temp.x();
-               q.y() = temp.y();
-               q.z() = temp.z();
-       }
-
-    return q;
+       return m_q;
 }
        
 /** 
@@ -113,114 +76,82 @@ getRotation(
 MT_ExpMap::
 getMatrix(
 ) const {
-
-       MT_Quaternion q = getRotation();
-       return MT_Matrix3x3(q);
+       return MT_Matrix3x3(m_q);
 }
 
-
-
-
 /** 
- * Force a reparameterization of the exponential
- * map.
+ * Update & reparameterizate the exponential map
  */
 
-       bool
+       void
 MT_ExpMap::
-reParameterize(
-       MT_Scalar &theta
+update(
+       const MT_Vector3& dv
 ){
-    bool rep(false);
-    theta = m_v.length();
-
-    if (theta > MT_PI){
-               MT_Scalar scl = theta;
-               if (theta > MT_2_PI){   /* first get theta into range 0..2PI */
-                       theta = MT_Scalar(fmod(theta, MT_2_PI));
-                       scl = theta/scl;
-                       m_v *= scl;
-                       rep = true;
-               }
-               if (theta > MT_PI){
-                       scl = theta;
-                       theta = MT_2_PI - theta;
-                       scl = MT_Scalar(1.0) - MT_2_PI/scl;
-                       m_v *= scl;
-                       rep = true;
-               }
-    }
-       return rep;
+       m_v += dv;
 
+       angleUpdated();
 }
 
 /**
  * Compute the partial derivatives of the exponential
- * map  (dR/de - where R is a 4x4 rotation matrix formed 
- * from the map) and return them as a 4x4 matrix
+ * map  (dR/de - where R is a 3x3 rotation matrix formed 
+ * from the map) and return them as a 3x3 matrix
  */
 
-       MT_Matrix4x4
+       void
 MT_ExpMap::
 partialDerivatives(
-       const int i
+       MT_Matrix3x3& dRdx,
+       MT_Matrix3x3& dRdy,
+       MT_Matrix3x3& dRdz
 ) const {
        
-       MT_Quaternion q = getRotation();
-       MT_Quaternion dQdx;
+       MT_Quaternion dQdx[3];
 
-       MT_Matrix4x4 output;
+       compute_dQdVi(dQdx);
 
-       compute_dQdVi(i,dQdx);
-       compute_dRdVi(q,dQdx,output);
-
-       return output;
+       compute_dRdVi(dQdx[0], dRdx);
+       compute_dRdVi(dQdx[1], dRdy);
+       compute_dRdVi(dQdx[2], dRdz);
 }
 
        void
 MT_ExpMap::
 compute_dRdVi(
-       const MT_Quaternion &q,
        const MT_Quaternion &dQdvi,
-       MT_Matrix4x4 & dRdvi
+       MT_Matrix3x3 & dRdvi
 ) const {
 
-    MT_Scalar  prod[9];
-    
-    /* This efficient formulation is arrived at by writing out the
-     * entire chain rule product dRdq * dqdv in terms of 'q' and 
-     * noticing that all the entries are formed from sums of just
-     * nine products of 'q' and 'dqdv' */
-
-    prod[0] = -MT_Scalar(4)*q.x()*dQdvi.x();
-    prod[1] = -MT_Scalar(4)*q.y()*dQdvi.y();
-    prod[2] = -MT_Scalar(4)*q.z()*dQdvi.z();
-    prod[3] = MT_Scalar(2)*(q.y()*dQdvi.x() + q.x()*dQdvi.y());
-    prod[4] = MT_Scalar(2)*(q.w()*dQdvi.z() + q.z()*dQdvi.w());
-    prod[5] = MT_Scalar(2)*(q.z()*dQdvi.x() + q.x()*dQdvi.z());
-    prod[6] = MT_Scalar(2)*(q.w()*dQdvi.y() + q.y()*dQdvi.w());
-    prod[7] = MT_Scalar(2)*(q.z()*dQdvi.y() + q.y()*dQdvi.z());
-    prod[8] = MT_Scalar(2)*(q.w()*dQdvi.x() + q.x()*dQdvi.w());
-
-    /* first row, followed by second and third */
-    dRdvi[0][0] = prod[1] + prod[2];
-    dRdvi[0][1] = prod[3] - prod[4];
-    dRdvi[0][2] = prod[5] + prod[6];
-
-    dRdvi[1][0] = prod[3] + prod[4];
-    dRdvi[1][1] = prod[0] + prod[2];
-    dRdvi[1][2] = prod[7] - prod[8];
-
-    dRdvi[2][0] = prod[5] - prod[6];
-    dRdvi[2][1] = prod[7] + prod[8];
-    dRdvi[2][2] = prod[0] + prod[1];
-
-    /* the 4th row and column are all zero */
-    int       i;
-
-    for (i=0; i<3; i++)        
-      dRdvi[3][i] = dRdvi[i][3] = MT_Scalar(0);
-    dRdvi[3][3] = 0;
+       MT_Scalar  prod[9];
+       
+       /* This efficient formulation is arrived at by writing out the
+        * entire chain rule product dRdq * dqdv in terms of 'q' and 
+        * noticing that all the entries are formed from sums of just
+        * nine products of 'q' and 'dqdv' */
+
+       prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
+       prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
+       prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
+       prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
+       prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
+       prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
+       prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
+       prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
+       prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
+
+       /* first row, followed by second and third */
+       dRdvi[0][0] = prod[1] + prod[2];
+       dRdvi[0][1] = prod[3] - prod[4];
+       dRdvi[0][2] = prod[5] + prod[6];
+
+       dRdvi[1][0] = prod[3] + prod[4];
+       dRdvi[1][1] = prod[0] + prod[2];
+       dRdvi[1][2] = prod[7] - prod[8];
+
+       dRdvi[2][0] = prod[5] - prod[6];
+       dRdvi[2][1] = prod[7] + prod[8];
+       dRdvi[2][2] = prod[0] + prod[1];
 }
 
 // compute partial derivatives dQ/dVi
@@ -228,44 +159,93 @@ compute_dRdVi(
        void
 MT_ExpMap::
 compute_dQdVi(
-       const int i,
-       MT_Quaternion & dQdX
+       MT_Quaternion *dQdX
 ) const {
 
-    MT_Scalar   theta = m_v.length();
-    MT_Scalar   cosp(cos(MT_Scalar(.5)*theta)), sinp(sin(MT_Scalar(.5)*theta));
-    
-    MT_assert(i>=0 && i<3);
-
-    /* This is an efficient implementation of the derivatives given
-     * in Appendix A of the paper with common subexpressions factored out */
-    if (theta < MT_EXPMAP_MINANGLE){
-               const int i2 = (i+1)%3, i3 = (i+2)%3;
-               MT_Scalar Tsinc = MT_Scalar(0.5) - theta*theta/MT_Scalar(48.0);
-               MT_Scalar vTerm = m_v[i] * (theta*theta/MT_Scalar(40.0) - MT_Scalar(1.0)) / MT_Scalar(24.0);
-               
-               dQdX.w() = -.5*m_v[i]*Tsinc;
-               dQdX[i]  = m_v[i]* vTerm + Tsinc;
-               dQdX[i2] = m_v[i2]*vTerm;
-               dQdX[i3] = m_v[i3]*vTerm;
-    } else {
-               const int i2 = (i+1)%3, i3 = (i+2)%3;
-               const MT_Scalar  ang = 1.0/theta, ang2 = ang*ang*m_v[i], sang = sinp*ang;
-               const MT_Scalar  cterm = ang2*(.5*cosp - sang);
+       /* This is an efficient implementation of the derivatives given
+        * in Appendix A of the paper with common subexpressions factored out */
+
+       MT_Scalar sinc, termCoeff;
+
+       if (m_theta < MT_EXPMAP_MINANGLE) {
+               sinc = 0.5 - m_theta*m_theta/48.0;
+               termCoeff = (m_theta*m_theta/40.0 - 1.0)/24.0;
+       }
+       else {
+               MT_Scalar cosp = m_q.w();
+               MT_Scalar ang = 1.0/m_theta;
+
+               sinc = m_sinp*ang;
+               termCoeff = ang*ang*(0.5*cosp - sinc);
+       }
+
+       for (int i = 0; i < 3; i++) {
+               MT_Quaternion& dQdx = dQdX[i];
+               int i2 = (i+1)%3;
+               int i3 = (i+2)%3;
+
+               MT_Scalar term = m_v[i]*termCoeff;
                
-               dQdX[i]  = cterm*m_v[i] + sang;
-               dQdX[i2] = cterm*m_v[i2];
-               dQdX[i3] = cterm*m_v[i3];
-               dQdX.w() = MT_Scalar(-.5)*m_v[i]*sang;
-    }
+               dQdx[i] = term*m_v[i] + sinc;
+               dQdx[i2] = term*m_v[i2];
+               dQdx[i3] = term*m_v[i3];
+               dQdx.w() = -0.5*m_v[i]*sinc;
+       }
 }
 
+// reParametize away from singularity, updating
+// m_v and m_theta
 
+       void
+MT_ExpMap::
+reParametrize(
+){
+       if (m_theta > MT_PI) {
+               MT_Scalar scl = m_theta;
+               if (m_theta > MT_2_PI){ /* first get theta into range 0..2PI */
+                       m_theta = MT_Scalar(fmod(m_theta, MT_2_PI));
+                       scl = m_theta/scl;
+                       m_v *= scl;
+               }
+               if (m_theta > MT_PI){
+                       scl = m_theta;
+                       m_theta = MT_2_PI - m_theta;
+                       scl = MT_Scalar(1.0) - MT_2_PI/scl;
+                       m_v *= scl;
+               }
+       }
+}
 
+// compute cached variables
 
+       void
+MT_ExpMap::
+angleUpdated(
+){
+       m_theta = m_v.length();
 
+       reParametrize();
 
+       // compute quaternion, sinp and cosp
 
+       if (m_theta < MT_EXPMAP_MINANGLE) {
+               m_sinp = MT_Scalar(0.0);
 
+               /* Taylor Series for sinc */
+               MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
+               m_q.x() = temp.x();
+               m_q.y() = temp.y();
+               m_q.z() = temp.z();
+               m_q.w() = MT_Scalar(1.0);
+       } else {
+               m_sinp = MT_Scalar(sin(.5*m_theta));
+
+               /* Taylor Series for sinc */
+               MT_Vector3 temp = m_v * (m_sinp/m_theta);
+               m_q.x() = temp.x();
+               m_q.y() = temp.y();
+               m_q.z() = temp.z();
+               m_q.w() = MT_Scalar(cos(.5*m_theta));
+       }
+}
 
index 814932f42e0356b1026a9e1bc10701256a54b296..401993764aa47538d0a81423f27aa5a6946c89e4 100644 (file)
  *
  * The Original Code is: all of this file.
  *
- * Contributor(s): none yet.
+ * Original author: Laurence
+ * Contributor(s): Brecht
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
 
-/**
-
- * $Id$
- * Copyright (C) 2001 NaN Technologies B.V.
- *
- * @author Laurence
- */
-
 #ifndef MT_ExpMap_H
 #define MT_ExpMap_H
 
@@ -93,13 +86,13 @@ public:
         */ 
 
     MT_ExpMap() {}
-    MT_ExpMap(const MT_Vector3& v) : m_v(v) {}
+    MT_ExpMap(const MT_Vector3& v) : m_v(v) { angleUpdated(); }
 
-    MT_ExpMap(const float v[3]) : m_v(v) {}
-    MT_ExpMap(const double v[3]) : m_v(v) {}
+    MT_ExpMap(const float v[3]) : m_v(v) { angleUpdated(); }
+    MT_ExpMap(const double v[3]) : m_v(v) { angleUpdated(); }
 
     MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) :
-        m_v(x, y, z) {}
+        m_v(x, y, z) { angleUpdated(); }
 
        /** 
         * Construct an exponential map from a quaternion
@@ -119,12 +112,6 @@ public:
         * on this class and some of them have no direct meaning.
         */
 
-               MT_Vector3 &
-       vector(
-       ) {
-               return m_v;
-       };
-
        const 
                MT_Vector3 &
        vector(
@@ -146,7 +133,7 @@ public:
         * representation
         */     
        
-               MT_Quaternion 
+               const MT_Quaternion&
        getRotation(
        ) const;
 
@@ -159,17 +146,13 @@ public:
        ) const; 
        
        /** 
-        * Force a reparameterization check of the exponential
-        * map.
-        * @param theta returns the new axis-angle.
-        * @return true iff a reParameterization took place.
-        * Use this function whenever you adjust the  vector
-        * representing the exponential map.
+        * Update (and reparameterize) the expontial map
+        * @param dv delta update values.
         */
 
-               bool
-       reParameterize(
-               MT_Scalar &theta
+               void
+       update(
+               const MT_Vector3& dv
        );
 
        /**
@@ -178,37 +161,51 @@ public:
         * from the map) and return them as a 4x4 matrix
         */
 
-               MT_Matrix4x4
+               void
        partialDerivatives(
-               const int i
+               MT_Matrix3x3& dRdx,
+               MT_Matrix3x3& dRdy,
+               MT_Matrix3x3& dRdz
        ) const ;
        
 private :
+
+       // m_v contains the exponential map, the other variables are
+       // cached for efficiency
        
        MT_Vector3 m_v;
+       MT_Scalar m_theta, m_sinp;
+       MT_Quaternion m_q;
 
        // private methods
 
        // Compute partial derivatives dR (3x3 rotation matrix) / dVi (EM vector)
        // given the partial derivative dQ (Quaternion) / dVi (ith element of EM vector)
 
-
                void
        compute_dRdVi(
-               const MT_Quaternion &q,
                const MT_Quaternion &dQdV,
-               MT_Matrix4x4 & dRdVi
+               MT_Matrix3x3 & dRdVi
        ) const;
 
        // compute partial derivatives dQ/dVi
 
                void
        compute_dQdVi(
-               int i,
-               MT_Quaternion & dQdX
+               MT_Quaternion *dQdX
        ) const ; 
 
-               
+       // reparametrize away from singularity
+
+               void
+       reParametrize(
+       );
+
+       // (re-)compute cached variables
+
+               void
+       angleUpdated(
+       );
 };
 
 #endif
index af32d816a803dd750d6186cbc623215cfd8ae4da..fff4aec252d5fb59884daa28c094aacaf9bee5c9 100644 (file)
@@ -33,7 +33,8 @@
 
 LIBNAME = iksolver
 DIR = $(OCGDIR)/intern/$(LIBNAME)
-CCSRCS = IK_QChain.cpp IK_QJacobianSolver.cpp IK_QSegment.cpp MT_ExpMap.cpp IK_Solver.cpp
+CCSRCS = IK_QJacobianSolver.cpp IK_QSegment.cpp IK_Solver.cpp IK_QJacobian.cpp
+CCSRCS += IK_QTask.cpp
 
 include nan_compile.mk