diff --git a/intern/iksolver/intern/IK_CGChainSolver.cpp b/intern/iksolver/intern/IK_CGChainSolver.cpp deleted file mode 100644 index 4c91c6b8ab1..00000000000 --- a/intern/iksolver/intern/IK_CGChainSolver.cpp +++ /dev/null @@ -1,303 +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 ***** - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include "IK_CGChainSolver.h" -#include "IK_Segment.h" - -using namespace std; - - ChainPotential * -ChainPotential:: -New( - IK_Chain &chain -){ - return new ChainPotential(chain); -}; - - - -// End effector goal - - void -ChainPotential:: -SetGoal( - const MT_Vector3 goal -){ - m_goal = goal; -}; - -// Inherited from DifferentiablePotenialFunctionNd -////////////////////////////////////////////////// - - MT_Scalar -ChainPotential:: -Evaluate( - MT_Scalar x -){ - - // evaluate the function at postiion x along the line specified - // by the vector pair (m_line_pos,m_line_dir) - - m_temp_pos.newsize(m_line_dir.size()); - - TNT::vectorscale(m_temp_pos,m_line_dir,x); - TNT::vectoradd(m_temp_pos,m_line_pos); - - return Evaluate(m_temp_pos); -} - - MT_Scalar -ChainPotential:: -Derivative( - MT_Scalar x -){ - m_temp_pos.newsize(m_line_dir.size()); - m_temp_grad.newsize(m_line_dir.size()); - - TNT::vectorscale(m_temp_pos,m_line_dir,x); - TNT::vectoradd(m_temp_pos,m_line_pos); - - Derivative(m_temp_pos,m_temp_grad); - - return TNT::dot_prod(m_temp_grad,m_line_dir); -} - - - MT_Scalar -ChainPotential:: -Evaluate( - const TNT::Vector &x -){ - - - // set the vector of angles in the backup chain - - vector &segs = m_t_chain.Segments(); - vector::iterator seg_it = segs.begin(); - TNT::Vector::const_iterator a_it = x.begin(); -#if 0 - TNT::Vector::const_iterator a_end = x.end(); -#endif - // while we are iterating through the angles and segments - // we compute the current angle potenial - MT_Scalar angle_potential = 0; -#if 0 - - for (; a_it != a_end; ++a_it, ++seg_it) { - - MT_Scalar dif = (*a_it - seg_it->CentralAngle()); - dif *= dif * seg_it->Weight(); - angle_potential += dif; - seg_it->SetTheta(*a_it); - } -#endif - - int chain_dof = m_t_chain.DoF(); - int n; - - for (n=0; n < chain_dof;seg_it ++) { - n += seg_it->SetAngles(a_it + n); - } - - - m_t_chain.UpdateGlobalTransformations(); - - MT_Scalar output = (DistancePotential(m_t_chain.EndEffector(),m_goal) + angle_potential); - - return output; -}; - - void -ChainPotential:: -Derivative( - const TNT::Vector &x, - TNT::Vector &dy -){ - - m_distance_grad.newsize(3); - // set the vector of angles in the backup chain - - vector & segs = m_t_chain.Segments(); - vector::iterator seg_it = segs.begin(); - TNT::Vector::const_iterator a_it = x.begin(); - TNT::Vector::const_iterator a_end = x.end(); - - m_angle_grad.newsize(segs.size()); - m_angle_grad = 0; -#if 0 - // FIXME compute angle gradients - TNT::Vector::iterator ag_it = m_angle_grad.begin(); -#endif - - const int chain_dof = m_t_chain.DoF(); - for (int n=0; n < chain_dof;seg_it ++) { - n += seg_it->SetAngles(a_it + n); - } - - m_t_chain.UpdateGlobalTransformations(); - m_t_chain.ComputeJacobian(); - - DistanceGradient(m_t_chain.EndEffector(),m_goal); - - // use chain rule for calculating derivative - // of potenial function - TNT::matmult(dy,m_t_chain.TransposedJacobian(),m_distance_grad); -#if 0 - TNT::vectoradd(dy,m_angle_grad); -#endif - -}; - - - MT_Scalar -ChainPotential:: -DistancePotential( - MT_Vector3 pos, - MT_Vector3 goal -) const { - return (pos - goal).length2(); -} - -// update m_distance_gradient - - void -ChainPotential:: -DistanceGradient( - MT_Vector3 pos, - MT_Vector3 goal -){ - - MT_Vector3 output = 2*(pos - goal); - m_distance_grad.newsize(3); - - m_distance_grad[0] = output.x(); - m_distance_grad[1] = output.y(); - m_distance_grad[2] = output.z(); -} - - - IK_CGChainSolver * -IK_CGChainSolver:: -New( -){ - - MEM_SmartPtr output (new IK_CGChainSolver()); - MEM_SmartPtrsolver (IK_ConjugateGradientSolver::New()); - - if (output == NULL || solver == NULL) return NULL; - - output->m_grad_solver = solver.Release(); - return output.Release(); -}; - - - bool -IK_CGChainSolver:: -Solve( - IK_Chain & chain, - MT_Vector3 new_position, - MT_Scalar tolerance -){ - - // first build a potenial function for the chain - - m_potential = ChainPotential::New(chain); - if (m_potential == NULL) return false; - - m_potential->SetGoal(new_position); - - // make a TNT::vector to describe the current - // chain state - - TNT::Vector p; - p.newsize(chain.DoF()); - - TNT::Vector::iterator p_it = p.begin(); - vector::const_iterator seg_it = chain.Segments().begin(); - vector::const_iterator seg_end = chain.Segments().end(); - - for (; seg_it != seg_end; seg_it++) { - - int i; - int seg_dof = seg_it->DoF(); - for (i=0; i < seg_dof; ++i,++p_it) { - *p_it = seg_it->ActiveAngle(i); - } - } - - // solve the thing - int iterations(0); - MT_Scalar return_value(0); - - m_grad_solver->Solve( - p, - tolerance, - iterations, - return_value, - m_potential.Ref(), - 100 - ); - - // update this chain - - vector::iterator out_seg_it = chain.Segments().begin(); - TNT::Vector::const_iterator p_cit = p.begin(); - - const int chain_dof = chain.DoF(); - int n; - - for (n=0; n < chain_dof;out_seg_it ++) { - n += out_seg_it->SetAngles(p_cit + n); - } - - chain.UpdateGlobalTransformations(); - chain.ComputeJacobian(); - - return true; -} - -IK_CGChainSolver:: -~IK_CGChainSolver( -){ - //nothing to do -}; - - -IK_CGChainSolver:: -IK_CGChainSolver( -){ - //nothing to do; -}; - diff --git a/intern/iksolver/intern/IK_Chain.cpp b/intern/iksolver/intern/IK_Chain.cpp deleted file mode 100644 index a86a615c859..00000000000 --- a/intern/iksolver/intern/IK_Chain.cpp +++ /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 ***** - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include "IK_Chain.h" - -using namespace std; - -IK_Chain:: -IK_Chain( -) -{ - // nothing to do; -}; - -const - vector & -IK_Chain:: -Segments( -) const { - return m_segments; -}; - - vector & -IK_Chain:: -Segments( -){ - return m_segments; -}; - - void -IK_Chain:: -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) { - s_it->UpdateGlobal(global); - global = s_it->GlobalTransform(); - } - // 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(); - - MT_Matrix3x3 last_basis = last_t.getBasis(); - last_basis.transpose(); - MT_Vector3 m_end_pose = last_basis[2]; - - -}; - -const - TNT::Matrix & -IK_Chain:: -Jacobian( -) const { - return m_jacobian; -} ; - - -const - TNT::Matrix & -IK_Chain:: -TransposedJacobian( -) const { - return m_t_jacobian; -}; - - void -IK_Chain:: -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; - int i = 0; - - - for (n= 0; n < num_segs; n++) { - - const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis(); - const MT_Vector3 &origin = segs[n].GlobalSegmentStart(); - - MT_Vector3 p = origin-m_end_effector; - - // iterate through the active angle vectors of this segment - - int angle_ind =0; - int seg_dof = segs[n].DoF(); - - const std::vector & angle_vectors = segs[n].AngleVectors(); - - for (angle_ind = 0;angle_ind ::const_iterator s_end = m_segments.end(); - vector::const_iterator s_it = m_segments.begin(); - - int dof = 0; - - for (;s_it != s_end; ++s_it) { - dof += s_it->DoF(); - } - - return dof; -} - - - - - - - - - - - - diff --git a/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp b/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp deleted file mode 100644 index 6760ba432d2..00000000000 --- a/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp +++ /dev/null @@ -1,165 +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 ***** - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include "IK_ConjugateGradientSolver.h" - -#define EPS 1.0e-10 - - IK_ConjugateGradientSolver * -IK_ConjugateGradientSolver:: -New( -){ - return new IK_ConjugateGradientSolver(); -}; - -IK_ConjugateGradientSolver:: -~IK_ConjugateGradientSolver( -){ - //nothing to do -}; - -// Compute the minimum of the potenial function -// starting at point p. On return p contains the -// computed minima, iter the number of iterations performed, -// fret the potenial value at the minima - - void -IK_ConjugateGradientSolver:: -Solve( - TNT::Vector &p, - MT_Scalar ftol, - int &iter, - MT_Scalar &fret, - DifferentiablePotenialFunctionNd &potenial, - int max_its -){ - - int j; - MT_Scalar gg,gam,fp,dgg; - - int n = potenial.Dimension(); - ArmVectors(n); - - // initialize --- FIXME we probably have these - // values to hand already. - - fp = potenial.Evaluate(p); - potenial.Derivative(p,m_xi); - - for (j = 1; j<=n; j++) { - m_g(j) = -m_xi(j); - m_xi(j) = m_h(j) = m_g(j); - } - for (iter =1;iter <= max_its; iter++) { - LineMinimize(p,m_xi,fret,potenial); - - if (2 *TNT::abs(fret-fp) <= ftol*(TNT::abs(fret) + TNT::abs(fp) + EPS)) { - return; - } - fp = fret; - potenial.Derivative(p,m_xi); - dgg = gg = 0.0; - - for (j = 1; j<=n;j++) { - gg += m_g(j)*m_g(j); - //dgg+= xi(j)*xi(j); - dgg += (m_xi(j) + m_g(j))*m_xi(j); - } - if (gg == 0.0) { - return; - } - gam = dgg/gg; - - for (j = 1; j<=n; j++) { - m_g(j) = -m_xi(j); - m_xi(j) = m_h(j) = m_g(j) + gam*m_h(j); - } - } - // FIXME throw exception - //assert(false); -}; - - -IK_ConjugateGradientSolver:: -IK_ConjugateGradientSolver( -){ - //nothing to do -} - - void -IK_ConjugateGradientSolver:: -ArmVectors( - int dimension -){ - m_g.newsize(dimension); - m_h.newsize(dimension); - m_xi.newsize(dimension); - m_xi_temp.newsize(dimension); -}; - - void -IK_ConjugateGradientSolver:: -LineMinimize( - TNT::Vector & p, - const TNT::Vector & xi, - MT_Scalar &fret, - DifferentiablePotenialFunctionNd &potenial -){ - MT_Scalar ax(0),bx(1); // initial bracket guess - MT_Scalar cx,fa,fb,fc; - - MT_Scalar xmin(0); // the 1d function minima - - potenial.SetLineVector(p,xi); - IK_LineMinimizer::InitialBracket1d(ax,bx,cx,fa,fb,fc,potenial); - fret = IK_LineMinimizer::DerivativeBrent1d(ax,bx,cx,potenial,xmin,0.001); - - // x_min in minimum along line and corresponds with - // p[] + x_min *xi[] - - TNT::vectorscale(m_xi_temp,xi,xmin); - TNT::vectoradd(p,m_xi_temp); -}; - - -#undef EPS - - - - - - - - diff --git a/intern/iksolver/intern/IK_JacobianSolver.cpp b/intern/iksolver/intern/IK_JacobianSolver.cpp deleted file mode 100644 index 9cca4ec7786..00000000000 --- a/intern/iksolver/intern/IK_JacobianSolver.cpp +++ /dev/null @@ -1,270 +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 ***** - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include "IK_JacobianSolver.h" -#include "TNT/svd.h" - -using namespace std; - - IK_JacobianSolver * -IK_JacobianSolver:: -New( -){ - return new IK_JacobianSolver(); -} - - bool -IK_JacobianSolver:: -Solve( - IK_Chain &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 -){ - - ArmMatrices(chain.DoF()); - - for (int iterations = 0; iterations < max_iterations; iterations++) { - - - 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(); - ComputeInverseJacobian(chain,x_length,max_angle_change); - - ComputeBetas(chain,d_pos); - UpdateChain(chain); - chain.UpdateGlobalTransformations(); - } - - return false; -}; - -IK_JacobianSolver:: -~IK_JacobianSolver( -){ - // nothing to do -} - - -IK_JacobianSolver:: -IK_JacobianSolver( -){ - // nothing to do -}; - - void -IK_JacobianSolver:: -ComputeBetas( - IK_Chain &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_JacobianSolver:: -ComputeInverseJacobian( - IK_Chain &chain, - const MT_Scalar x_length, - const MT_Scalar max_angle_change -) { - - int dimension = 0; - - m_svd_u = 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]; - } - } - - m_svd_w = 0; - m_svd_v = 0; - - TNT::SVD_a(m_svd_u,m_svd_w,m_svd_v); - - // 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 = 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 = 0; - } - - lambda *= lambda; - - for (i= 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] < epsilon) { - m_svd_w[i] = 0; - } else { - m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); - } - } - - m_svd_w_diag.diagonal(m_svd_w); - - // FIXME optimize these matrix multiplications - // using the fact that m_svd_w_diag is diagonal! - - TNT::matmult(m_svd_temp1,m_svd_w_diag,m_svd_u_t); - TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1); - return dimension; - -} - - void -IK_JacobianSolver:: -UpdateChain( - IK_Chain &chain -){ - - // iterate through the set of angles and - // update their values from the d_thetas - - int n; - vector &segs = chain.Segments(); - - int chain_dof = chain.DoF(); - int seg_ind = 0; - for (n=0; n < chain_dof;seg_ind ++) { - n += segs[seg_ind].IncrementAngles(m_d_theta.begin() + n); - } -}; - - void -IK_JacobianSolver:: -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_u = 0; - m_svd_v = 0; - m_svd_w = 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); - -}; - - - - - - - - - - - - - - - - - diff --git a/intern/iksolver/intern/IK_Segment.cpp b/intern/iksolver/intern/IK_Segment.cpp deleted file mode 100644 index af7f240e55f..00000000000 --- a/intern/iksolver/intern/IK_Segment.cpp +++ /dev/null @@ -1,281 +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 ***** - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include "IK_Segment.h" - - -IK_Segment:: -IK_Segment ( - const MT_Point3 tr1, - const MT_Matrix3x3 A, - const MT_Scalar length, - const bool pitch_on, - const bool yaw_on, - const bool role_on -){ - m_transform.setOrigin(tr1); - m_transform.setBasis(A); - m_angles[0] =MT_Scalar(0); - m_angles[1] =MT_Scalar(0); - m_angles[2] =MT_Scalar(0); - - m_active_angles[0] = role_on; - m_active_angles[1] = yaw_on; - m_active_angles[2] = pitch_on; - m_length = length; - - if (role_on) { - m_angle_vectors.push_back(MT_Vector3(1,0,0)); - } - if (yaw_on) { - m_angle_vectors.push_back(MT_Vector3(0,1,0)); - } - if (pitch_on) { - m_angle_vectors.push_back(MT_Vector3(0,0,1)); - } - UpdateLocalTransform(); - - -}; - - -IK_Segment:: -IK_Segment ( -) { - m_transform.setIdentity(); - - m_angles[0] =MT_Scalar(0); - m_angles[1] =MT_Scalar(0); - m_angles[2] =MT_Scalar(0); - - m_active_angles[0] = false; - m_active_angles[1] = false; - m_active_angles[2] = false; - m_length = MT_Scalar(1); - - UpdateLocalTransform(); -} - - - -// accessors -//////////// - -// The length of the segment - -const - MT_Scalar -IK_Segment:: -Length( -) const { - return m_length; -} - - -// This is the transform from adjacent -// coordinate systems in the chain. - -const - MT_Transform & -IK_Segment:: -LocalTransform( -) const { - return m_local_transform; -} - - void -IK_Segment:: -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 * m_local_transform; -} - -const - MT_Transform & -IK_Segment:: -GlobalTransform( -) const { - return m_global_transform; -} - -const - MT_Vector3 & -IK_Segment:: -GlobalSegmentStart( -) const{ - return m_seg_start; -} - - -// Return the number of Degrees of Freedom -// for this segment - - int -IK_Segment:: -DoF( -) const { - return - (m_active_angles[0] == true) + - (m_active_angles[1] == true) + - (m_active_angles[2] == true); -} - - -// suspect interface... -// Increment the active angles (at most 3) by -// d_theta. Which angles are incremented depends -// on which are active. It returns DoF - - int -IK_Segment:: -IncrementAngles( - MT_Scalar *d_theta -){ - int i =0; - if (m_active_angles[0]) { - m_angles[0] += d_theta[i]; - i++; - } - if (m_active_angles[1]) { - m_angles[1] += d_theta[i]; - i++; - } - if (m_active_angles[2]) { - m_angles[2] += d_theta[i]; - i++; - } - UpdateLocalTransform(); - - return i; -} - - - int -IK_Segment:: -SetAngles( - const MT_Scalar *angles -){ - int i =0; - if (m_active_angles[0]) { - m_angles[0] = angles[i]; - i++; - } - if (m_active_angles[1]) { - m_angles[1] = angles[i]; - i++; - } - if (m_active_angles[2]) { - m_angles[2] = angles[i]; - i++; - } - UpdateLocalTransform(); - - return i; -} - - - void -IK_Segment:: -UpdateLocalTransform( -){ - // The local transformation is defined by - // a user defined translation and rotation followed by - // rotation by (roll,pitch,yaw) followed by - // a translation in x of m_length - - MT_Quaternion rotx,roty,rotz; - - rotx.setRotation(MT_Vector3(1,0,0),m_angles[0]); - roty.setRotation(MT_Vector3(0,1,0),m_angles[1]); - rotz.setRotation(MT_Vector3(0,0,1),m_angles[2]); - - MT_Quaternion rot = rotx * roty *rotz; - - MT_Transform rx(MT_Point3(0,0,0),rot); - - MT_Transform translation; - translation.setIdentity(); - translation.translate(MT_Vector3(0,m_length,0)); - - m_local_transform = m_transform * rx * translation; -}; - - -const - std::vector & -IK_Segment:: -AngleVectors( -) const{ - return m_angle_vectors; -}; - - MT_Scalar -IK_Segment:: -ActiveAngle( - int i -) const { - MT_assert((i >=0) && (i < DoF())); - - // umm want to return the ith active angle - // and not the ith angle - - int j; - int angles = -1; - for (j=0;j < 3;j++) { - if (m_active_angles[j]) angles ++; - if (i == angles) return m_angles[j]; - } - return m_angles[0]; -} - - MT_Scalar -IK_Segment:: -Angle( - int i -) const { - MT_assert((i >=0) && (i < 3)); - return m_angles[i]; -} - - - - -