From 665a6b2d9521bb4fb9fc57ee0c94f6a580d55c91 Mon Sep 17 00:00:00 2001 From: Ton Roosendaal Date: Sat, 27 Aug 2005 12:44:41 +0000 Subject: [PATCH] First commit of Brecht's new IK work. This only does the IK module. Don't start compiling yet! --- intern/iksolver/intern/IK_QChain.cpp | 264 ---- intern/iksolver/intern/IK_QChain.h | 211 ---- intern/iksolver/intern/IK_QJacobianSolver.cpp | 464 +++---- intern/iksolver/intern/IK_QJacobianSolver.h | 147 +-- intern/iksolver/intern/IK_QSegment.cpp | 1120 ++++++++++++----- intern/iksolver/intern/IK_QSegment.h | 416 +++--- intern/iksolver/intern/IK_QSolver_Class.h | 108 -- intern/iksolver/intern/IK_Solver.cpp | 373 +++--- intern/iksolver/intern/MT_ExpMap.cpp | 284 ++--- intern/iksolver/intern/MT_ExpMap.h | 71 +- intern/iksolver/intern/Makefile | 3 +- 11 files changed, 1674 insertions(+), 1787 deletions(-) delete mode 100644 intern/iksolver/intern/IK_QChain.cpp delete mode 100644 intern/iksolver/intern/IK_QChain.h delete mode 100644 intern/iksolver/intern/IK_QSolver_Class.h diff --git a/intern/iksolver/intern/IK_QChain.cpp b/intern/iksolver/intern/IK_QChain.cpp deleted file mode 100644 index de62195b985..00000000000 --- a/intern/iksolver/intern/IK_QChain.cpp +++ /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 -#endif - -#include "IK_QChain.h" - -using namespace std; - -IK_QChain:: -IK_QChain( -) -{ - // nothing to do; -}; - -const - vector & -IK_QChain:: -Segments( -) const { - return m_segments; -}; - - vector & -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::const_iterator s_end = m_segments.end(); - vector::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::reverse_iterator s_rit = m_segments.rbegin(); - vector::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 & -IK_QChain:: -Jacobian( -) const { - return m_jacobian; -} ; - - -const - TNT::Matrix & -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::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::const_iterator s_end = m_segments.end(); - vector::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 index df249fe3f67..00000000000 --- a/intern/iksolver/intern/IK_QChain.h +++ /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 -#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 & - 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 & - 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 & - Jacobian( - ) const ; - - /** - * @return A const reference to the last computed transposed jacobian matrix - */ - - const - TNT::Matrix & - 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 m_segments; - - /// The jacobain of the IK_QChain - TNT::Matrix m_jacobian; - - /// It's transpose - TNT::Matrix m_t_jacobian; - - MT_Vector3 m_end_effector; - MT_Vector3 m_end_pose; - - -}; - -#endif - diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp index 450baf8fa9e..4bd3d28ce69 100644 --- a/intern/iksolver/intern/IK_QJacobianSolver.cpp +++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp @@ -24,308 +24,224 @@ * * 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 -#endif - #include "IK_QJacobianSolver.h" -#include "TNT/svd.h" +//#include "analyze.h" +#include using namespace std; - IK_QJacobianSolver * -IK_QJacobianSolver:: -New( -){ - return new IK_QJacobianSolver(); +void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg) +{ + m_segments.push_back(seg); + + IK_QSegment *child; + for (child = seg->Child(); child; child = child->Sibling()) + AddSegmentList(child); } - 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 -){ +bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list& tasks) +{ + m_segments.clear(); + AddSegmentList(root); - const vector & segs = chain.Segments(); - if (segs.size() == 0) return false; - - // compute the goal direction from the base of the chain - // in global coordinates + // assing each segment a unique id for the jacobian + std::vector::iterator seg; + int num_dof = 0; - MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart(); - - - const MT_Scalar chain_max_extension = chain.MaxExtension(); - - bool do_parallel_check(false); - - 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::iterator task; + for (task = tasks.begin(); task != tasks.end(); task++) { + IK_QTask *qtask = *task; - ArmMatrices(chain.DoF()); - - for (int iterations = 0; iterations < max_iterations; iterations++) { - - // check to see if the chain is pointing in the right direction - - 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++; } - - 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(); - } - - - 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(); - - 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; - - for (row = 0; row < 3; row ++) { - for (column = 0; column < chain.Jacobian().num_cols(); column ++) { - m_svd_u[row][column] = chain.Jacobian()[row][column]; + else { + qtask->SetId(secondary_size); + secondary_size += qtask->Size(); + secondary_weight += qtask->Weight(); + secondary++; } } - 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. - - // 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 - - MT_Scalar epsilon = MT_Scalar(1e-10); - - for ( i = 0; i epsilon && m_svd_w[i] < w_min) { - w_min = m_svd_w[i]; - } - } - - MT_Scalar lambda(0); - - 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); - } - - - 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); - } - } - - - 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; - -} - - void -IK_QJacobianSolver:: -UpdateChain( - IK_QChain &chain -){ - - // iterate through the set of angles and - // update their values from the d_thetas - - vector &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); - } - -}; - - 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 &segs = chain.Segments(); - - int num_segs = segs.size(); - - if (num_segs == 0) { + if (primary_size == 0 || MT_fuzzyZero(primary_weight)) return false; + + m_secondary_enabled = (secondary > 0); + + // 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; + + if (qtask->Primary()) + qtask->SetWeight(qtask->Weight()*primary_rescale); + else + qtask->SetWeight(qtask->Weight()*secondary_rescale); } - MT_Scalar crossp_sum = 0; + // set matrix sizes + m_jacobian.ArmMatrices(num_dof, primary_size, primary); + if (secondary > 0) + m_jacobian_sub.ArmMatrices(num_dof, secondary_size, secondary); + // set dof weights int i; - for (i = 0; i < num_segs; i++) { - MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() - - segs[i].GlobalSegmentStart(); - global_seg_direction.normalize(); + 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)); - MT_Scalar crossp = (global_seg_direction.cross(goal_dir)).length(); - crossp_sum += MT_Scalar(fabs(crossp)); - } - - if (crossp_sum < MT_Scalar(0.01)) { - return true; - } else { - return false; - } + return true; } +bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm) +{ + // assing each segment a unique id for the jacobian + std::vector::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; + } + } + } + } + } + + if (minseg) { + minseg->Lock(mindof, m_jacobian, mindelta); + locked = true; + + if (minabsdelta > norm) + norm = minabsdelta; + } + + if (locked == false) + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { + (*seg)->UnLock(); + (*seg)->UpdateAngleApply(); + } + + return locked; +} + +bool IK_QJacobianSolver::Solve( + IK_QSegment *root, + std::list tasks, + MT_Scalar, + int max_iterations +) +{ + //double dt = analyze_time(); + + if (!Setup(root, tasks)) + return false; + + // iterate + for (int iterations = 0; iterations < max_iterations; iterations++) { + // update transform + root->UpdateTransform(MT_Transform::Identity()); + + std::list::iterator task; + + //bool done = true; + + // compute jacobian + for (task = tasks.begin(); task != tasks.end(); task++) { + if ((*task)->Primary()) + (*task)->ComputeJacobian(m_jacobian); + else + (*task)->ComputeJacobian(m_jacobian_sub); + + //printf("#> %f\n", (*task)->Distance()); + //if ((*task)->Distance() > 1e-4) + // done = false; + } + + /*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::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; + } + } + + //analyze_add_run(max_iterations, analyze_time()-dt); + return false; +} diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h index 2a01fd05f83..caa47b77539 100644 --- a/intern/iksolver/intern/IK_QJacobianSolver.h +++ b/intern/iksolver/intern/IK_QJacobianSolver.h @@ -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 ***** */ @@ -38,141 +39,49 @@ * @date 28/6/2001 */ -#include "TNT/cmat.h" #include +#include + #include "MT_Vector3.h" -#include "IK_QChain.h" +#include "IK_QJacobian.h" +#include "IK_QSegment.h" +#include "IK_QTask.h" -class IK_QJacobianSolver { - -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 tasks, const MT_Scalar tolerance, - const int max_iterations, - const MT_Scalar max_angle_change + const int max_iterations ); - ~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 - ); - - /** - * 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 m_beta; - - /// the vector of computed angle changes - TNT::Vector m_d_theta; - - /// the constraint gradients for each angle. - TNT::Vector m_dh; - - /// Space required for SVD computation - - TNT::Vector m_svd_w; - TNT::Vector m_svd_work_space; - TNT::Matrix m_svd_v; - TNT::Matrix m_svd_u; - - TNT::Matrix m_svd_w_diag; - TNT::Matrix m_svd_v_t; - TNT::Matrix m_svd_u_t; - TNT::Matrix m_svd_inverse; - TNT::Matrix m_svd_temp1; - +private: + void AddSegmentList(IK_QSegment *seg); + bool Setup(IK_QSegment *root, std::list& 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 m_segments; }; #endif diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp index 2b7c126f973..a380e29e6fa 100644 --- a/intern/iksolver/intern/IK_QSegment.cpp +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -24,344 +24,896 @@ * * 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 -#endif - #include "IK_QSegment.h" + #include +using namespace std; -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) -{ +// Utility functions - m_max_extension = tr1.length() + length; - - m_transform.setOrigin(tr1); - m_transform.setBasis(A); - - UpdateLocalTransform(); -}; - - -IK_QSegment:: -IK_QSegment ( -) : - m_length(0), - m_q (0,0,0), - m_max_extension(0) +static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis) { - // Intentionally empty + 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); } - - -// accessors -//////////// - -// The length of the segment - -const - MT_Scalar -IK_QSegment:: -Length( -) const { - return m_length; +static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis) +{ + return RotationMatrix(sin(angle), cos(angle), axis); } -const - MT_Scalar -IK_QSegment:: -MaxExtension( -) const { - return m_max_extension; -} - -// This is the transform from adjacent -// coordinate systems in the chain. - -const - MT_Transform & -IK_QSegment:: -LocalTransform( -) const { - return m_local_transform; +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); } -const - MT_ExpMap & -IK_QSegment:: -LocalJointParameter( -) const { - return m_q; +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; } - MT_Transform -IK_QSegment:: -UpdateGlobal( - const MT_Transform & global -){ - // compute the global transform - // and the start of the segment in global coordinates. - - m_seg_start = global * m_transform.getOrigin(); - m_global_transform = global; - - const MT_Transform new_global = GlobalTransform(); - - m_seg_end = new_global.getOrigin(); - - return new_global; +static MT_Matrix3x3 ComputeTwistMatrix(MT_Scalar tau) +{ + return RotationMatrix(tau, 1); } - MT_Transform -IK_QSegment:: -GlobalTransform( -) const { - return m_global_transform * m_local_transform; -} - +static void RemoveTwist(MT_Matrix3x3& R) +{ + // compute twist parameter + MT_Scalar tau = ComputeTwist(R); - MT_Transform -IK_QSegment:: -UpdateAccumulatedLocal( - const MT_Transform & acc_local -){ - m_accum_local = acc_local; - return m_local_transform * m_accum_local; -} + // compute twist matrix + MT_Matrix3x3 T = ComputeTwistMatrix(tau); -const - MT_Transform & -IK_QSegment:: -AccumulatedLocal( -) const { - return m_accum_local; + // remove twist + R = R*T.transposed(); } - MT_Vector3 -IK_QSegment:: -ComputeJacobianColumn( - int angle -) const { +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]); - MT_Transform translation; - translation.setIdentity(); - translation.translate(MT_Vector3(0,m_length,0)); + // 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; - // 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) + return MT_Vector3(ax, tau, az); +} -#if 0 +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); - // use euler angles as a test of the matrices and this method. + // compute swing matrix + MT_Matrix3x3 S(MT_Quaternion(ax, 0.0, az, -cosine2)); - MT_Matrix3x3 dRda; + return S; +} - MT_Quaternion rotx,roty,rotz; +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]); - rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); - roty.setRotation(MT_Vector3(0,1,0),m_q[1]); - rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); - - MT_Matrix3x3 rotx_m(rotx); - MT_Matrix3x3 roty_m(roty); - MT_Matrix3x3 rotz_m(rotz); - - if (angle == 0) { + MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1)/2); + MT_Scalar l = delta.length(); - MT_Scalar ci = cos(m_q[0]); - MT_Scalar si = sin(m_q[1]); - - dRda = MT_Matrix3x3( - 0, 0, 0, - 0,-si,-ci, - 0, ci,-si - ); - - dRda = rotz_m * roty_m * dRda; - } else + if (!MT_fuzzyZero(l)) + delta *= c/l; - if (angle == 1) { + return delta; +} + +// 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) +{ + 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); + + m_orig_basis = m_basis; + m_orig_translation = m_translation; +} + +MT_Matrix3x3 IK_QSegment::BasisChange() const +{ + return m_orig_basis.transposed()*m_basis; +} + +MT_Vector3 IK_QSegment::TranslationChange() const +{ + return m_translation - m_orig_translation; +} + +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; - MT_Scalar cj = cos(m_q[1]); - MT_Scalar sj = sin(m_q[1]); - - dRda = MT_Matrix3x3( - -sj, 0, cj, - 0, 0, 0, - -cj, 0,-sj - ); - - dRda = rotz_m * dRda * rotx_m; - } else + if (m_parent) + m_parent->RemoveChild(this); - if (angle == 2) { - - MT_Scalar ck = cos(m_q[2]); - MT_Scalar sk = sin(m_q[2]); - - dRda = MT_Matrix3x3( - -sk,-ck, 0, - ck,-sk, 0, - 0, 0, 0 - ); - - dRda = dRda * roty_m * rotx_m; + if (parent) { + m_sibling = parent->m_child; + parent->m_child = this; } - - MT_Transform dRda_t(MT_Point3(0,0,0),dRda); - // convert to 4x4 matrices coz dRda is singular. - MT_Matrix4x4 dRda_m(dRda_t); - dRda_m[3][3] = 0; + m_parent = parent; +} -#else +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; - // use exponential map derivatives - MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle); + while (seg->m_sibling != child); + seg = seg->m_sibling; -#endif + 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); +} + +// IK_QSphericalSegment + +IK_QSphericalSegment::IK_QSphericalSegment() +: IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false) +{ +} + +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); + + m_min_y = MT_radians(lmin); + m_max_y = MT_radians(lmax); + + 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; + } + } +} + +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(); + + 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 ysine = w.y()*sine; + MT_Scalar zsine = w.z()*sine; + + 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; + + MT_Matrix3x3 M( + cosine + xxcosine, -zsine + xycosine, ysine + xzcosine, + zsine + xycosine, cosine + yycosine, -xsine + yzcosine, + -ysine + xzcosine, xsine + yzcosine, cosine + zzcosine); + + m_new_basis = m_basis*M; + } + else + m_new_basis = m_basis; - // Once we have computed the local derivatives we can compute - // derivatives of the end effector. + if (m_limit_y == false && m_limit_x == false && m_limit_z == false) + return false; - // Imagine a chain consisting of 5 segments each with local - // transformation Li - // Then the global transformation G is L1 *L2 *L3 *L4 *L5 - // If we want to compute the partial derivatives of this expression - // w.r.t one of the angles x of L3 we should then compute - // dG/dx = d(L1 *L2 *L3 *L4 *L5)/dx - // = L1 *L2 * dL3/dx *L4 *L5 - // but L1 *L2 is the global transformation of the parent of this - // bone and L4 *L5 is the accumulated local transform of the children - // of this bone (m_accum_local) - // so dG/dx = m_global_transform * dL3/dx * m_accum_local - // - // so now we need to compute dL3/dx - // L3 = m_transform * rotation(m_q) * translate(0,m_length,0) - // do using the same procedure we get - // dL3/dx = m_transform * dR/dx * translate(0,m_length,0) - // dR/dx is the partial derivative of the exponential map w.r.t - // one of it's parameters. This is computed in MT_ExpMap + MT_Vector3 a = SphericalRangeParameters(m_new_basis); - 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); + 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; - - MT_Matrix4x4 dFda_m = global_m * transform_m * dRda_m * translation_m * accum_local_m; - - MT_Vector4 result = dFda_m * MT_Vector4(0,0,0,1); - return MT_Vector3(result[0],result[1],result[2]); -}; - - - -const - MT_Vector3 & -IK_QSegment:: -GlobalSegmentStart( -) const{ - return m_seg_start; -} - -const - MT_Vector3 & -IK_QSegment:: -GlobalSegmentEnd( -) const { - return m_seg_end; -} - - - void -IK_QSegment:: -IncrementAngle( - const MT_Vector3 &dq -){ - m_q.vector() += dq; - MT_Scalar theta(0); - m_q.reParameterize(theta); - - UpdateLocalTransform(); -} - - - void -IK_QSegment:: -SetAngle( - const MT_ExpMap &dq -){ - m_q = dq; - UpdateLocalTransform(); -} - - - void -IK_QSegment:: -UpdateLocalTransform( -){ -#if 0 - - //use euler angles - test - MT_Quaternion rotx,roty,rotz; - - rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); - roty.setRotation(MT_Vector3(0,1,0),m_q[1]); - rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); - - MT_Matrix3x3 rotx_m(rotx); - MT_Matrix3x3 roty_m(roty); - MT_Matrix3x3 rotz_m(rotz); - - MT_Matrix3x3 rot = rotz_m * roty_m * rotx_m; -#else - - //use exponential map - MT_Matrix3x3 rot = m_q.getMatrix(); + MT_Scalar ax = a.x(), ay = a.y(), az = a.z(); + clamp[0] = clamp[1] = clamp[2] = false; -#endif + 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; + } + } - MT_Transform rx(MT_Point3(0,0,0),rot); + if (m_limit_x && m_limit_z) { + /* check in ellipsoid region */ + ax = a.x() + m_offset_x; + az = a.z() + m_offset_z; - MT_Transform translation; - translation.setIdentity(); - translation.translate(MT_Vector3(0,m_length,0)); + MT_Scalar invX = 1.0/(m_max_x*m_max_x); + MT_Scalar invZ = 1.0/(m_max_z*m_max_z); - m_local_transform = m_transform * rx * translation; -}; + 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; + } + + 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; + } + + 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; + + 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) +{ +} + +// IK_QRevoluteSegment + +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; + + 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; + + // clamp and convert to axis angle parameters + lmin = MT_clamp(lmin, -180, 180); + lmax = MT_clamp(lmax, -180, 180); + + m_min = MT_radians(lmin); + m_max = MT_radians(lmax); + + m_limit = true; +} + +void IK_QRevoluteSegment::SetWeight(int axis, MT_Scalar weight) +{ + if (axis == m_axis) + m_weight[0] = weight; +} + +// IK_QSwingSegment + +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; + + 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; + + 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; + } + + if (clamp[0] == false && clamp[1] == false) + return false; + + m_new_basis = ComputeSwingMatrix(ax, az); + + delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis); + delta[1] = delta[2]; delta[2] = 0.0; + + 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; +} + +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) +{ +} + +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; + } + } + } + + 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; + } + } + } + + return (clamp[0] || clamp[1]); +} + +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; + + m_sin_twist = sin(m_twist); + m_cos_twist = cos(m_twist); + + MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis); + MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1); + + m_basis = m_orig_basis*A*T; +} + +void IK_QElbowSegment::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 = MT_radians(lmin); + lmax = MT_radians(lmax); + + 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; + + m_axis[0] = axis1; + m_axis[1] = axis2; +} + +IK_QTranslateSegment::IK_QTranslateSegment() +: IK_QSegment(3, true) +{ + m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true; + + 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; +} diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h index 7d4540f0543..c6367dc4e03 100644 --- a/intern/iksolver/intern/IK_QSegment.h +++ b/intern/iksolver/intern/IK_QSegment.h @@ -24,39 +24,30 @@ * * 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 /** * 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 @@ -71,220 +62,263 @@ * use exactly the same transformations when displaying the segments */ -class IK_QSegment { +class IK_QSegment +{ +public: + virtual ~IK_QSegment(); -public : + // start: a user defined translation + // rest_basis: a user defined rotation + // basis: a user defined rotation + // length: length of this segment - /** - * 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 + 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 - */ + IK_QSegment *Parent() const + { return m_parent; } - const - MT_Scalar - Length( - ) const ; + // number of degrees of freedom + int NumberOfDoF() const + { return m_num_DoF; } - /** - * @return the max distance of the end of this - * bone from the local origin. - */ + // unique id for this segment, for identification in the jacobian + int DoFId() const + { return m_DoF_id; } - const - MT_Scalar - MaxExtension( - ) const ; + void SetDoFId(int dof_id) + { m_DoF_id = dof_id; } - /** - * @return The transform from adjacent - * coordinate systems in the chain. - */ + // the max distance of the end of this bone from the local origin. + const MT_Scalar MaxExtension() const + { return m_max_extension; } - const - MT_Transform & - LocalTransform( - ) const ; + // the change in rotation and translation w.r.t. the rest pose + MT_Matrix3x3 BasisChange() const; + MT_Vector3 TranslationChange() const; - const - MT_ExpMap & - LocalJointParameter( - ) const; + // the start and end of the segment + const MT_Point3 &GlobalStart() const + { return m_global_start; } - /** - * 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. - */ + const MT_Point3 &GlobalEnd() const + { return m_global_transform.getOrigin(); } - MT_Transform - UpdateGlobal( - const MT_Transform & global - ); + // the global transformation at the end of the segment + const MT_Transform &GlobalTransform() const + { return m_global_transform; } - /** - * @return The global transformation - */ + // is a translational segment? + bool Translational() const + { return m_translational; } - 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 - ); + // locking (during inner clamping loop) + bool Locked(int dof) const + { return m_locked[dof]; } - /** - * @return A const reference to accumulated local - * transform of this segment. - */ + void UnLock() + { m_locked[0] = m_locked[1] = m_locked[2] = false; } - const - MT_Transform & - AccumulatedLocal( - ) const; - - /** - * @return A const Reference to start of segment in global - * coordinates - */ - - const - MT_Vector3 & - GlobalSegmentStart( - ) const; + // per dof joint weighting + MT_Scalar Weight(int dof) const + { return m_weight[dof]; } - /** - * @return A const Reference to end of segment in global - * coordinates - */ + void ScaleWeight(int dof, MT_Scalar scale) + { m_weight[dof] *= scale; } - const - MT_Vector3 & - GlobalSegmentEnd( - ) const; + // recursively update the global coordinates of this segment, 'global' + // is the global transformation from the parent segment + void UpdateTransform(const MT_Transform &global); + // get axis from rotation matrix for derivative computation + virtual MT_Vector3 Axis(int dof) const=0; - /** - * @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] - */ + // 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; - MT_Vector3 - ComputeJacobianColumn( - int angle - ) const ; - - /** - * Explicitly set the angle parameterization value. - */ + // set joint limits + virtual void SetLimit(int, MT_Scalar, MT_Scalar) {}; - void - SetAngle( - const MT_ExpMap &q - ); + // set joint weights (per axis) + virtual void SetWeight(int, MT_Scalar) {}; - /** - * Increment the angle parameterization value. - */ +protected: - void - IncrementAngle( - const MT_Vector3 &dq - ); + // 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); - /** - * Return the parameterization of this angle - */ + // tree structure variables + IK_QSegment *m_parent; + IK_QSegment *m_child; + IK_QSegment *m_sibling; - const - MT_ExpMap & - ExpMap( - ) const { - return m_q; - }; + // 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; -private : - - void - UpdateLocalTransform( - ); - - -private : - - /** - * m_transform The user defined transformation, composition of the - * translation and rotation from constructor. - */ - MT_Transform m_transform; - - /** - * The exponential map parameterization of this joint. - */ - - MT_Scalar m_length; - MT_ExpMap m_q; - - /** - * The maximum extension of this segment - * This is the magnitude of the user offset + the length of the - * chain - */ - + // 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 index 3f69140640d..00000000000 --- a/intern/iksolver/intern/IK_QSolver_Class.h +++ /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 output (new IK_QSolver_Class); - - MEM_SmartPtr 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 m_solver; - -}; - -#endif - diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp index d04b0de0daf..a335a946166 100644 --- a/intern/iksolver/intern/IK_Solver.cpp +++ b/intern/iksolver/intern/IK_Solver.cpp @@ -24,177 +24,258 @@ * * 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 -#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 "IK_QJacobianSolver.h" +#include "IK_QSegment.h" +#include "IK_QTask.h" + +#include #include +using namespace std; - IK_Chain_ExternPtr -IK_CreateChain( - void -) { +typedef struct { + IK_QJacobianSolver solver; + IK_QSegment *root; + std::list tasks; +} IK_QSolver; - MEM_SmartPtr output (new IK_Chain_Extern); - MEM_SmartPtr output_void (IK_QSolver_Class::New()); - +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; - if (output == NULL || output_void == NULL) { - return NULL; + 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(); -}; - - - int -IK_LoadChain( - IK_Chain_ExternPtr chain, - IK_Segment_ExternPtr segments, - int num_segs -) { - - if (chain == NULL || segments == NULL) return 0; - - IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern; - if (intern_cpp == NULL) return 0; - - std::vector & segs = intern_cpp->Chain().Segments(); - if (segs.size() != (unsigned int) num_segs) { - segs = std::vector(num_segs); - } - - std::vector::const_iterator seg_end= segs.end(); - std::vector::iterator seg_begin= segs.begin(); - - IK_Segment_ExternPtr extern_seg_it = segments; - - - for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) { - - MT_Point3 tr1(extern_seg_it->seg_start); - - MT_Matrix3x3 A( - extern_seg_it->basis[0],extern_seg_it->basis[1],extern_seg_it->basis[2], - extern_seg_it->basis[3],extern_seg_it->basis[4],extern_seg_it->basis[5], - extern_seg_it->basis[6],extern_seg_it->basis[7],extern_seg_it->basis[8] - ); - - MT_Scalar length(extern_seg_it->length); - - - *seg_begin = IK_QSegment( - tr1,A,length,MT_Vector3(0,0,0) - ); - - } - - - intern_cpp->Chain().UpdateGlobalTransformations(); - intern_cpp->Chain().ComputeJacobian(); - - chain->num_segments = num_segs; - chain->chain_dof = intern_cpp->Chain().DoF(); - chain->segments = segments; - - return 1; -}; - - int -IK_SolveChain( - IK_Chain_ExternPtr chain, - float goal[3], - float tolerance, - int max_iterations, - float max_angle_change, - IK_Segment_ExternPtr output -){ - if (chain == NULL || output == NULL) return 0; - if (chain->intern == NULL) return 0; - - IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern; - - IK_QJacobianSolver & solver = intern_cpp->Solver(); - - bool solve_result = solver.Solve( - intern_cpp->Chain(), - MT_Vector3(goal), - MT_Vector3(0,0,0), - MT_Scalar(tolerance), - max_iterations, - MT_Scalar(max_angle_change) - ); - - // turn the computed role->pitch->yaw into a quaternion and - // return the result in output - - std::vector & segs = intern_cpp->Chain().Segments(); - std::vector::const_iterator seg_end= segs.end(); - std::vector::iterator seg_begin= segs.begin(); - - for (;seg_begin != seg_end; ++seg_begin, ++output) { - MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix(); - - // don't forget to transpose this qrot for use by blender! - - qrot.transpose(); // blender uses transpose here ???? - - output->basis_change[0] = float(qrot[0][0]); - output->basis_change[1] = float(qrot[0][1]); - output->basis_change[2] = float(qrot[0][2]); - output->basis_change[3] = float(qrot[1][0]); - output->basis_change[4] = float(qrot[1][1]); - output->basis_change[5] = float(qrot[1][2]); - output->basis_change[6] = float(qrot[2][0]); - output->basis_change[7] = float(qrot[2][1]); - output->basis_change[8] = float(qrot[2][2]); - - } - - - return solve_result ? 1 : 0; + return (IK_Segment*)seg; } - void -IK_FreeChain( - IK_Chain_ExternPtr chain -){ - IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern; +void IK_FreeSegment(IK_Segment *seg) +{ + IK_QSegment *qseg = (IK_QSegment*)seg; - delete(intern_cpp); - delete(chain); + delete qseg; +} -} +void IK_SetParent(IK_Segment *seg, IK_Segment *parent) +{ + IK_QSegment *qseg = (IK_QSegment*)seg; + qseg->SetParent((IK_QSegment*)parent); +} +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); +} +void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax) +{ + IK_QSegment *qseg = (IK_QSegment*)seg; + + 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); +} + +void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness) +{ + if (stiffness < 1.0) + return; + + 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); +} + +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]; +} + +IK_Solver *IK_CreateSolver(IK_Segment *root) +{ + if (root == NULL) + return NULL; + IK_QSolver *solver = new IK_QSolver(); + solver->root = (IK_QSegment*)root; + + return (IK_Solver*)solver; +} + +void IK_FreeSolver(IK_Solver *solver) +{ + if (solver == NULL) + return; + + IK_QSolver *qsolver = (IK_QSolver*)solver; + std::list& tasks = qsolver->tasks; + std::list::iterator task; + + for (task = tasks.begin(); task != tasks.end(); task++) + delete (*task); + delete qsolver; +} +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); + // qsolver->tasks.empty() + IK_QTask *ee = new IK_QPositionTask(true, qtip, pos); + ee->SetWeight(weight); + qsolver->tasks.push_back(ee); +} + +void IK_SolverAddGoalOrientation(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; + + // 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& tasks = qsolver->tasks; + MT_Scalar tol = tolerance; + + bool result = jacobian.Solve(root, tasks, tol, max_iterations); + + return ((result)? 1: 0); +} diff --git a/intern/iksolver/intern/MT_ExpMap.cpp b/intern/iksolver/intern/MT_ExpMap.cpp index c936466c8d5..9b758103de8 100644 --- a/intern/iksolver/intern/MT_ExpMap.cpp +++ b/intern/iksolver/intern/MT_ExpMap.cpp @@ -24,23 +24,12 @@ * * 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 -#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' */ + 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()); + 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]; + /* 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[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; + 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 */ - /* 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); + 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.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); - - 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)); + } +} diff --git a/intern/iksolver/intern/MT_ExpMap.h b/intern/iksolver/intern/MT_ExpMap.h index 814932f42e0..401993764aa 100644 --- a/intern/iksolver/intern/MT_ExpMap.h +++ b/intern/iksolver/intern/MT_ExpMap.h @@ -24,19 +24,12 @@ * * 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 diff --git a/intern/iksolver/intern/Makefile b/intern/iksolver/intern/Makefile index af32d816a80..fff4aec252d 100644 --- a/intern/iksolver/intern/Makefile +++ b/intern/iksolver/intern/Makefile @@ -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