Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorKent Mein <mein@cs.umn.edu>2002-12-03 15:11:07 +0300
committerKent Mein <mein@cs.umn.edu>2002-12-03 15:11:07 +0300
commit671a355e9f49ede62794284dd056cb3d6cc77cef (patch)
tree7f7ae6dfcc27f71c1d0b891d1aaa31dd56be2da6 /intern/iksolver
parent5b88406c3f58ee717ac8cd348b1e3fdbe39ff2ec (diff)
Got rid of unused files in intern/iksolver
(removed them from cvs and from the Makefile.am) Kent -- mein@cs.umn.edu
Diffstat (limited to 'intern/iksolver')
-rw-r--r--intern/iksolver/intern/IK_CGChainSolver.cpp303
-rw-r--r--intern/iksolver/intern/IK_Chain.cpp211
-rw-r--r--intern/iksolver/intern/IK_ConjugateGradientSolver.cpp165
-rw-r--r--intern/iksolver/intern/IK_JacobianSolver.cpp270
-rw-r--r--intern/iksolver/intern/IK_Segment.cpp281
5 files changed, 0 insertions, 1230 deletions
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 <config.h>
-#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<MT_Scalar> &x
-){
-
-
- // set the vector of angles in the backup chain
-
- vector<IK_Segment> &segs = m_t_chain.Segments();
- vector<IK_Segment>::iterator seg_it = segs.begin();
- TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
-#if 0
- TNT::Vector<MT_Scalar>::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<MT_Scalar> &x,
- TNT::Vector<MT_Scalar> &dy
-){
-
- m_distance_grad.newsize(3);
- // set the vector of angles in the backup chain
-
- vector<IK_Segment> & segs = m_t_chain.Segments();
- vector<IK_Segment>::iterator seg_it = segs.begin();
- TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
- TNT::Vector<MT_Scalar>::const_iterator a_end = x.end();
-
- m_angle_grad.newsize(segs.size());
- m_angle_grad = 0;
-#if 0
- // FIXME compute angle gradients
- TNT::Vector<MT_Scalar>::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<IK_CGChainSolver> output (new IK_CGChainSolver());
- MEM_SmartPtr<IK_ConjugateGradientSolver>solver (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<MT_Scalar> p;
- p.newsize(chain.DoF());
-
- TNT::Vector<MT_Scalar>::iterator p_it = p.begin();
- vector<IK_Segment>::const_iterator seg_it = chain.Segments().begin();
- vector<IK_Segment>::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<IK_Segment>::iterator out_seg_it = chain.Segments().begin();
- TNT::Vector<MT_Scalar>::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 <config.h>
-#endif
-
-#include "IK_Chain.h"
-
-using namespace std;
-
-IK_Chain::
-IK_Chain(
-)
-{
- // nothing to do;
-};
-
-const
- vector<IK_Segment> &
-IK_Chain::
-Segments(
-) const {
- return m_segments;
-};
-
- vector<IK_Segment> &
-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<IK_Segment>::const_iterator s_end = m_segments.end();
- vector<IK_Segment>::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<MT_Scalar> &
-IK_Chain::
-Jacobian(
-) const {
- return m_jacobian;
-} ;
-
-
-const
- TNT::Matrix<MT_Scalar> &
-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<IK_Segment>::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<MT_Vector3> & angle_vectors = segs[n].AngleVectors();
-
- for (angle_ind = 0;angle_ind <seg_dof; angle_ind++,i++) {
-
- MT_Vector3 angle_axis = angle_vectors[angle_ind];
-
- MT_Vector3 a = basis * angle_axis;
- MT_Vector3 pca = p.cross(a);
-
- m_t_jacobian(i + 1,1) = pca.x();
- m_t_jacobian(i + 1,2) = pca.y();
- m_t_jacobian(i + 1,3) = pca.z();
-
- }
- }
-
- // get the origina1 jacobain
-
- TNT::transpose(m_t_jacobian,m_jacobian);
-};
-
- MT_Vector3
-IK_Chain::
-EndEffector(
-) const {
- return m_end_effector;
-};
-
- MT_Vector3
-IK_Chain::
-EndPose(
-) const {
- return m_end_pose;
-};
-
-
- int
-IK_Chain::
-DoF(
-) const {
-
- // iterate through the segs and compute the DOF
-
- vector<IK_Segment>::const_iterator s_end = m_segments.end();
- vector<IK_Segment>::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 <config.h>
-#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<MT_Scalar> &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<MT_Scalar> & p,
- const TNT::Vector<MT_Scalar> & 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 <config.h>
-#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 <m_svd_w.size() ; i++) {
-
- if (m_svd_w[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<IK_Segment> &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 <config.h>
-#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<MT_Vector3> &
-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];
-}
-
-
-
-
-