Initial revision

intern/iksolver/Makefile

@@ -0,0 +1,52 @@
+#
+# $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 *****
+# iksolver main makefile.
+#
+
+include nan_definitions.mk
+
+LIBNAME = iksolver
+SOURCEDIR = intern/$(LIBNAME)
+DIR = $(OCGDIR)/$(SOURCEDIR)
+DIRS = intern
+TESTDIRS = test
+
+include nan_subdirs.mk
+
+install: all debug
+	@[ -d $(NAN_IKSOLVER) ] || mkdir $(NAN_IKSOLVER)
+	@[ -d $(NAN_IKSOLVER)/include ] || mkdir $(NAN_IKSOLVER)/include
+	@[ -d $(NAN_IKSOLVER)/lib ] || mkdir $(NAN_IKSOLVER)/lib
+	@[ -d $(NAN_IKSOLVER)/lib/debug ] || mkdir $(NAN_IKSOLVER)/lib/debug
+	cp -f $(DIR)/libiksolver.a $(NAN_IKSOLVER)/lib/
+	cp -f $(DIR)/debug/libiksolver.a $(NAN_IKSOLVER)/lib/debug/
+	cp -f extern/*.h $(NAN_IKSOLVER)/include/
+

intern/iksolver/extern/IK_solver.h

@@ -0,0 +1,207 @@
+/**
+ * $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
+ * @mainpage IK - Blender inverse kinematics module.
+ *
+ * @section about About the IK module
+ *
+ * This module allows you to create segments and form them into 
+ * chains. You can then define a goal point that the end of the 
+ * chain should attempt to reach - an inverse kinematic problem.
+ * This module will then modify the segments in the chain in
+ * order to get the end of the chain as near as possible to the 
+ * goal. This solver uses an inverse jacobian method to find
+ * a solution.
+ * 
+ * @section issues Known issues with this IK solver.
+ *
+ * - The current solver works with only one type of segment. These
+ * segments always have 3 degress of freedom (DOF). i.e. the solver
+ * uses all these degrees to solve the IK problem. It would be 
+ * nice to allow the user to specify different segment types such
+ * as 1 DOF joints in a given plane. 2 DOF joints about given axis.
+ * - There is currently no support for joint constraints in the
+ * solver. This is within the realms of possibility - please ask
+ * if this functionality is required.
+ * - The solver is slow, inverse jacobian methods in general give
+ * 'smooth' solutions and the method is also very flexible, it 
+ * does not rely on specific angle parameterization and can be 
+ * extended to deal with different joint types and joint 
+ * constraints. However it is not suitable for real time use. 
+ * Other algorithms exist which are more suitable for real-time
+ * applications, please ask if this functionality is required.     
+ * 
+ * @section dependencies Dependencies
+ * 
+ * This module only depends on Moto.
+ */
+
+#ifndef NAN_INCLUDED_IK_solver_h
+#define NAN_INCLUDED_IK_solver_h
+
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** 
+ * External segment structure
+ */
+
+
+/**
+ * This structure defines a single segment of an IK chain. 
+ * - Individual segments are always defined in local coordinates.
+ * - The segment is assumed to be oriented in  the local 
+ * y-direction.
+ * - seg_start is the start of the segment relative to the end 
+ * of the parent segment.
+ * - basis is a column major matrix defining the rest position 
+ * of the bone.
+ * - length is the simply the length of the bone.  
+ * - basis_change is a 3x3 matrix representing the change 
+ * from the rest position of the segment to the solved position.
+ * In fact it is the transpose of this matrix because blender 
+ * does something weird with quaternion conversion. This is 
+ * strictly an ouput variable for returning the results of an
+ * an ik solve back to you.
+ * The local transformation specified as a column major matrix 
+ * of a segment is then defined as.
+ * translate(seg_start)*basis*transpose(basis_change)*translate(0,length,0) 
+ */
+
+typedef struct IK_Segment_Extern {
+	float seg_start[3];
+	float basis[9];
+	float length;
+	float basis_change[9];
+} IK_Segment_Extern;
+
+typedef IK_Segment_Extern* IK_Segment_ExternPtr; 
+
+/** 
+ * External chain structure.
+ * This structure is filled when you call IK_LoadChain.
+ * The first segment in the chain is the root segment.
+ * The end of the last segment is the end-effector of the chain 
+ * this is the point that tries to move to the goal in the ik 
+ * solver. 
+ * - num_segments is the number of segments in the array pointed
+ * to by the member segments.
+ * - chain_dof is the number of degrees of freedom of the chain
+ * that is the number of independent ways the chain can be changed
+ * to reach the goal. 
+ * - segments points to an array of IK_Segment_Extern structs
+ * containing the segments of this chain. 
+ * - intern is pointer used by the module to store information
+ * about the chain. Please do not touch the member in any way.   
+ */
+
+typedef struct IK_Chain_Extern {
+	int num_segments;
+	int chain_dof;
+	IK_Segment_ExternPtr segments;
+	void * intern;
+} IK_Chain_Extern; 
+
+typedef IK_Chain_Extern* IK_Chain_ExternPtr;
+
+
+/**
+ * Create a clean chain structure. 
+ * @return A IK_Chain_Extern structure allocated on the heap.
+ * Do not attempt to delete or free this memory yourself please 
+ * use the FreeChain(...) function for this.
+ */
+
+extern IK_Chain_ExternPtr IK_CreateChain(void);
+
+/**
+ * Copy segment information into the chain structure.
+ * @param chain A chain to load the segments into.
+ * @param segments a ptr to an array of IK_Input_Segment_Extern structures
+ * @param num_segs the number of segments to load into the chain
+ * @return 1 if the chain was correctly loaded into the structure. 
+ * @return 0 if an error occured loading the chain. This will normally
+ * occur when there is not enough memory to allocate internal chain data.
+ * In this case you should not use the chain structure and should call
+ * IK_FreeChain to free the memory associated with the chain.
+ */
+
+extern int IK_LoadChain(IK_Chain_ExternPtr chain,IK_Segment_ExternPtr segments, int num_segs);
+
+/**
+ * Compute the solution of an inverse kinematic problem.
+ * @param chain a ptr to an IK_Segment_Extern loaded with the segments
+ * to solve for.
+ * @param goal the goal of the IK problem
+ * @param tolerance .The distance to the solution within which the chain is deemed 
+ * to be solved.
+ * @param max_iterations. The maximum number of iterations to use in solving the 
+ * problem. 
+ * @param max_angle_change. The maximum allowed angular change. 0.1 is a good value here.
+ * @param output. Results of the solution are written to the segments pointed to by output.
+ * Only the basis and basis_change fields are written. You must make sure that you have
+ * allocated enough room for the output segments.
+ * @return 0 if the solved chain did not reach the goal. This occurs when the
+ * goal was unreachable by the chain end effector.
+ * @return 1 if the chain reached the goal.
+ */ 
+
+extern int IK_SolveChain(
+	IK_Chain_ExternPtr chain,
+	float goal[3],
+	float tolerance,
+	int max_iterations,
+	float max_angle_change, 
+	IK_Segment_ExternPtr output
+);
+
+/** 
+ * Free a chain and all it's internal memory.
+ */
+
+extern void IK_FreeChain(IK_Chain_ExternPtr);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NAN_INCLUDED_IK_solver_h

intern/iksolver/intern/IK_CGChainSolver.cpp

@@ -0,0 +1,300 @@
+/**
+ * $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 *****
+ */
+
+#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;
+};
+

intern/iksolver/intern/IK_CGChainSolver.h

@@ -0,0 +1,178 @@
+/**
+ * $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 *****
+ */
+
+#ifndef CGChainSolver_h
+
+#define CGChainSolver_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "IK_ConjugateGradientSolver.h"
+#include "IK_Chain.h"
+#include "MT_Scalar.h"
+#include "TNT/vec.h"
+#include "MEM_SmartPtr.h"
+
+
+/**
+ * This class is a concrete differentiable potenial function for
+ * an IK_Chain representing the distance to the goal. 
+ * @warning This method of solving IK problems is not as good
+ * as IK_JacobianSolver. I advise you to use that class instead.
+ */
+
+class ChainPotential : 
+public DifferentiablePotenialFunctionNd 
+{
+public :
+
+	static 
+		ChainPotential *
+	New(
+		IK_Chain &chain
+	);
+	
+	// End effector goal
+
+		void
+	SetGoal(
+		const MT_Vector3 goal
+	);
+
+	// Inherited from DifferentiablePotenialFunctionNd
+	//////////////////////////////////////////////////
+
+		MT_Scalar 
+	Evaluate(
+		const MT_Scalar x
+	);
+
+		MT_Scalar
+	Derivative(
+		const MT_Scalar x
+	);
+
+		MT_Scalar
+	Evaluate(
+		const TNT::Vector<MT_Scalar> &x
+	);
+
+		void
+	Derivative(
+		const TNT::Vector<MT_Scalar> &x,
+		TNT::Vector<MT_Scalar> &dy
+	);
+
+	// return the dimension of the domain of the potenial
+	// function
+
+		int
+	Dimension(
+	) const {
+		return m_dimension;
+	}
+	
+	~ChainPotential(
+	){
+	};
+	
+private :
+
+		MT_Scalar
+	DistancePotential(
+		MT_Vector3 pos,
+		MT_Vector3 goal
+	) const;
+
+		void
+	DistanceGradient(
+		MT_Vector3 pos,
+		MT_Vector3 goal
+	);
+
+	ChainPotential(
+		IK_Chain & chain
+	) :
+		DifferentiablePotenialFunctionNd(),
+		m_chain(chain),
+		m_t_chain(chain),
+		m_dimension (chain.Segments().size())
+	{
+	};
+
+	MT_Vector3 m_goal;
+	TNT::Vector<MT_Scalar> m_distance_grad;
+	TNT::Vector<MT_Scalar> m_angle_grad;
+	TNT::Vector<MT_Scalar> m_temp_pos;
+	TNT::Vector<MT_Scalar> m_temp_grad;
+
+	TNT::Vector<MT_Scalar> m_original_pos;
+	int m_dimension;
+
+	IK_Chain &m_chain;
+	IK_Chain m_t_chain; // deep copy 
+
+};
+		
+
+class IK_CGChainSolver : public MEM_NonCopyable
+{
+public :
+	
+	
+	static
+		IK_CGChainSolver *
+	New(
+	);
+
+		bool
+	Solve(
+		IK_Chain & chain,
+		MT_Vector3 new_position,
+		MT_Scalar tolerance
+	);
+
+	~IK_CGChainSolver();	
+
+private :
+
+	IK_CGChainSolver(
+	);
+
+	MEM_SmartPtr<ChainPotential>  m_potential;
+	MEM_SmartPtr<IK_ConjugateGradientSolver> m_grad_solver;
+};
+
+
+#endif

intern/iksolver/intern/IK_Chain.cpp

@@ -0,0 +1,208 @@
+/**
+ * $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 *****
+ */
+
+#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;
+}
+
+
+
+
+	
+
+
+
+
+
+
+

intern/iksolver/intern/IK_Chain.h

@@ -0,0 +1,194 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_Chain_h
+
+#define NAN_INCLUDED_IK_Chain_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "IK_Segment.h"
+#include <vector>
+#include "MT_Scalar.h"
+#include "TNT/cmat.h"
+
+/**
+ * This class is a collection of ordered segments that are used
+ * in an Inverse Kinematic solving routine. An IK solver operating
+ * on the chain, will in general manipulate all the segments of the
+ * chain in order to solve the IK problem.
+ * 
+ * To build a chain use the default constructor. Once built it's
+ * then possible to add IK_Segments to the chain by inserting
+ * them into the vector of IK_Segments. Note that segments will be
+ * copied into the chain so chain's cannot share instances of 
+ * IK_Segments. 
+ * 
+ * You have full control of which segments form the chain via the 
+ * the std::vector routines. 
+ */
+class IK_Chain{
+
+public :
+
+	/**
+	 * Construct a IK_Chain with no segments.
+	 */
+
+	IK_Chain(
+	);
+
+	// IK_Chains also have the default copy constructors
+	// available. 
+
+	/** 
+     * Const access to the array of segments
+	 * comprising the IK_Chain. Used for rendering
+	 * etc
+     * @return a vector of segments
+	 */
+
+	const 
+		std::vector<IK_Segment> &
+	Segments(
+	) const ;
+
+
+	/**
+	 * Full access to segments used to initialize
+	 * the IK_Chain and manipulate the segments.
+	 * Use the push_back() method of std::vector to add
+     * segments in order to the chain
+	 */
+
+		std::vector<IK_Segment> &
+	Segments(
+	);
+
+
+	/** 
+	 * Force the IK_Chain 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 reference to the last computed jacobian matrix
+	 */
+
+	const 
+		TNT::Matrix<MT_Scalar> &
+	Jacobian(
+	) const ;
+
+	/** 
+	 * @return A reference to the last computed transposed jacobian matrix
+	 */
+
+	const 
+		TNT::Matrix<MT_Scalar> &
+	TransposedJacobian(
+	) const ;
+
+	/** 
+	 * Count the degrees of freedom in the IK_Chain
+	 * @warning store this value rather than using this function
+	 * as the termination value of a for loop etc.
+	 */
+
+		int
+	DoF(
+	) const;
+
+
+private :
+
+	/// The vector of segments comprising the chain
+	std::vector<IK_Segment> m_segments;
+
+	/// The jacobain of the IK_Chain
+	TNT::Matrix<MT_Scalar> m_jacobian;
+
+	/// It's transpose
+	TNT::Matrix<MT_Scalar> m_t_jacobian;
+
+	MT_Vector3 m_end_effector;		
+	MT_Vector3 m_end_pose;
+
+};
+
+
+#endif

intern/iksolver/intern/IK_ConjugateGradientSolver.cpp

@@ -0,0 +1,162 @@
+/**
+ * $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 *****
+ */
+
+#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		
+
+
+
+
+
+
+
+

intern/iksolver/intern/IK_ConjugateGradientSolver.h

@@ -0,0 +1,195 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_ConjugateGradientSolver_h
+
+#define NAN_INCLUDED_IK_ConjugateGradientSolver_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "TNT/cmat.h"
+#include "MT_Scalar.h"
+#include "IK_LineMinimizer.h"
+
+/** 
+ * These classes locally minimize n dimensional potenial functions.
+ * See Numerical Recipes in C www.nr.com for more details.
+ * If a n dimensionable potenial function is 
+ * differentiable, then it is diferentiable along
+ * any vector x. This can be found by the dot product
+ * of the gradient operator with x. 
+ * The conjugate gradient solver boils down to 
+ * a collection of line minimizations along various lines
+ * defined by position,direction pairs. There are 
+ * methods in this class to set the lines along which
+ * minimizations via the DiffentiablePotenialFunction1d interface
+ * are to be performed.
+ *	
+ * @warning I don't like data inheritance but it is the most efficient
+ * wasy to do this here.
+ */
+
+
+class DifferentiablePotenialFunctionNd 
+: public DifferentiablePotenialFunction1d 
+{
+public :
+
+	/** 
+	 * Inherited from DiffentiablePotenialFunction1d
+	 *
+	 * virtual
+	 *	MT_Scalar 
+	 * Evaluate1d(
+	 * 	MT_Scalar x
+	 * ) = 0;
+     *
+	 * virtual
+	 * 	MT_Scalar
+	 * Derivative1d(
+	 * 	MT_Scalar x
+	 * ) = 0;
+	 */
+
+	/// Methods to set the current line in N dimensions
+
+		void
+	SetLineVector(
+		const TNT::Vector<MT_Scalar> &pos,
+		const TNT::Vector<MT_Scalar> &dir
+	){
+		m_line_pos = pos;
+		m_line_dir = dir;
+	};
+
+	virtual
+		MT_Scalar
+	Evaluate(
+		const TNT::Vector<MT_Scalar> &x
+	) = 0;
+
+	virtual
+		void
+	Derivative(
+		const TNT::Vector<MT_Scalar> &x,
+		TNT::Vector<MT_Scalar> &dy
+	) = 0;
+
+	/// @return The dimension of the domain of the potenial function
+
+	virtual
+		int
+	Dimension(
+	) const =0;
+	
+	virtual
+	~DifferentiablePotenialFunctionNd(
+	){
+	};
+
+protected :
+
+	DifferentiablePotenialFunctionNd(){};
+
+	TNT::Vector<MT_Scalar> m_line_pos;
+	TNT::Vector<MT_Scalar> m_line_dir;
+
+};
+
+
+class IK_ConjugateGradientSolver 
+: public MEM_NonCopyable
+{
+public :
+
+	/** 
+	 * This class necessarily needs some (potenially large) 
+	 * temporary vectors to aid computation. We therefore
+	 * insist creation of these objects on the heap.
+	 */
+
+	static
+		IK_ConjugateGradientSolver *
+	New(
+	);
+
+	/** 
+	 * 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
+	Solve(
+		TNT::Vector<MT_Scalar> &p,
+		MT_Scalar ftol,
+		int &iter,
+		MT_Scalar &fret,
+		DifferentiablePotenialFunctionNd &potenial,
+		int max_its = 200
+	);
+
+	~IK_ConjugateGradientSolver(
+	);
+
+private :
+		void
+	LineMinimize(
+		TNT::Vector<MT_Scalar> & p,
+		const TNT::Vector<MT_Scalar> & xi,
+		MT_Scalar &fret,
+		DifferentiablePotenialFunctionNd &potenial
+	);
+
+	IK_ConjugateGradientSolver(
+	);
+
+		void
+	ArmVectors(
+		int dimension
+	);
+
+
+	TNT::Vector<MT_Scalar> m_g;
+	TNT::Vector<MT_Scalar> m_h;
+	TNT::Vector<MT_Scalar> m_xi;
+
+	TNT::Vector<MT_Scalar> m_xi_temp;
+
+};
+
+#endif
+
+

intern/iksolver/intern/IK_JacobianSolver.cpp

@@ -0,0 +1,267 @@
+/**
+ * $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 *****
+ */
+
+#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(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);
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+	

intern/iksolver/intern/IK_JacobianSolver.h

@@ -0,0 +1,171 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_JacobianSolver_h
+
+#define NAN_INCLUDED_IK_JacobianSolver_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "TNT/cmat.h"
+#include <vector>
+#include "MT_Vector3.h"
+#include "IK_Chain.h"
+
+class IK_JacobianSolver {
+
+public :
+
+	/**
+	 * Create a new IK_JacobianSolver on the heap
+	 * @return A newly created IK_JacobianSolver you take ownership of the object
+     * and responsibility for deleting it
+	 */
+
+
+	static 
+		IK_JacobianSolver *
+	New(
+	);
+
+	/**
+	 * 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 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_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
+	);
+
+	~IK_JacobianSolver(
+	);
+
+
+private :
+
+	/**
+	 * Private constructor to force use of New()
+	 */
+
+	IK_JacobianSolver(
+	);
+
+
+	/** 
+	 * Compute the inverse jacobian matrix of the chain.
+	 * Uses a damped least squares solution when the matrix is 
+	 * is approaching singularity
+     */
+
+		int
+	ComputeInverseJacobian(
+		IK_Chain &chain,
+		const MT_Scalar x_length,
+		const MT_Scalar max_angle_change
+	);
+
+		void
+	ComputeBetas(
+		IK_Chain &chain,
+		const MT_Vector3 d_pos
+	);
+
+	/** 
+	 * Updates the angles of the chain with the newly
+	 * computed values
+     **/
+
+		void
+	UpdateChain(
+		IK_Chain &chain
+	);
+	
+	/**
+	 * Make sure all the matrices are of the correct size
+     **/
+
+		void
+	ArmMatrices(
+		int dof
+	);
+
+
+private :
+
+	/// the vector of intermediate betas
+	TNT::Vector<MT_Scalar> m_beta;
+
+	/// the vector of computed angle changes
+	TNT::Vector<MT_Scalar> m_d_theta;
+
+	/// the contraint gradients for each angle.
+	TNT::Vector<MT_Scalar> m_dh;
+
+	/// Space required for SVD computation
+
+	TNT::Vector<MT_Scalar> m_svd_w;
+	TNT::Matrix<MT_Scalar> m_svd_v;
+	TNT::Matrix<MT_Scalar> m_svd_u;
+
+	TNT::Matrix<MT_Scalar> m_svd_w_diag;
+	TNT::Matrix<MT_Scalar> m_svd_v_t;
+	TNT::Matrix<MT_Scalar> m_svd_u_t;
+	TNT::Matrix<MT_Scalar> m_svd_inverse;
+	TNT::Matrix<MT_Scalar> m_svd_temp1;
+
+
+};
+
+
+
+
+#endif
+
+
+

intern/iksolver/intern/IK_LineMinimizer.h

@@ -0,0 +1,298 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_LineMinimizer_h
+
+#define NAN_INCLUDED_IK_LineMinimizer_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "MT_Scalar.h"
+#include <vector>
+#include "TNT/tntmath.h"
+#include "MEM_NonCopyable.h"
+
+
+
+#define GOLD 1.618034
+#define GLIMIT 100.0
+#define TINY 1.0e-20
+#define ZEPS 1.0e-10
+
+/** 
+ * Routines for line - minization in n dimensions
+ * these should be templated on the potenial function
+ * p instead of using a virtual class. Please see 
+ * numerical recipes in c for more details. www.nr.com
+ */
+
+class DifferentiablePotenialFunction1d {
+public :
+	virtual
+		MT_Scalar 
+	Evaluate(
+		const MT_Scalar x
+	) = 0;
+
+	virtual
+		MT_Scalar
+	Derivative(
+		const MT_Scalar x
+	) = 0;
+};
+
+
+/**
+ * TODO get rid of this class and make some
+ * template functions in a seperate namespace
+ */
+
+class IK_LineMinimizer : public MEM_NonCopyable {
+
+public :
+
+	/**
+	 * Before we proceed with line minimization 
+	 * we need to construct an initial bracket
+	 * of a minima of the PotenialFunction
+	 */
+
+	static
+		void
+	InitialBracket1d (	
+		MT_Scalar &ax,
+		MT_Scalar &bx,
+		MT_Scalar &cx,
+		MT_Scalar &fa,
+		MT_Scalar &fb,
+		MT_Scalar &fc,
+		DifferentiablePotenialFunction1d &potenial
+	) {
+		MT_Scalar ulim,u,r,q,fu;
+
+		fa = potenial.Evaluate(ax);
+		fb = potenial.Evaluate(bx);
+
+		if (fb > fa) {
+			std::swap(ax,bx);
+			std::swap(fa,fb);
+		}
+		cx = bx + GOLD*(bx-ax);
+		fc = potenial.Evaluate(cx);
+		
+		while (fb > fc) {
+
+			r = (bx - ax) * (fb - fc);
+			q = (bx - cx) * (fb - fa);
+			u = bx - ((bx - cx)*q - (bx - ax) *r)/ 
+				(2 * TNT::sign(TNT::max(TNT::abs(q-r),TINY),q-r));
+			ulim = bx + GLIMIT*(cx-bx);
+
+			if ((bx-u)*(u-cx) > 0.0) {
+				fu = potenial.Evaluate(u);
+				if (fu < fc) {
+					ax = bx;
+					bx = u;
+					fa = fb;
+					fb = fu;
+					return;
+				} else if (fu > fb) {
+					cx = u;
+					fc = fu;
+					return;
+				}
+				u = cx + GOLD*(cx-bx);
+				fu = potenial.Evaluate(u);
+
+			} else if ((cx - u)*(u - ulim) > 0.0) {
+				fu = potenial.Evaluate(u);
+ 
+				if (fu < fc) {
+					bx = cx;
+					cx = u;
+					u = cx + GOLD*(cx - bx);
+					fb = fc;
+					fc = fu;
+					fu = potenial.Evaluate(u);
+				}
+			} else if ((u-ulim)*(ulim-cx) >=0.0) {
+				u = ulim;
+				fu = potenial.Evaluate(u);
+			} else {
+				u = cx + GOLD*(cx-bx);
+				fu = potenial.Evaluate(u);
+			}
+			ax = bx;
+			bx = cx;
+			cx = u;
+			fa = fb;
+			fb = fc;
+			fc = fu;
+		}
+	};
+
+	/**
+	 * This is a 1 dimensional brent method for
+	 * line-minization with derivatives
+	 */
+
+
+	static
+		MT_Scalar
+	DerivativeBrent1d(
+		MT_Scalar ax,
+		MT_Scalar bx,
+		MT_Scalar cx,
+		DifferentiablePotenialFunction1d &potenial,
+		MT_Scalar &x_min,
+		const MT_Scalar tol,
+		int max_iter = 100
+	) {
+		int iter;
+		bool ok1,ok2;
+		MT_Scalar a,b,d,d1,d2,du,dv,dw,dx,e(0);
+		MT_Scalar fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;
+
+		a = (ax < cx ? ax : cx);
+		b = (ax > cx ? ax : cx);
+		x = w = v = bx;
+		fw = fv = fx = potenial.Evaluate(x);
+		dw = dv = dx = potenial.Derivative(x);
+
+		for (iter = 1; iter <= max_iter; iter++) {
+			xm = 0.5*(a+b);
+			tol1 = tol*fabs(x) + ZEPS;
+			tol2 = 2 * tol1;
+			if (fabs(x - xm) <= (tol2 - 0.5*(b-a))) {
+				x_min = x;
+				return fx;
+			}
+
+			if (fabs(e) > tol1) {
+				d1 = 2*(b-a);
+				d2 = d1;
+				if (dw != dx) {
+					d1 = (w-x)*dx/(dx-dw);
+				}
+				if (dv != dx) {
+					d2 = (v-x)*dx/(dx-dv);
+				}
+					
+				u1 = x+d1;
+				u2 = x+d2;
+				ok1 = ((a-u1)*(u1-b) > 0.0) && ((dx*d1) <= 0.0);
+				ok2 = ((a-u2)*(u2-b) > 0.0) && ((dx*d2) <= 0.0);
+				olde = e;
+				e = d;
+
+				if (ok1 || ok2) {
+					if (ok1 && ok2) {
+						d = fabs(d1) < fabs(d2) ? d1 : d2;
+					} else if (ok1)  {
+						d = d1;
+					} else {
+						d = d2;
+					}
+					if (fabs(d) <= fabs(0.5*olde)) {			
+						u = x+d;
+						if ((u-a < tol2) || (b-u < tol2)) {
+							d = TNT::sign(tol1,xm-x);
+						}
+					} else {
+						d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
+					}
+				} else {
+					d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
+				}
+			} else {
+				d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
+			}
+
+			if (fabs(d) >= tol1) {
+				u = x+d;
+				fu = potenial.Evaluate(u);
+			} else {
+				u  = x + TNT::sign(tol1,d);
+				fu = potenial.Evaluate(u);
+				if (fu > fx) {
+					x_min = x;
+					return fx;
+				}
+			}
+			du = potenial.Derivative(u);
+			if (fu <= fx) {
+				if (u >= x) {
+					a = x;
+				} else {
+					b = x;
+				}
+				v = w; fv = fw; dv = dw;
+				w = x; fw = fx; dw = dx;
+				x = u; fx = fu; dx = du;
+			} else {
+				if (u < x) {
+					a = u;
+				} else {
+					b = u;
+				}
+				if (fu <= fw || w == x) {
+					v = w; fv = fw; dv = dw;
+					w = u; fw = fu; dw = du;
+				} else if ( fu < fv || v == x || v == w) {
+					v = u; fv = fu; dv = du;
+				}
+			}
+		}
+		// FIXME throw exception		
+	
+		assert(false);
+		return MT_Scalar(0);
+	};
+
+private :
+
+	/// This class just contains static helper methods so no instantiation
+
+	IK_LineMinimizer();		
+
+};
+
+#undef GOLD
+#undef GLIMIT
+#undef TINY
+#undef ZEPS
+
+
+				
+#endif

intern/iksolver/intern/IK_QChain.cpp

@@ -0,0 +1,280 @@
+/**
+ * $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
+ */
+
+#include "IK_QChain.h"
+
+using namespace std;
+
+IK_QChain::
+IK_QChain(
+)
+{
+	// nothing to do;
+};
+
+const 
+	vector<IK_QSegment> &
+IK_QChain::
+Segments(
+) const {
+	return m_segments;
+};
+
+	vector<IK_QSegment> &
+IK_QChain::
+Segments(
+){
+	return m_segments;
+};	
+
+	void
+IK_QChain::
+UpdateGlobalTransformations(
+){
+
+	// now iterate through the segment list
+	// compute their local transformations if needed
+		
+	// assign their global transformations
+	// (relative to chain origin)
+
+	vector<IK_QSegment>::const_iterator s_end = m_segments.end();
+	vector<IK_QSegment>::iterator s_it = m_segments.begin();
+
+	MT_Transform global;
+	global.setIdentity();
+
+	for (; s_it != s_end; ++s_it) {
+		global = s_it->UpdateGlobal(global);
+	}
+
+	// we also need to compute the accumulated local transforms
+	// for each segment
+
+	MT_Transform acc_local;
+	acc_local.setIdentity();
+
+	vector<IK_QSegment>::reverse_iterator s_rit = m_segments.rbegin();
+	vector<IK_QSegment>::reverse_iterator s_rend = m_segments.rend();
+
+	for (; s_rit != s_rend; ++s_rit) {
+		acc_local = s_rit->UpdateAccumulatedLocal(acc_local);
+	}		
+
+	// compute the position of the end effector and it's pose
+
+	const MT_Transform &last_t = m_segments.back().GlobalTransform();
+	m_end_effector = last_t.getOrigin();
+
+#if 0
+	
+	// The end pose is not currently used. 
+
+	MT_Matrix3x3 last_basis = last_t.getBasis();
+	last_basis.transpose();
+	MT_Vector3 m_end_pose = last_basis[1];
+	
+#endif
+	
+};
+	
+const 
+	TNT::Matrix<MT_Scalar> &
+IK_QChain::
+Jacobian(
+) const  {
+	return m_jacobian;
+} ;
+ 
+
+const 
+	TNT::Matrix<MT_Scalar> &
+IK_QChain::
+TransposedJacobian(
+) const {
+	return m_t_jacobian;
+};
+
+	void
+IK_QChain::
+ComputeJacobian(
+){
+	// let's assume that the chain's global transfomations
+	// have already been computed.
+	
+	int dof = DoF();
+
+	int num_segs = m_segments.size();
+	vector<IK_QSegment>::const_iterator segs = m_segments.begin();
+
+	m_t_jacobian.newsize(dof,3);
+	m_jacobian.newsize(3,dof);
+
+	// compute the transposed jacobian first
+
+	int n;
+	int i = 0;
+
+	for (n= 0; n < num_segs; n++) {
+#if 0
+
+		// For euler angle computation we can use a slightly quicker method.
+
+		const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis();
+		const MT_Vector3 &origin  = segs[n].GlobalSegmentStart();
+				
+		const MT_Vector3 p = origin-m_end_effector;
+
+		const MT_Vector3 x_axis(1,0,0);
+		const MT_Vector3 y_axis(0,1,0);
+		const MT_Vector3 z_axis(0,0,1);
+		
+		MT_Vector3 a = basis * x_axis;
+		MT_Vector3 pca = p.cross(a);
+
+		m_t_jacobian(n*3 + 1,1) = pca.x();
+		m_t_jacobian(n*3 + 1,2) = pca.y();
+		m_t_jacobian(n*3 + 1,3) = pca.z();
+
+		a = basis * y_axis;
+		pca = p.cross(a);
+
+		m_t_jacobian(n*3 + 2,1) = pca.x();
+		m_t_jacobian(n*3 + 2,2) = pca.y();
+		m_t_jacobian(n*3 + 2,3) = pca.z();
+
+		a = basis * z_axis;
+		pca = p.cross(a);
+
+		m_t_jacobian(n*3 + 3,1) = pca.x();
+		m_t_jacobian(n*3 + 3,2) = pca.y();
+		m_t_jacobian(n*3 + 3,3) = pca.z();
+#else
+		// user slower general jacobian computation method
+
+		MT_Vector3 j1 = segs[n].ComputeJacobianColumn(0);
+
+		m_t_jacobian(n*3 + 1,1) = j1.x();
+		m_t_jacobian(n*3 + 1,2) = j1.y();
+		m_t_jacobian(n*3 + 1,3) = j1.z();
+
+		j1 = segs[n].ComputeJacobianColumn(1);
+
+		m_t_jacobian(n*3 + 2,1) = j1.x();
+		m_t_jacobian(n*3 + 2,2) = j1.y();
+		m_t_jacobian(n*3 + 2,3) = j1.z();
+
+		j1 = segs[n].ComputeJacobianColumn(2);
+
+		m_t_jacobian(n*3 + 3,1) = j1.x();
+		m_t_jacobian(n*3 + 3,2) = j1.y();
+		m_t_jacobian(n*3 + 3,3) = j1.z();
+#endif
+
+
+
+	}
+
+
+	// get the origina1 jacobain
+
+	TNT::transpose(m_t_jacobian,m_jacobian);
+};
+
+	MT_Vector3 
+IK_QChain::
+EndEffector(
+) const {
+	return m_end_effector;
+};
+
+	MT_Vector3 
+IK_QChain::
+EndPose(
+) const {
+	return m_end_pose;
+};
+		
+
+	int
+IK_QChain::
+DoF(
+) const {
+	return 3 * m_segments.size();
+}
+
+const 
+	MT_Scalar
+IK_QChain::
+MaxExtension(
+) const {
+
+	vector<IK_QSegment>::const_iterator s_end = m_segments.end();
+	vector<IK_QSegment>::const_iterator s_it = m_segments.begin();
+
+	if (m_segments.size() == 0) return MT_Scalar(0);
+
+	MT_Scalar output = s_it->Length();
+	
+	++s_it	;
+	for (; s_it != s_end; ++s_it) {
+		output += s_it->MaxExtension();
+	}
+	return output;
+}
+
+	
+
+
+
+
+
+
+
+
+
+
+	
+
+
+
+
+
+
+

intern/iksolver/intern/IK_QChain.h

@@ -0,0 +1,211 @@
+/**
+ * $Id$
+ * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. The Blender
+ * Foundation also sells licenses for use in proprietary software under
+ * the Blender License.  See http://www.blender.org/BL/ for information
+ * about this.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): none yet.
+ *
+ * ***** END GPL/BL DUAL LICENSE BLOCK *****
+ */
+
+/**
+
+ * $Id$
+ * Copyright (C) 2001 NaN Technologies B.V.
+ *
+ * @author Laurence
+ */
+
+#ifndef NAN_INCLUDED_IK_QChain_h
+#define NAN_INCLUDED_IK_QChain_h
+
+
+#include "IK_QSegment.h"
+#include <vector>
+#include "MT_Scalar.h"
+#include "TNT/cmat.h"
+
+/**
+ * This class is a collection of ordered segments that are used
+ * in an Inverse Kinematic solving routine. An IK solver operating
+ * on the chain, will in general manipulate all the segments of the
+ * chain in order to solve the IK problem.
+ * 
+ * To build a chain use the default constructor. Once built it's
+ * then possible to add IK_Segments to the chain by inserting
+ * them into the vector of IK_Segments. Note that segments will be
+ * copied into the chain so chains cannot share instances of 
+ * IK_Segments. 
+ * 
+ * You have full control of which segments form the chain via the 
+ * the std::vector routines. 
+ */
+
+class IK_QChain{
+
+public :
+
+	/**
+	 * Construct a IK_QChain with no segments.
+	 */
+
+	IK_QChain(
+	);
+
+	// IK_QChains also have the default copy constructors
+	// available. 
+
+	/** 
+     * Const access to the array of segments
+	 * comprising the IK_QChain. Used for rendering
+	 * etc
+     * @return a const reference to a vector of segments
+	 */
+
+	const 
+		std::vector<IK_QSegment> &
+	Segments(
+	) const ;
+
+
+	/**
+	 * Full access to segments used to initialize
+	 * the IK_QChain and manipulate the segments.
+	 * Use the push_back() method of std::vector to add
+     * segments in order to the chain
+     * @return a reference to a vector of segments
+	 */
+
+		std::vector<IK_QSegment> &
+	Segments(
+	);
+
+
+	/** 
+	 * Force the IK_QChain to recompute all the local 
+	 * segment transformations and composite them
+	 * to calculate the global transformation for 
+	 * each segment. Must be called before 
+	 * ComputeJacobian()
+     */
+
+		void
+	UpdateGlobalTransformations(
+	);
+
+	/** 
+	 * Return the global position of the end
+	 * of the last segment.
+	 */
+
+		MT_Vector3 
+	EndEffector(
+	) const;
+
+
+	/** 
+	 * Return the global pose of the end
+	 * of the last segment.
+	 */
+
+		MT_Vector3
+	EndPose(
+	) const;
+
+
+	/** 
+	 * Calculate the jacobian matrix for
+	 * the current end effector position.
+	 * A jacobian is the set of column vectors
+	 * of partial derivatives for each active angle.
+	 * This method also computes the transposed jacobian.
+	 * @pre You must have updated the global transformations
+	 * of the chain's segments before a call to this method. Do this
+	 * with UpdateGlobalTransformation()
+	 */
+
+		void
+	ComputeJacobian(
+	);
+	
+
+	/** 
+	 * @return A const reference to the last computed jacobian matrix
+	 */
+
+	const 
+		TNT::Matrix<MT_Scalar> &
+	Jacobian(
+	) const ;
+
+	/** 
+	 * @return A const reference to the last computed transposed jacobian matrix
+	 */
+
+	const 
+		TNT::Matrix<MT_Scalar> &
+	TransposedJacobian(
+	) const ;
+
+	/** 
+	 * Count the degrees of freedom in the IK_QChain
+	 * @warning store this value rather than using this function
+	 * as the termination value of a for loop etc.
+	 */
+
+		int
+	DoF(
+	) const;
+
+	/** 
+	 * Compute the maximum extension of the chain from the
+	 * root segment. This is the length of the root segments
+	 * + the max extensions of all the other segments
+	 */
+	
+	const 
+		MT_Scalar
+	MaxExtension(
+	) const;
+
+
+private :
+
+	/// The vector of segments comprising the chain
+	std::vector<IK_QSegment> m_segments;
+
+	/// The jacobain of the IK_QChain
+	TNT::Matrix<MT_Scalar> m_jacobian;
+
+	/// It's transpose
+	TNT::Matrix<MT_Scalar> m_t_jacobian;
+
+	MT_Vector3 m_end_effector;		
+	MT_Vector3 m_end_pose;
+
+
+};
+
+
+#endif

intern/iksolver/intern/IK_QJacobianSolver.cpp

@@ -0,0 +1,327 @@
+/**
+ * $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 *****
+ */
+
+#include "IK_QJacobianSolver.h"
+
+#include "TNT/svd.h"
+
+using namespace std;
+
+	IK_QJacobianSolver *
+IK_QJacobianSolver::
+New(
+){
+	return new IK_QJacobianSolver();
+}
+
+	bool
+IK_QJacobianSolver::
+Solve(
+	IK_QChain &chain,
+	const MT_Vector3 &g_position,
+	const MT_Vector3 &g_pose,
+	const MT_Scalar tolerance,
+	const int max_iterations,
+	const MT_Scalar max_angle_change
+){
+
+	const vector<IK_QSegment> & segs = chain.Segments();
+	if (segs.size() == 0) return false;
+	
+	// compute the goal direction from the base of the chain
+	// in global coordinates
+
+	MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart();
+	
+
+	const MT_Scalar chain_max_extension = chain.MaxExtension();
+
+	bool do_parallel_check(false);
+
+	if (chain_max_extension < goal_dir.length()) {
+		do_parallel_check = true;
+	}
+
+	goal_dir.normalize();
+
+
+	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;
+		}
+		
+		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];
+		}
+	}
+
+	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 <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 = 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<IK_QSegment> &segs = chain.Segments(); 
+	
+	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<IK_QSegment> &segs = chain.Segments(); 
+
+	int num_segs = segs.size();
+	
+	if (num_segs == 0) {
+		return false;
+	}
+
+	MT_Scalar crossp_sum = 0;
+
+	int i;
+	for (i = 0; i < num_segs; i++) {
+		MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() - 
+			segs[i].GlobalSegmentStart();
+
+		global_seg_direction.normalize();	
+
+		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;
+	}
+}
+
+

intern/iksolver/intern/IK_QJacobianSolver.h

@@ -0,0 +1,184 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_QJacobianSolver_h
+
+#define NAN_INCLUDED_IK_QJacobianSolver_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include "TNT/cmat.h"
+#include <vector>
+#include "MT_Vector3.h"
+#include "IK_QChain.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(
+	);
+
+	/**
+	 * 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 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,
+		const MT_Scalar tolerance,
+		const int max_iterations,
+		const MT_Scalar max_angle_change
+	);
+
+	~IK_QJacobianSolver(
+	);
+
+
+private :
+
+	/**
+	 * Private constructor to force use of New()
+	 */
+
+	IK_QJacobianSolver(
+	);
+
+
+	/** 
+	 * Compute the inverse jacobian matrix of the chain.
+	 * Uses a damped least squares solution when the matrix is 
+	 * is approaching singularity
+     */
+
+		int
+	ComputeInverseJacobian(
+		IK_QChain &chain,
+		const MT_Scalar x_length,
+		const MT_Scalar max_angle_change
+	);
+
+		void
+	ComputeBetas(
+		IK_QChain &chain,
+		const MT_Vector3 d_pos
+	);
+
+	/** 
+	 * Updates the angles of the chain with the newly
+	 * computed values
+     */
+
+		void
+	UpdateChain(
+		IK_QChain &chain
+	);
+	
+	/**
+	 * Make sure all the matrices are of the correct size
+     */
+
+		void
+	ArmMatrices(
+		int dof
+	);
+
+	/**
+	 * Quick check to see if all the segments are parallel
+	 * to the goal direction.
+	 */
+
+		bool
+	ParallelCheck(
+		const IK_QChain &chain,
+		const MT_Vector3 goal
+	) const;
+	
+
+
+private :
+
+	/// the vector of intermediate betas
+	TNT::Vector<MT_Scalar> m_beta;
+
+	/// the vector of computed angle changes
+	TNT::Vector<MT_Scalar> m_d_theta;
+
+	/// the constraint gradients for each angle.
+	TNT::Vector<MT_Scalar> m_dh;
+
+	/// Space required for SVD computation
+
+	TNT::Vector<MT_Scalar> m_svd_w;
+    TNT::Vector<MT_Scalar> m_svd_work_space;        
+	TNT::Matrix<MT_Scalar> m_svd_v;
+	TNT::Matrix<MT_Scalar> m_svd_u;
+
+	TNT::Matrix<MT_Scalar> m_svd_w_diag;
+	TNT::Matrix<MT_Scalar> m_svd_v_t;
+	TNT::Matrix<MT_Scalar> m_svd_u_t;
+	TNT::Matrix<MT_Scalar> m_svd_inverse;
+	TNT::Matrix<MT_Scalar> m_svd_temp1;
+
+
+};
+
+
+
+
+#endif
+
+
+

intern/iksolver/intern/IK_QSegment.cpp

@@ -0,0 +1,363 @@
+/**
+ * $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
+ */
+
+#include "IK_QSegment.h"
+#include <iostream.h>
+
+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)	
+{	
+
+	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)
+{
+	// Intentionally empty
+}
+
+
+
+// accessors
+////////////
+
+// The length of the segment 
+
+const
+	MT_Scalar
+IK_QSegment::
+Length(
+) const {
+	return m_length;
+}
+
+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;
+}
+
+const
+	MT_ExpMap &
+IK_QSegment::
+LocalJointParameter(
+) const {
+	return m_q;
+}
+
+	MT_Transform  
+IK_QSegment::
+UpdateGlobal(
+	const MT_Transform & global
+){
+	// compute the global transform
+	// and the start of the segment in global coordinates.
+	
+	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;
+}
+
+	MT_Transform
+IK_QSegment::
+GlobalTransform(
+) const {
+	return m_global_transform * m_local_transform;
+}
+	
+
+	MT_Transform
+IK_QSegment::
+UpdateAccumulatedLocal(
+	const MT_Transform & acc_local
+){
+	m_accum_local = acc_local;
+	return m_local_transform * m_accum_local;
+}		
+
+const 
+	MT_Transform &
+IK_QSegment::
+AccumulatedLocal(
+) const {
+	return m_accum_local;
+}
+
+	MT_Vector3 	
+IK_QSegment::
+ComputeJacobianColumn(
+	int angle
+) const {
+
+
+	MT_Transform translation;
+	translation.setIdentity();
+	translation.translate(MT_Vector3(0,m_length,0));
+
+
+	// 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)
+
+#if 0
+
+	// use euler angles as a test of the matrices and this method.
+
+	MT_Matrix3x3 dRda;
+
+	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);
+ 
+	if (angle == 0) {
+	
+		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 (angle == 1) {
+	
+		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 (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;
+	}
+		
+	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;
+
+#else
+
+	// use exponential map derivatives
+	MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle);
+
+#endif	
+
+	
+	// Once we have computed the local derivatives we can compute 
+	// derivatives of the end effector. 
+
+	// 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_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);
+
+		
+	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();	
+
+	
+#endif
+
+	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;
+};
+
+
+
+
+
+

intern/iksolver/intern/IK_QSegment.h

@@ -0,0 +1,290 @@
+/**
+ * $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_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 <vector>
+
+/**
+ * An IK_Qsegment encodes information about a segments
+ * local coordinate system.
+ * In these segments exponential maps are used to parameterize
+ * the 3 DOF joints. Please see the file MT_ExpMap.h for more
+ * information on this parameterization.
+ * 
+ * These segments always point along the y-axis.
+ * 
+ * Here wee define the local coordinates of a joint as
+ * local_transform = 
+ * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
+ * We use the standard moto column ordered matrices. You can read
+ * this as:
+ * - first translate by (0,length,0)
+ * - multiply by the rotation matrix derived from the current
+ * angle parameterization q.
+ * - multiply by the user defined matrix representing the rest
+ * position of the bone.
+ * - translate by the used defined translation (tr1)
+ * The ordering of these transformations is vital, you must
+ * use exactly the same transformations when displaying the segments
+ */
+
+class IK_QSegment {
+
+public :
+
+	/**
+	 * Constructor.
+	 * @param tr1 a user defined translation 
+	 * @param a used defined rotation matrix representin
+	 * the rest position of the bone.
+	 * @param the length of the bone.
+	 * @param an exponential map can also be used to 
+	 * define the bone rest position.
+	 */
+
+	IK_QSegment(
+		const MT_Point3 tr1,
+		const MT_Matrix3x3 A,
+		const MT_Scalar length,
+		const MT_ExpMap q
+	);
+
+	/** 
+	 * Default constructor
+	 * Defines identity local coordinate system,
+	 * with a bone length of 1.
+	 */
+	
+
+	IK_QSegment(
+	);
+
+
+	/**
+     * @return The length of the segment
+     */
+
+	const
+		MT_Scalar
+	Length(
+	) const ;
+
+	/**
+	 * @return the max distance of the end of this
+	 * bone from the local origin.
+	 */
+
+	const 
+		MT_Scalar
+	MaxExtension(
+	) const ;
+
+	/**
+     * @return The transform from adjacent
+	 * coordinate systems in the chain.
+     */
+
+	const 
+		MT_Transform &
+	LocalTransform(
+	) const ;
+
+	const
+		MT_ExpMap &
+	LocalJointParameter(
+	) const;
+
+	/**
+	 * Update the global coordinates of this segment.
+	 * @param global the global coordinates of the
+	 * previous bone in the chain
+	 * @return the global coordinates of this segment.
+	 */
+
+		MT_Transform
+	UpdateGlobal(
+		const MT_Transform & global
+	);
+
+	/**
+	 * @return The global transformation 
+	 */
+
+		MT_Transform
+	GlobalTransform(
+	) const;
+ 
+	
+	/** 
+	 * Update the accumulated local transform of this segment
+	 * The accumulated local transform is the end effector
+	 * transform in the local coordinates of this segment.
+	 * @param acc_local the accumulated local transform of 
+	 * the child of this bone.
+	 * @return the accumulated local transorm of this segment
+	 */
+	
+		MT_Transform
+	UpdateAccumulatedLocal(
+		const MT_Transform & acc_local
+	);
+
+	/**
+	 * @return A const reference to accumulated local 
+	 * transform of this segment.
+	 */
+
+	const 
+		MT_Transform &
+	AccumulatedLocal(
+	) const;
+		
+	/**
+	 * @return A const Reference to start of segment in global 
+     * coordinates
+	 */
+	
+	const 	
+		MT_Vector3 &
+	GlobalSegmentStart(
+	) const;
+
+	/**
+	 * @return A const Reference to end of segment in global 
+     * coordinates
+	 */
+
+	const 
+		MT_Vector3 &
+	GlobalSegmentEnd(
+	) const;
+
+
+	/**
+	 * @return the partial derivative of the end effector 
+	 * with respect to one of the degrees of freedom of this 
+	 * segment.
+	 * @param angle the angle parameter you want to compute
+	 * the partial derivatives for. For these segments this
+	 * must be in the range [0,2]
+	 */
+
+		MT_Vector3 	
+	ComputeJacobianColumn(
+		int angle
+	) const ;
+		
+	/**
+	 * Explicitly set the angle parameterization value. 
+	 */
+
+		void
+	SetAngle(
+		const MT_ExpMap &q
+	);
+
+	/**
+	 * Increment the angle parameterization value. 
+	 */
+
+		void
+	IncrementAngle(
+		const MT_Vector3 &dq
+	);	
+
+
+	/**
+	 * Return the parameterization of this angle
+	 */
+
+	const 
+		MT_ExpMap &
+	ExpMap(
+	) const {
+		return m_q;
+	};
+
+
+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_ExpMap m_q;
+	MT_Scalar m_length;
+
+	/**
+	 * The maximum extension of this segment
+	 * This is the magnitude of the user offset + the length of the 
+	 * chain 
+	 */
+
+	MT_Scalar m_max_extension;
+
+	MT_Transform m_local_transform;
+	MT_Transform m_global_transform;
+	MT_Transform m_accum_local;
+
+	MT_Vector3 m_seg_start;
+	MT_Vector3 m_seg_end;
+
+};
+
+#endif

intern/iksolver/intern/IK_QSolver_Class.h

@@ -0,0 +1,107 @@
+/**
+ * $Id$
+ * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. The Blender
+ * Foundation also sells licenses for use in proprietary software under
+ * the Blender License.  See http://www.blender.org/BL/ for information
+ * about this.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): none yet.
+ *
+ * ***** END GPL/BL DUAL LICENSE BLOCK *****
+ */
+
+/**
+
+ * $Id$
+ * Copyright (C) 2001 NaN Technologies B.V.
+ *
+ * @author Laurence
+ */
+
+#ifndef NAN_INCLUDED_IK_Solver_Class
+#define NAN_INCLUDED_IK_Solver_Class 
+
+#include "IK_QChain.h"
+#include "IK_QJacobianSolver.h"
+#include "IK_QSegment.h"
+#include "MEM_SmartPtr.h"
+
+/**
+ * This class just contains all instances of internal data 
+ * associated with the external chain structure needed for 
+ * an ik solve. A pointer to this class gets hidden in the 
+ * external structure as a void pointer.
+ */
+
+class IK_QSolver_Class {
+
+public :
+
+	static 
+		IK_QSolver_Class *
+	New(
+	){
+		MEM_SmartPtr<IK_QSolver_Class> output (new IK_QSolver_Class);
+	
+		MEM_SmartPtr<IK_QJacobianSolver> solver (IK_QJacobianSolver::New());
+
+		if (output == NULL ||
+			solver == NULL
+		) {
+			return NULL;
+		}
+
+		output->m_solver = solver.Release();
+	
+		return output.Release();
+	};
+	
+		IK_QChain &	
+	Chain(
+	) {
+		return m_chain;
+	};
+		
+		IK_QJacobianSolver &
+	Solver(
+	) {
+		return m_solver.Ref();
+	}
+
+	~IK_QSolver_Class(
+	) {
+		// nothing to do
+	}
+
+
+private :
+
+	IK_QSolver_Class(
+	) {
+	};
+
+	IK_QChain m_chain;
+	MEM_SmartPtr<IK_QJacobianSolver> m_solver;
+
+};	
+
+#endif

intern/iksolver/intern/IK_Segment.cpp

@@ -0,0 +1,277 @@
+/**
+ * $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 *****
+ */
+
+#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];
+}
+
+
+
+
+

intern/iksolver/intern/IK_Segment.h

@@ -0,0 +1,217 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_Segment_h
+
+#define NAN_INCLUDED_Segment_h
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+
+#include "MT_Vector3.h"
+#include "MT_Transform.h"
+#include <vector>
+
+class IK_Segment {
+
+public :
+
+	/**
+	 * Constructor.
+     * @warning This class uses axis angles for it's parameterization.
+     * Axis angles are a poor representation for joints of more than 1 DOF
+     * because they suffer from Gimbal lock. This becomes noticeable in 
+	 * IK solutions. A better solution is to do use a quaternion to represent
+     * angles with 3 DOF 
+     */	
+
+	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
+	);
+
+
+	IK_Segment(
+	);
+
+
+	/**
+     * @return The length of the segment
+     */
+
+	const
+		MT_Scalar
+	Length(
+	) const ;
+
+	/**
+     * @return The transform from adjacent
+	 * coordinate systems in the chain.
+     */
+
+	const 
+		MT_Transform &
+	LocalTransform(
+	) const ;
+
+	
+	/** 
+     * Get the segment to compute it's 
+	 * global transform given the global transform
+	 * of the parent. This method also updtes the
+	 * global segment start
+	 */
+
+		void
+	UpdateGlobal(
+		const MT_Transform & global
+	);
+	
+	/**
+	 * @return A const reference to the global trnasformation 
+	 */
+
+	const 
+		MT_Transform &
+	GlobalTransform(
+	) const;
+		
+	/**
+	 * @return A const Reference to start of segment in global 
+     * coordinates
+	 */
+	
+	const 	
+		MT_Vector3 &
+	GlobalSegmentStart(
+	) const;
+
+	/**  
+	 * Computes the number of degrees of freedom of this segment
+	 */
+
+		int
+	DoF(
+	) const;
+
+
+	/**
+	 * Increment the active angles (at most DoF()) by 
+	 * d_theta. Which angles are incremented depends 
+	 * on which are active. 
+     * @return DoF()
+	 * @warning Bad interface
+     */
+
+		int
+	IncrementAngles(
+		MT_Scalar *d_theta
+	);
+
+	
+	// FIXME - interface bloat
+
+	/**
+     * @return the vectors about which the active
+	 * angles operate
+	 */
+
+	const
+		std::vector<MT_Vector3> &
+	AngleVectors(
+	) const;
+
+	/**
+	 * @return the ith active angle
+	 */
+
+		MT_Scalar
+	ActiveAngle(
+		int i
+	) const;
+
+	/**
+	 * @return the ith angle
+	 */
+		MT_Scalar
+	Angle(
+		int i
+	) const;
+
+
+	/**
+	 * Set the active angles from the array
+	 * @return the number of active angles
+	 */
+
+		int
+	SetAngles(
+		const MT_Scalar *angles
+	);
+
+
+private :
+	
+		void
+	UpdateLocalTransform(
+	);
+
+
+
+private :
+
+	/** The user defined transformation, composition of the 
+	 * translation and rotation from constructor.
+     */
+
+	MT_Transform m_transform;
+	MT_Scalar m_angles[3];
+	MT_Scalar m_length;
+
+	MT_Transform m_local_transform;
+	MT_Transform m_global_transform;
+
+	bool m_active_angles[3];
+
+	MT_Vector3 m_seg_start;
+
+	std::vector<MT_Vector3> m_angle_vectors;
+
+};
+
+#endif

intern/iksolver/intern/IK_Solver.cpp

@@ -0,0 +1,196 @@
+/**
+ * $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 *****
+ */
+
+#define IK_USE_EXPMAP
+
+
+
+#ifdef IK_USE_EXPMAP 
+#	include "IK_QSolver_Class.h"
+#else 
+#	include "IK_Solver_Class.h"
+#endif
+#include "../extern/IK_solver.h"
+
+#include <iostream.h>
+
+	IK_Chain_ExternPtr  
+IK_CreateChain(
+	void
+) {
+
+	MEM_SmartPtr<IK_Chain_Extern> output (new IK_Chain_Extern);
+	MEM_SmartPtr<IK_QSolver_Class> output_void (IK_QSolver_Class::New()); 	
+	
+
+	if (output == NULL || output_void == NULL) {
+		return NULL;
+	}
+
+	output->chain_dof = 0;
+	output->num_segments = 0;
+	output->segments = NULL;
+	output->intern = output_void.Release();
+	return output.Release();
+};
+
+
+	int 
+IK_LoadChain(
+	IK_Chain_ExternPtr chain,
+	IK_Segment_ExternPtr segments,
+	int num_segs
+) {
+
+	if (chain == NULL || segments == NULL) return 0;
+
+	IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
+	if (intern_cpp == NULL) return 0;
+
+	std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();	
+	if (segs.size() != num_segs) {
+		segs = std::vector<IK_QSegment>(num_segs);
+	}
+
+	std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
+	std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
+
+	IK_Segment_ExternPtr extern_seg_it = segments;
+	
+
+	for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) {
+	
+		MT_Point3 tr1(extern_seg_it->seg_start);
+
+		MT_Matrix3x3 A(
+			extern_seg_it->basis[0],extern_seg_it->basis[1],extern_seg_it->basis[2],
+			extern_seg_it->basis[3],extern_seg_it->basis[4],extern_seg_it->basis[5],
+			extern_seg_it->basis[6],extern_seg_it->basis[7],extern_seg_it->basis[8]
+		);
+
+		MT_Scalar length(extern_seg_it->length);
+
+
+		*seg_begin = IK_QSegment( 
+			tr1,A,length,MT_Vector3(0,0,0)
+		);
+
+	}
+
+
+	intern_cpp->Chain().UpdateGlobalTransformations();
+	intern_cpp->Chain().ComputeJacobian();
+	
+	chain->num_segments = num_segs;
+	chain->chain_dof = intern_cpp->Chain().DoF();
+	chain->segments = segments;
+
+	return 1;
+};		
+		
+	int 
+IK_SolveChain(
+	IK_Chain_ExternPtr chain,
+	float goal[3],
+	float tolerance,
+	int max_iterations,
+	float max_angle_change, 
+	IK_Segment_ExternPtr output
+){
+	if (chain == NULL || output == NULL) return 0;
+	if (chain->intern == NULL) return 0;
+
+	IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
+
+	IK_QJacobianSolver & solver = intern_cpp->Solver();
+
+	bool solve_result = solver.Solve(
+		intern_cpp->Chain(),
+		MT_Vector3(goal),
+		MT_Vector3(0,0,0),
+		MT_Scalar(tolerance),
+		max_iterations,
+		MT_Scalar(max_angle_change)	
+	);
+	
+	// turn the computed role->pitch->yaw into a quaternion and 
+	// return the result in output
+	
+	std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();	
+	std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
+	std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
+
+	for (;seg_begin != seg_end; ++seg_begin, ++output) {
+		MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix();
+
+		// don't forget to transpose this qrot for use by blender!
+
+		qrot.transpose(); // blender uses transpose here ????
+
+		output->basis_change[0] = float(qrot[0][0]);
+		output->basis_change[1] = float(qrot[0][1]);
+		output->basis_change[2] = float(qrot[0][2]);
+		output->basis_change[3] = float(qrot[1][0]);
+		output->basis_change[4] = float(qrot[1][1]);
+		output->basis_change[5] = float(qrot[1][2]);
+		output->basis_change[6] = float(qrot[2][0]);
+		output->basis_change[7] = float(qrot[2][1]);
+		output->basis_change[8] = float(qrot[2][2]);
+
+	}
+
+
+	return solve_result ? 1 : 0;
+}
+
+	void 
+IK_FreeChain(
+	IK_Chain_ExternPtr chain
+){
+	IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
+
+	delete(intern_cpp);
+	delete(chain);
+
+}	
+
+
+
+
+
+	
+	
+
+
+	
+
+
+

intern/iksolver/intern/IK_Solver_Class.h

@@ -0,0 +1,93 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_IK_Solver_Class
+
+#define NAN_INCLUDED_IK_Solver_Class 
+
+#include "IK_Chain.h"
+#include "IK_JacobianSolver.h"
+#include "IK_Segment.h"
+#include "MEM_SmartPtr.h"
+
+class IK_Solver_Class {
+
+public :
+
+	static 
+		IK_Solver_Class *
+	New(
+	){
+		MEM_SmartPtr<IK_Solver_Class> output (new IK_Solver_Class);
+	
+		MEM_SmartPtr<IK_JacobianSolver> solver (IK_JacobianSolver::New());
+
+		if (output == NULL ||
+			solver == NULL
+		) {
+			return NULL;
+		}
+
+		output->m_solver = solver.Release();
+	
+		return output.Release();
+	};
+	
+		IK_Chain &	
+	Chain(
+	) {
+		return m_chain;
+	};
+		
+		IK_JacobianSolver &
+	Solver(
+	) {
+		return m_solver.Ref();
+	}
+
+	~IK_Solver_Class(
+	) {
+		// nothing to do
+	}
+
+
+private :
+
+	IK_Solver_Class(
+	) {
+	};
+
+	IK_Chain m_chain;
+	MEM_SmartPtr<IK_JacobianSolver> m_solver;
+
+};	
+
+#endif

intern/iksolver/intern/MT_ExpMap.cpp

@@ -0,0 +1,268 @@
+/**
+ * $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
+ */
+
+#include "MT_ExpMap.h"
+
+/** 
+ * Set the exponential map from a quaternion. The quaternion must be non-zero.
+ */
+
+	void
+MT_ExpMap::
+setRotation(
+	const MT_Quaternion &q
+) {
+	// ok first normailize 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();
+
+	MT_Vector3 axis(qt.x(),qt.y(),qt.z());
+	MT_Scalar cosp = qt.w();
+	MT_Scalar sinp = axis.length();
+	axis /= sinp;
+
+	MT_Scalar theta = atan2(double(sinp),double(cosp));
+
+	axis *= theta;	
+	m_v = axis;
+}
+ 	
+/** 
+ * Convert from an exponential map to a quaternion
+ * representation
+ */	
+
+	MT_Quaternion 
+MT_ExpMap::
+getRotation(
+) const {
+    bool  rep=0;
+    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;
+}
+	
+/** 
+ * Convert the exponential map to a 3x3 matrix
+ */
+
+	MT_Matrix3x3
+MT_ExpMap::
+getMatrix(
+) const {
+
+	MT_Quaternion q = getRotation();
+	return MT_Matrix3x3(q);
+}
+
+
+
+
+/** 
+ * Force a reparameterization of the exponential
+ * map.
+ */
+
+	bool
+MT_ExpMap::
+reParameterize(
+	MT_Scalar &theta
+){
+    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;
+
+}
+
+/**
+ * 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
+ */
+
+	MT_Matrix4x4
+MT_ExpMap::
+partialDerivatives(
+	const int i
+) const {
+	
+	MT_Quaternion q = getRotation();
+	MT_Quaternion dQdx;
+
+	MT_Matrix4x4 output;
+
+	compute_dQdVi(i,dQdx);
+	compute_dRdVi(q,dQdx,output);
+
+	return output;
+}
+
+	void
+MT_ExpMap::
+compute_dRdVi(
+	const MT_Quaternion &q,
+	const MT_Quaternion &dQdvi,
+	MT_Matrix4x4 & dRdvi
+) const {
+
+    MT_Scalar  prod[9];
+    
+    /* This efficient formulation is arrived at by writing out the
+     * entire chain rule product dRdq * dqdv in terms of 'q' and 
+     * noticing that all the entries are formed from sums of just
+     * nine products of 'q' and 'dqdv' */
+
+    prod[0] = -MT_Scalar(4)*q.x()*dQdvi.x();
+    prod[1] = -MT_Scalar(4)*q.y()*dQdvi.y();
+    prod[2] = -MT_Scalar(4)*q.z()*dQdvi.z();
+    prod[3] = MT_Scalar(2)*(q.y()*dQdvi.x() + q.x()*dQdvi.y());
+    prod[4] = MT_Scalar(2)*(q.w()*dQdvi.z() + q.z()*dQdvi.w());
+    prod[5] = MT_Scalar(2)*(q.z()*dQdvi.x() + q.x()*dQdvi.z());
+    prod[6] = MT_Scalar(2)*(q.w()*dQdvi.y() + q.y()*dQdvi.w());
+    prod[7] = MT_Scalar(2)*(q.z()*dQdvi.y() + q.y()*dQdvi.z());
+    prod[8] = MT_Scalar(2)*(q.w()*dQdvi.x() + q.x()*dQdvi.w());
+
+    /* first row, followed by second and third */
+    dRdvi[0][0] = prod[1] + prod[2];
+    dRdvi[0][1] = prod[3] - prod[4];
+    dRdvi[0][2] = prod[5] + prod[6];
+
+    dRdvi[1][0] = prod[3] + prod[4];
+    dRdvi[1][1] = prod[0] + prod[2];
+    dRdvi[1][2] = prod[7] - prod[8];
+
+    dRdvi[2][0] = prod[5] - prod[6];
+    dRdvi[2][1] = prod[7] + prod[8];
+    dRdvi[2][2] = prod[0] + prod[1];
+
+    /* the 4th row and column are all zero */
+    int       i;
+
+    for (i=0; i<3; i++)	
+      dRdvi[3][i] = dRdvi[i][3] = MT_Scalar(0);
+    dRdvi[3][3] = 0;
+}
+
+// compute partial derivatives dQ/dVi
+
+	void
+MT_ExpMap::
+compute_dQdVi(
+	const int i,
+	MT_Quaternion & dQdX
+) const {
+
+    MT_Scalar   theta = m_v.length();
+    MT_Scalar   cosp(cos(MT_Scalar(.5)*theta)), sinp(sin(MT_Scalar(.5)*theta));
+    
+    MT_assert(i>=0 && i<3);
+
+    /* This is an efficient implementation of the derivatives given
+     * in Appendix A of the paper with common subexpressions factored out */
+    if (theta < MT_EXPMAP_MINANGLE){
+		const int i2 = (i+1)%3, i3 = (i+2)%3;
+		MT_Scalar Tsinc = MT_Scalar(0.5) - theta*theta/MT_Scalar(48.0);
+		MT_Scalar vTerm = m_v[i] * (theta*theta/MT_Scalar(40.0) - MT_Scalar(1.0)) / MT_Scalar(24.0);
+		
+		dQdX.w() = -.5*m_v[i]*Tsinc;
+		dQdX[i]  = m_v[i]* vTerm + Tsinc;
+		dQdX[i2] = m_v[i2]*vTerm;
+		dQdX[i3] = m_v[i3]*vTerm;
+    } else {
+		const int i2 = (i+1)%3, i3 = (i+2)%3;
+		const MT_Scalar  ang = 1.0/theta, ang2 = ang*ang*m_v[i], sang = sinp*ang;
+		const MT_Scalar  cterm = ang2*(.5*cosp - sang);
+		
+		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;
+    }
+}
+
+ 
+
+
+
+
+
+
+
+

intern/iksolver/intern/MT_ExpMap.h

@@ -0,0 +1,219 @@
+/**
+ * $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 MT_ExpMap_H
+#define MT_ExpMap_H
+
+#include <MT_assert.h>
+
+#include "MT_Vector3.h"
+#include "MT_Quaternion.h"
+#include "MT_Matrix4x4.h"
+
+const MT_Scalar MT_EXPMAP_MINANGLE (1e-7);
+
+/**
+ * MT_ExpMap an exponential map parameterization of rotations
+ * in 3D. This implementation is derived from the paper 
+ * "F. Sebastian Grassia. Practical parameterization of 
+ * rotations using the exponential map. Journal of Graphics Tools,
+ *  3(3):29-48, 1998" Please go to http://www.acm.org/jgt/papers/Grassia98/
+ * for a thorough description of the theory and sample code used 
+ * to derive this class. 
+ *
+ * Basic overview of why this class is used.
+ * In an IK system we need to paramterize the joint angles in some
+ * way. Typically 2 parameterizations are used.
+ * - Euler Angles
+ * These suffer from singularities in the parameterization known
+ * as gimbal lock. They also do not interpolate well. For every
+ * set of euler angles there is exactly 1 corresponding 3d rotation.
+ * - Quaternions.
+ * Great for interpolating. Only unit quaternions are valid rotations
+ * means that in a differential ik solver we often stray outside of
+ * this manifold into invalid rotations. Means we have to do a lot
+ * of nasty normalizations all the time. Does not suffer from 
+ * gimbal lock problems. More expensive to compute partial derivatives
+ * as there are 4 of them.
+ * 
+ * So exponential map is similar to a quaternion axis/angle 
+ * representation but we store the angle as the length of the
+ * axis. So require only 3 parameters. Means that all exponential
+ * maps are valid rotations. Suffers from gimbal lock. But it's
+ * possible to detect when gimbal lock is near and reparameterize
+ * away from it. Also nice for interpolating.
+ * Exponential maps are share some of the useful properties of
+ * euler and quaternion parameterizations. And are very useful
+ * for differential IK solvers.
+ */
+
+class MT_ExpMap {
+public:
+
+	/**
+	 * Default constructor
+	 * @warning there is no initialization in the
+	 * default constructor
+	 */ 
+
+    MT_ExpMap() {}
+    MT_ExpMap(const MT_Vector3& v) : m_v(v) {}
+
+    MT_ExpMap(const float v[3]) : m_v(v) {}
+    MT_ExpMap(const double v[3]) : m_v(v) {}
+
+    MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) :
+        m_v(x, y, z) {}
+
+	/** 
+	 * Construct an exponential map from a quaternion
+	 */
+
+	MT_ExpMap(
+		const MT_Quaternion &q
+	) {
+		setRotation(q);
+	};
+
+	/** 
+	 * Accessors
+	 * Decided not to inherit from MT_Vector3 but rather
+	 * this class contains an MT_Vector3. This is because
+	 * it is very dangerous to use MT_Vector3 functions
+	 * on this class and some of them have no direct meaning.
+	 */
+
+		MT_Vector3 &
+	vector(
+	) {
+		return m_v;
+	};
+
+	const 
+		MT_Vector3 &
+	vector(
+	) const {
+		return m_v;
+	};
+
+	/** 
+	 * Set the exponential map from a quaternion
+	 */
+
+		void
+	setRotation(
+		const MT_Quaternion &q
+	);
+
+	/** 
+	 * Convert from an exponential map to a quaternion
+	 * representation
+	 */	
+	
+		MT_Quaternion 
+	getRotation(
+	) const;
+
+	/** 
+	 * Convert the exponential map to a 3x3 matrix
+	 */
+
+		MT_Matrix3x3
+	getMatrix(
+	) 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.
+	 */
+
+		bool
+	reParameterize(
+		MT_Scalar &theta
+	);
+
+	/**
+	 * Compute the partial derivatives of the exponential
+	 * map  (dR/de - where R is a 4x4 matrix formed 
+	 * from the map) and return them as a 4x4 matrix
+	 */
+
+		MT_Matrix4x4
+	partialDerivatives(
+		const int i
+	) const ;
+	
+private :
+	
+	MT_Vector3 m_v;
+
+	// 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
+	) const;
+
+	// compute partial derivatives dQ/dVi
+
+		void
+	compute_dQdVi(
+		int i,
+		MT_Quaternion & dQdX
+	) const ; 
+
+		
+};
+
+
+
+#endif
+
+
+

intern/iksolver/intern/Makefile

@@ -0,0 +1,44 @@
+#
+# $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 *****
+# iksolver intern Makefile
+#
+
+LIBNAME = iksolver
+DIR = $(OCGDIR)/intern/$(LIBNAME)
+CCSRCS = IK_QChain.cpp IK_QJacobianSolver.cpp IK_QSegment.cpp MT_ExpMap.cpp IK_Solver.cpp
+
+include nan_compile.mk
+
+CCFLAGS += $(LEVEL_2_CPP_WARNINGS)
+
+CPPFLAGS += -I$(NAN_MOTO)/include
+CPPFLAGS += -I$(NAN_MEMUTIL)/include
+

intern/iksolver/intern/TNT/cholesky.h

@@ -0,0 +1,128 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+#ifndef CHOLESKY_H
+#define CHOLESKY_H
+
+#include <cmath>
+
+// index method
+
+namespace TNT
+{
+
+
+//
+// Only upper part of A is used.  Cholesky factor is returned in
+// lower part of L.  Returns 0 if successful, 1 otherwise.
+//
+template <class SPDMatrix, class SymmMatrix>
+int Cholesky_upper_factorization(SPDMatrix &A, SymmMatrix &L)
+{
+    Subscript M = A.dim(1);
+    Subscript N = A.dim(2);
+
+    assert(M == N);                 // make sure A is square
+
+    // readjust size of L, if necessary
+
+    if (M != L.dim(1) || N != L.dim(2))
+        L = SymmMatrix(N,N);
+
+    Subscript i,j,k;
+
+
+    typename SPDMatrix::element_type dot=0;
+
+
+    for (j=1; j<=N; j++)                // form column j of L
+    {
+        dot= 0;
+
+        for (i=1; i<j; i++)             // for k= 1 TO j-1
+            dot = dot +  L(j,i)*L(j,i);
+
+        L(j,j) = A(j,j) - dot;
+
+        for (i=j+1; i<=N; i++)
+        {
+            dot = 0;
+            for (k=1; k<j; k++)
+                dot = dot +  L(i,k)*L(j,k);
+            L(i,j) = A(j,i) - dot;
+        }
+
+        if (L(j,j) <= 0.0) return 1;
+
+        L(j,j) = sqrt( L(j,j) );
+
+        for (i=j+1; i<=N; i++)
+            L(i,j) = L(i,j) / L(j,j);
+
+    }
+
+    return 0;
+}
+
+
+
+
+}  
+// namespace TNT
+
+#endif
+// CHOLESKY_H

intern/iksolver/intern/TNT/cmat.h

@@ -0,0 +1,661 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// C compatible matrix: row-oriented, 0-based [i][j] and 1-based (i,j) indexing
+//
+
+#ifndef CMAT_H
+#define CMAT_H
+
+#include "subscript.h"
+#include "vec.h"
+#include <stdlib.h>
+#include <assert.h>
+#include <iostream>
+#include <strstream>
+#ifdef TNT_USE_REGIONS
+#include "region2d.h"
+#endif
+
+namespace TNT
+{
+
+template <class T>
+class Matrix 
+{
+
+
+  public:
+
+    typedef Subscript   size_type;
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+
+    Subscript lbound() const { return 1;}
+ 
+  protected:
+    Subscript m_;
+    Subscript n_;
+    Subscript mn_;      // total size
+    T* v_;                  
+    T** row_;           
+    T* vm1_ ;       // these point to the same data, but are 1-based 
+    T** rowm1_;
+
+    // internal helper function to create the array
+    // of row pointers
+
+    void initialize(Subscript M, Subscript N)
+    {
+        mn_ = M*N;
+        m_ = M;
+        n_ = N;
+
+        v_ = new T[mn_]; 
+        row_ = new T*[M];
+        rowm1_ = new T*[M];
+
+        assert(v_  != NULL);
+        assert(row_  != NULL);
+        assert(rowm1_ != NULL);
+
+        T* p = v_;              
+        vm1_ = v_ - 1;
+        for (Subscript i=0; i<M; i++)
+        {
+            row_[i] = p;
+            rowm1_[i] = p-1;
+            p += N ;
+            
+        }
+
+        rowm1_ -- ;     // compensate for 1-based offset
+    }
+   
+    void copy(const T*  v)
+    {
+        Subscript N = m_ * n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = v[i];
+            v_[i+1] = v[i+1];
+            v_[i+2] = v[i+2];
+            v_[i+3] = v[i+3];
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = v[i];
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = v[i];
+#endif      
+    }
+
+    void set(const T& val)
+    {
+        Subscript N = m_ * n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = val;
+            v_[i+1] = val;
+            v_[i+2] = val;
+            v_[i+3] = val; 
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = val;
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = val;
+        
+#endif      
+    }
+    
+
+    
+    void destroy()
+    {     
+        /* do nothing, if no memory has been previously allocated */
+        if (v_ == NULL) return ;
+
+        /* if we are here, then matrix was previously allocated */
+        if (v_ != NULL) delete [] (v_);     
+        if (row_ != NULL) delete [] (row_);
+
+        /* return rowm1_ back to original value */
+        rowm1_ ++;
+        if (rowm1_ != NULL ) delete [] (rowm1_);
+    }
+
+
+  public:
+
+    operator T**(){ return  row_; }
+    operator T**() const { return row_; }
+
+
+    Subscript size() const { return mn_; }
+
+    // constructors
+
+    Matrix() : m_(0), n_(0), mn_(0), v_(0), row_(0), vm1_(0), rowm1_(0) {};
+
+    Matrix(const Matrix<T> &A)
+    {
+        initialize(A.m_, A.n_);
+        copy(A.v_);
+    }
+
+    Matrix(Subscript M, Subscript N, const T& value = T())
+    {
+        initialize(M,N);
+        set(value);
+    }
+
+    Matrix(Subscript M, Subscript N, const T* v)
+    {
+        initialize(M,N);
+        copy(v);
+    }
+
+    Matrix(Subscript M, Subscript N, const char *s)
+    {
+        initialize(M,N);
+        std::istrstream ins(s);
+
+        Subscript i, j;
+
+        for (i=0; i<M; i++)
+            for (j=0; j<N; j++)
+                ins >> row_[i][j];
+    }
+
+		
+    // destructor
+    //
+    ~Matrix()
+    {
+        destroy();
+    }
+
+
+    // reallocating
+    //
+    Matrix<T>& newsize(Subscript M, Subscript N)
+    {
+        if (num_rows() == M && num_cols() == N)
+            return *this;
+
+        destroy();
+        initialize(M,N);
+        
+        return *this;
+    }
+
+		void
+	diagonal(Vector<T> &diag)
+	{
+		int sz = diag.dim();
+		newsize(sz,sz);
+		set(0);
+
+		Subscript i;
+		for (i = 0; i < sz; i++) {
+			row_[i][i] = diag[i];
+		}
+	}
+
+
+
+    // assignments
+    //
+    Matrix<T>& operator=(const Matrix<T> &A)
+    {
+        if (v_ == A.v_)
+            return *this;
+
+        if (m_ == A.m_  && n_ == A.n_)      // no need to re-alloc
+            copy(A.v_);
+
+        else
+        {
+            destroy();
+            initialize(A.m_, A.n_);
+            copy(A.v_);
+        }
+
+        return *this;
+    }
+        
+    Matrix<T>& operator=(const T& scalar)
+    { 
+        set(scalar); 
+        return *this;
+    }
+
+
+    Subscript dim(Subscript d) const 
+    {
+#ifdef TNT_BOUNDS_CHECK
+       assert( d >= 1);
+        assert( d <= 2);
+#endif
+        return (d==1) ? m_ : ((d==2) ? n_ : 0); 
+    }
+
+    Subscript num_rows() const { return m_; }
+    Subscript num_cols() const { return n_; }
+
+
+
+
+    inline T* operator[](Subscript i)
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i < m_) ;
+#endif
+        return row_[i];
+    }
+
+    inline const T* operator[](Subscript i) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i < m_) ;
+#endif
+        return row_[i];
+    }
+
+    inline reference operator()(Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= mn_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+    inline const_reference operator()(Subscript i) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= mn_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+
+
+    inline reference operator()(Subscript i, Subscript j)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= m_) ;
+        assert(1<=j);
+        assert(j <= n_);
+#endif
+        return  rowm1_[i][j]; 
+    }
+
+
+    
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= m_) ;
+        assert(1<=j);
+        assert(j <= n_);
+#endif
+        return rowm1_[i][j]; 
+    }
+
+
+
+#ifdef TNT_USE_REGIONS
+
+    typedef Region2D<Matrix<T> > Region;
+    
+
+    Region operator()(const Index1D &I, const Index1D &J)
+    {
+        return Region(*this, I,J);
+    }
+
+
+    typedef const_Region2D< Matrix<T> > const_Region;
+    const_Region operator()(const Index1D &I, const Index1D &J) const
+    {
+        return const_Region(*this, I,J);
+    }
+
+#endif
+
+
+};
+
+
+/* ***************************  I/O  ********************************/
+
+template <class T>
+std::ostream& operator<<(std::ostream &s, const Matrix<T> &A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << "\n";
+
+    for (Subscript i=0; i<M; i++)
+    {
+        for (Subscript j=0; j<N; j++)
+        {
+            s << A[i][j] << " ";
+        }
+        s << "\n";
+    }
+
+
+    return s;
+}
+
+template <class T>
+std::istream& operator>>(std::istream &s, Matrix<T> &A)
+{
+
+    Subscript M, N;
+
+    s >> M >> N;
+
+    if ( !(M == A.num_rows() && N == A.num_cols() ))
+    {
+        A.newsize(M,N);
+    }
+
+
+    for (Subscript i=0; i<M; i++)
+        for (Subscript j=0; j<N; j++)
+        {
+            s >>  A[i][j];
+        }
+
+
+    return s;
+}
+
+// *******************[ basic matrix algorithms ]***************************
+
+template <class T>
+Matrix<T> operator+(const Matrix<T> &A, 
+    const Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=0; i<M; i++)
+        for (j=0; j<N; j++)
+            tmp[i][j] = A[i][j] + B[i][j];
+
+    return tmp;
+}
+
+template <class T>
+Matrix<T> operator-(const Matrix<T> &A, 
+    const Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=0; i<M; i++)
+        for (j=0; j<N; j++)
+            tmp[i][j] = A[i][j] - B[i][j];
+
+    return tmp;
+}
+
+template <class T>
+Matrix<T> mult_element(const Matrix<T> &A, 
+    const Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=0; i<M; i++)
+        for (j=0; j<N; j++)
+            tmp[i][j] = A[i][j] * B[i][j];
+
+    return tmp;
+}
+
+template <class T>
+void transpose(const Matrix<T> &A, Matrix<T> &S)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==S.num_cols());
+    assert(N==S.num_rows());
+
+    Subscript i, j;
+
+    for (i=0; i<M; i++)
+        for (j=0; j<N; j++)
+            S[j][i] = A[i][j];
+
+}
+
+
+template <class T>
+inline void matmult(Matrix<T>& C, const Matrix<T>  &A, 
+    const Matrix<T> &B)
+{
+
+    assert(A.num_cols() == B.num_rows());
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+    Subscript K = B.num_cols();
+
+    C.newsize(M,K);
+
+    T sum;
+
+    const T* row_i;
+    const T* col_k;
+
+    for (Subscript i=0; i<M; i++)
+    for (Subscript k=0; k<K; k++)
+    {
+        row_i  = &(A[i][0]);
+        col_k  = &(B[0][k]);
+        sum = 0;
+        for (Subscript j=0; j<N; j++)
+        {
+            sum  += *row_i * *col_k;
+            row_i++;
+            col_k += K;
+        }
+        C[i][k] = sum; 
+    }
+
+}
+
+template <class T>
+void matmult(Vector<T> &y, const Matrix<T>  &A, const Vector<T> &x)
+{
+
+#ifdef TNT_BOUNDS_CHECK
+    assert(A.num_cols() == x.dim());
+	assert(A.num_rows() == y.dim());
+#endif
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    T sum;
+
+    for (Subscript i=0; i<M; i++)
+    {
+        sum = 0;
+        const T* rowi = A[i];
+        for (Subscript j=0; j<N; j++)
+            sum = sum +  rowi[j] * x[j];
+
+        y[i] = sum; 
+    }
+}
+
+template <class T>
+inline void matmultdiag(
+	Matrix<T>& C, 
+	const Matrix<T> &A, 
+    const Vector<T> &diag
+){
+#ifdef TNT_BOUNDS_CHECK
+    assert(A.num_cols() ==A.num_rows()== diag.dim());
+#endif
+
+    Subscript M = A.num_rows();
+    Subscript K = diag.dim();
+
+    C.newsize(M,K);
+
+    const T* row_i;
+    const T* col_k;
+
+    for (Subscript i=0; i<M; i++) {
+		for (Subscript k=0; k<K; k++)
+		{
+			C[i][k] = A[i,k] * diag[k];		
+		}
+	}
+}
+	
+
+template <class T>
+inline void matmultdiag(
+	Matrix<T>& C, 
+    const Vector<T> &diag,
+	const Matrix<T> &A 
+){
+#ifdef TNT_BOUNDS_CHECK
+    assert(A.num_cols() ==A.num_rows()== diag.dim());
+#endif
+
+    Subscript M = A.num_rows();
+    Subscript K = diag.dim();
+
+    C.newsize(M,K);
+
+    for (Subscript i=0; i<M; i++) {
+	
+		const T diag_element = diag[i];
+
+		for (Subscript k=0; k<K; k++)
+		{
+			C[i][k] = A[i][k] * diag_element;		
+		}
+	}
+}
+
+
+
+
+} // namespace TNT
+
+
+
+
+#endif
+// CMAT_H

intern/iksolver/intern/TNT/fcscmat.h

@@ -0,0 +1,197 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+//  Templated compressed sparse column matrix (Fortran conventions).
+//  uses 1-based offsets in storing row indices.
+//  Used primarily to interface with Fortran sparse matrix libaries.
+//  (CANNOT BE USED AS AN STL CONTAINER.)
+
+
+#ifndef FCSCMAT_H
+#define FCSCMAT_H
+
+#include <iostream>
+#include <cassert>
+#include "tnt.h"
+#include "vec.h"
+
+using namespace std;
+
+namespace TNT
+{
+
+template <class T>
+class Fortran_Sparse_Col_Matrix 
+{
+
+   protected:
+
+       Vector<T>           val_;       // data values (nz_ elements)
+       Vector<Subscript>   rowind_;    // row_ind (nz_ elements)
+       Vector<Subscript>   colptr_;    // col_ptr (n_+1 elements)
+
+       int nz_;                   // number of nonzeros
+       Subscript m_;              // global dimensions
+       Subscript n_;
+  
+   public:
+
+
+       Fortran_Sparse_Col_Matrix(void);
+       Fortran_Sparse_Col_Matrix(const Fortran_Sparse_Col_Matrix<T> &S)
+        : val_(S.val_), rowind_(S.rowind_), colptr_(S.colptr_), nz_(S.nz_),
+            m_(S.m_), n_(S.n_) {};
+       Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, 
+            Subscript nz, const T  *val,  const Subscript *r, 
+            const Subscript *c) : val_(nz, val), rowind_(nz, r), 
+            colptr_(N+1, c), nz_(nz), m_(M), n_(N) {};
+
+       Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, 
+            Subscript nz, char *val,  char  *r, 
+            char *c) : val_(nz, val), rowind_(nz, r), 
+            colptr_(N+1, c), nz_(nz), m_(M), n_(N) {};
+
+       Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, 
+            Subscript nz, const T  *val, Subscript *r, Subscript *c)
+            : val_(nz, val), rowind_(nz, r), colptr_(N+1, c), nz_(nz), 
+                    m_(M), n_(N) {};
+    
+      ~Fortran_Sparse_Col_Matrix() {};
+        
+
+       T &      val(Subscript i) { return val_(i); }
+       const T &      val(Subscript i) const { return val_(i); }
+
+       Subscript &   row_ind(Subscript i) { return rowind_(i); }
+       const Subscript &   row_ind(Subscript i) const { return rowind_(i); }
+
+       Subscript    col_ptr(Subscript i) { return colptr_(i);}
+       const Subscript    col_ptr(Subscript i) const { return colptr_(i);}
+
+
+       Subscript    num_cols() const { return m_;}
+       Subscript    num_rows() const { return n_; }
+
+       Subscript          dim(Subscript i) const 
+       {
+#ifdef TNT_BOUNDS_CHECK
+            assert( 1 <= i );
+            assert( i <= 2 );
+#endif
+            if (i==1) return m_;
+            else if (i==2) return m_;
+            else return 0;
+        }
+
+       Subscript          num_nonzeros() const {return nz_;};
+       Subscript          lbound() const {return 1;}
+
+
+
+       Fortran_Sparse_Col_Matrix& operator=(const 
+            Fortran_Sparse_Col_Matrix &C)
+        {
+            val_ = C.val_;
+            rowind_ = C.rowind_;
+            colptr_ = C.colptr_;
+            nz_ = C.nz_;
+            m_ = C.m_;
+            n_ = C.n_;
+
+            return *this;
+        }
+
+       Fortran_Sparse_Col_Matrix& newsize(Subscript M, Subscript N, 
+                Subscript nz)
+        {
+            val_.newsize(nz);
+            rowind_.newsize(nz);
+            colptr_.newsize(N+1);
+            return *this;
+        }
+};
+
+template <class T>
+ostream& operator<<(ostream &s, const Fortran_Sparse_Col_Matrix<T> &A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << " " << A.num_nonzeros() <<  endl;
+
+
+    for (Subscript k=1; k<=N; k++)
+    {
+        Subscript start = A.col_ptr(k);
+        Subscript end = A.col_ptr(k+1);
+
+        for (Subscript i= start; i<end; i++)
+        {
+            s << A.row_ind(i) << " " << k << " " << A.val(i) << endl;
+        }
+    }
+
+    return s;
+}
+
+
+
+} // namespace TNT
+
+#endif  
+/* FCSCMAT_H */
+

intern/iksolver/intern/TNT/fmat.h

@@ -0,0 +1,609 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Fortran-compatible matrix: column oriented, 1-based (i,j) indexing
+
+#ifndef FMAT_H
+#define FMAT_H
+
+#include "subscript.h"
+#include "vec.h"
+#include <cstdlib>
+#include <cassert>
+#include <iostream>
+#include <strstream>
+#ifdef TNT_USE_REGIONS
+#include "region2d.h"
+#endif
+
+// simple 1-based, column oriented Matrix class
+
+namespace TNT
+{
+
+template <class T>
+class Fortran_Matrix 
+{
+
+
+  public:
+
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+
+    Subscript lbound() const { return 1;}
+ 
+  protected:
+    T* v_;                  // these are adjusted to simulate 1-offset
+    Subscript m_;
+    Subscript n_;
+    T** col_;           // these are adjusted to simulate 1-offset
+
+    // internal helper function to create the array
+    // of row pointers
+
+    void initialize(Subscript M, Subscript N)
+    {
+        // adjust col_[] pointers so that they are 1-offset:
+        //   col_[j][i] is really col_[j-1][i-1];
+        //
+        // v_[] is the internal contiguous array, it is still 0-offset
+        //
+        v_ = new T[M*N];
+        col_ = new T*[N];
+
+        assert(v_  != NULL);
+        assert(col_ != NULL);
+
+
+        m_ = M;
+        n_ = N;
+        T* p = v_ - 1;              
+        for (Subscript i=0; i<N; i++)
+        {
+            col_[i] = p;
+            p += M ;
+            
+        }
+        col_ --; 
+    }
+   
+    void copy(const T*  v)
+    {
+        Subscript N = m_ * n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = v[i];
+            v_[i+1] = v[i+1];
+            v_[i+2] = v[i+2];
+            v_[i+3] = v[i+3];
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = v[i];
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = v[i];
+#endif      
+    }
+
+    void set(const T& val)
+    {
+        Subscript N = m_ * n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = val;
+            v_[i+1] = val;
+            v_[i+2] = val;
+            v_[i+3] = val; 
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = val;
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = val;
+        
+#endif      
+    }
+    
+
+
+    void destroy()
+    {     
+        /* do nothing, if no memory has been previously allocated */
+        if (v_ == NULL) return ;
+
+        /* if we are here, then matrix was previously allocated */
+        delete [] (v_);     
+        col_ ++;                // changed back to 0-offset
+        delete [] (col_);
+    }
+
+
+  public:
+
+    T* begin() { return v_; }
+    const T* begin() const { return v_;}
+
+    T* end() { return v_ + m_*n_; }
+    const T* end() const { return v_ + m_*n_; }
+
+
+    // constructors
+
+    Fortran_Matrix() : v_(0), m_(0), n_(0), col_(0)  {};
+    Fortran_Matrix(const Fortran_Matrix<T> &A)
+    {
+        initialize(A.m_, A.n_);
+        copy(A.v_);
+    }
+
+    Fortran_Matrix(Subscript M, Subscript N, const T& value = T())
+    {
+        initialize(M,N);
+        set(value);
+    }
+
+    Fortran_Matrix(Subscript M, Subscript N, const T* v)
+    {
+        initialize(M,N);
+        copy(v);
+    }
+
+
+    Fortran_Matrix(Subscript M, Subscript N, char *s)
+    {
+        initialize(M,N);
+        std::istrstream ins(s);
+
+        Subscript i, j;
+
+        for (i=1; i<=M; i++)
+            for (j=1; j<=N; j++)
+                ins >> (*this)(i,j);
+    }
+
+    // destructor
+    ~Fortran_Matrix()
+    {
+        destroy();
+    }
+
+
+    // assignments
+    //
+    Fortran_Matrix<T>& operator=(const Fortran_Matrix<T> &A)
+    {
+        if (v_ == A.v_)
+            return *this;
+
+        if (m_ == A.m_  && n_ == A.n_)      // no need to re-alloc
+            copy(A.v_);
+
+        else
+        {
+            destroy();
+            initialize(A.m_, A.n_);
+            copy(A.v_);
+        }
+
+        return *this;
+    }
+        
+    Fortran_Matrix<T>& operator=(const T& scalar)
+    { 
+        set(scalar); 
+        return *this;
+    }
+
+
+    Subscript dim(Subscript d) const 
+    {
+#ifdef TNT_BOUNDS_CHECK
+       assert( d >= 1);
+        assert( d <= 2);
+#endif
+        return (d==1) ? m_ : ((d==2) ? n_ : 0); 
+    }
+
+    Subscript num_rows() const { return m_; }
+    Subscript num_cols() const { return n_; }
+
+    Fortran_Matrix<T>& newsize(Subscript M, Subscript N)
+    {
+        if (num_rows() == M && num_cols() == N)
+            return *this;
+
+        destroy();
+        initialize(M,N);
+
+        return *this;
+    }
+
+
+
+    // 1-based element access
+    //
+    inline reference operator()(Subscript i, Subscript j)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= m_) ;
+        assert(1<=j);
+        assert(j <= n_);
+#endif
+        return col_[j][i]; 
+    }
+
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= m_) ;
+        assert(1<=j);
+        assert(j <= n_);
+#endif
+        return col_[j][i]; 
+    }
+
+
+#ifdef TNT_USE_REGIONS
+
+    typedef Region2D<Fortran_Matrix<T> > Region;
+    typedef const_Region2D< Fortran_Matrix<T> > const_Region;
+
+    Region operator()(const Index1D &I, const Index1D &J)
+    {
+        return Region(*this, I,J);
+    }
+
+    const_Region operator()(const Index1D &I, const Index1D &J) const
+    {
+        return const_Region(*this, I,J);
+    }
+
+#endif
+
+
+};
+
+
+/* ***************************  I/O  ********************************/
+
+template <class T>
+std::ostream& operator<<(std::ostream &s, const Fortran_Matrix<T> &A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << "\n";
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript j=1; j<=N; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << "\n";
+    }
+
+
+    return s;
+}
+
+template <class T>
+std::istream& operator>>(std::istream &s, Fortran_Matrix<T> &A)
+{
+
+    Subscript M, N;
+
+    s >> M >> N;
+
+    if ( !(M == A.num_rows() && N == A.num_cols()))
+    {
+        A.newsize(M,N);
+    }
+
+
+    for (Subscript i=1; i<=M; i++)
+        for (Subscript j=1; j<=N; j++)
+        {
+            s >>  A(i,j);
+        }
+
+
+    return s;
+}
+
+// *******************[ basic matrix algorithms ]***************************
+
+
+template <class T>
+Fortran_Matrix<T> operator+(const Fortran_Matrix<T> &A, 
+    const Fortran_Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Fortran_Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=1; i<=M; i++)
+        for (j=1; j<=N; j++)
+            tmp(i,j) = A(i,j) + B(i,j);
+
+    return tmp;
+}
+
+template <class T>
+Fortran_Matrix<T> operator-(const Fortran_Matrix<T> &A, 
+    const Fortran_Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Fortran_Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=1; i<=M; i++)
+        for (j=1; j<=N; j++)
+            tmp(i,j) = A(i,j) - B(i,j);
+
+    return tmp;
+}
+
+// element-wise multiplication  (use matmult() below for matrix
+// multiplication in the linear algebra sense.)
+//
+//
+template <class T>
+Fortran_Matrix<T> mult_element(const Fortran_Matrix<T> &A, 
+    const Fortran_Matrix<T> &B)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(M==B.num_rows());
+    assert(N==B.num_cols());
+
+    Fortran_Matrix<T> tmp(M,N);
+    Subscript i,j;
+
+    for (i=1; i<=M; i++)
+        for (j=1; j<=N; j++)
+            tmp(i,j) = A(i,j) * B(i,j);
+
+    return tmp;
+}
+
+
+template <class T>
+Fortran_Matrix<T> transpose(const Fortran_Matrix<T> &A)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    Fortran_Matrix<T> S(N,M);
+    Subscript i, j;
+
+    for (i=1; i<=M; i++)
+        for (j=1; j<=N; j++)
+            S(j,i) = A(i,j);
+
+    return S;
+}
+
+
+    
+template <class T>
+inline Fortran_Matrix<T> matmult(const Fortran_Matrix<T>  &A, 
+    const Fortran_Matrix<T> &B)
+{
+
+#ifdef TNT_BOUNDS_CHECK
+    assert(A.num_cols() == B.num_rows());
+#endif
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+    Subscript K = B.num_cols();
+
+    Fortran_Matrix<T> tmp(M,K);
+    T sum;
+
+    for (Subscript i=1; i<=M; i++)
+    for (Subscript k=1; k<=K; k++)
+    {
+        sum = 0;
+        for (Subscript j=1; j<=N; j++)
+            sum = sum +  A(i,j) * B(j,k);
+
+        tmp(i,k) = sum; 
+    }
+
+    return tmp;
+}
+
+template <class T>
+inline Fortran_Matrix<T> operator*(const Fortran_Matrix<T> &A, 
+    const Fortran_Matrix<T> &B)
+{
+    return matmult(A,B);
+}
+
+template <class T>
+inline int matmult(Fortran_Matrix<T>& C, const Fortran_Matrix<T>  &A, 
+    const Fortran_Matrix<T> &B)
+{
+
+    assert(A.num_cols() == B.num_rows());
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+    Subscript K = B.num_cols();
+
+    C.newsize(M,K);         // adjust shape of C, if necessary
+
+
+    T sum; 
+
+    const T* row_i;
+    const T* col_k;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript k=1; k<=K; k++)
+        {
+            row_i = &A(i,1);
+            col_k = &B(1,k);
+            sum = 0;
+            for (Subscript j=1; j<=N; j++)
+            {
+                sum +=  *row_i * *col_k;
+                row_i += M;
+                col_k ++;
+            }
+        
+            C(i,k) = sum; 
+        }
+
+    }
+
+    return 0;
+}
+
+
+template <class T>
+Vector<T> matmult(const Fortran_Matrix<T>  &A, const Vector<T> &x)
+{
+
+#ifdef TNT_BOUNDS_CHECK
+    assert(A.num_cols() == x.dim());
+#endif
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    Vector<T> tmp(M);
+    T sum;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        sum = 0;
+        for (Subscript j=1; j<=N; j++)
+            sum = sum +  A(i,j) * x(j);
+
+        tmp(i) = sum; 
+    }
+
+    return tmp;
+}
+
+template <class T>
+inline Vector<T> operator*(const Fortran_Matrix<T>  &A, const Vector<T> &x)
+{
+    return matmult(A,x);
+}
+
+template <class T>
+inline Fortran_Matrix<T> operator*(const Fortran_Matrix<T>  &A, const T &x)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    Subscript MN = M*N; 
+
+    Fortran_Matrix<T> res(M,N);
+    const T* a = A.begin();
+    T* t = res.begin();
+    T* tend = res.end();
+
+    for (t=res.begin(); t < tend; t++, a++)
+        *t = *a * x;
+
+    return res;
+} 
+
+}  // namespace TNT
+#endif
+// FMAT_H

intern/iksolver/intern/TNT/fortran.h

@@ -0,0 +1,99 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Header file to define C/Fortran conventions (Platform specific)
+
+#ifndef FORTRAN_H
+#define FORTRAN_H
+
+// help map between C/C++ data types and Fortran types
+
+typedef int     Fortran_integer;
+typedef float   Fortran_float;
+typedef double  Fortran_double;
+
+
+typedef Fortran_double *fda_;        // (in/out) double precision array
+typedef const Fortran_double *cfda_; // (in) double precsion array
+
+typedef Fortran_double *fd_;        // (in/out)  single double precision
+typedef const Fortran_double *cfd_; // (in) single double precision
+
+typedef Fortran_float *ffa_;        // (in/out) float precision array
+typedef const Fortran_float *cffa_; // (in) float precsion array
+
+typedef Fortran_float *ff_;         // (in/out)  single float precision
+typedef const Fortran_float *cff_;  // (in) single float precision
+
+typedef Fortran_integer *fia_;          // (in/out)  single integer array
+typedef const Fortran_integer *cfia_;   // (in) single integer array
+
+typedef Fortran_integer *fi_;           // (in/out)  single integer
+typedef const Fortran_integer *cfi_;    // (in) single integer
+
+typedef char *fch_;                // (in/out) single character
+typedef char *cfch_;               // (in) single character
+
+
+
+#ifndef TNT_SUBSCRIPT_TYPE
+#define TNT_SUBSCRIPT_TYPE TNT::Fortran_integer
+#endif
+
+
+#endif
+// FORTRAN_H

intern/iksolver/intern/TNT/fspvec.h

@@ -0,0 +1,200 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+//  Templated sparse vector (Fortran conventions).
+//  Used primarily to interface with Fortran sparse matrix libaries.
+//  (CANNOT BE USED AS AN STL CONTAINER.)
+
+#ifndef FSPVEC_H
+#define FSPVEC_H
+
+#include "tnt.h"
+#include "vec.h"
+#include <cstdlib>
+#include <cassert>
+#include <iostream>
+#include <strstream>
+
+using namespace std;
+
+namespace TNT
+{
+
+template <class T>
+class Fortran_Sparse_Vector 
+{
+
+
+  public:
+
+    typedef Subscript   size_type;
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+
+    Subscript lbound() const { return 1;}
+ 
+  protected:
+    Vector<T>   val_;
+    Vector<Subscript> index_;
+    Subscript dim_;                  // prescribed dimension
+
+
+  public:
+
+    // size and shape information
+
+    Subscript dim() const { return dim_; }
+    Subscript num_nonzeros() const { return val_.dim(); }
+
+    // access
+
+    T& val(Subscript i) { return val_(i); }
+    const T& val(Subscript i) const { return val_(i); }
+
+    Subscript &index(Subscript i) { return index_(i); }
+    const Subscript &index(Subscript i) const { return index_(i); }
+
+    // constructors
+
+    Fortran_Sparse_Vector() : val_(), index_(), dim_(0)  {};
+    Fortran_Sparse_Vector(Subscript N, Subscript nz) : val_(nz), 
+            index_(nz), dim_(N)  {};
+    Fortran_Sparse_Vector(Subscript N, Subscript nz, const T *values,
+        const Subscript *indices): val_(nz, values), index_(nz, indices),
+            dim_(N) {}
+
+    Fortran_Sparse_Vector(const Fortran_Sparse_Vector<T> &S): 
+        val_(S.val_), index_(S.index_), dim_(S.dim_) {}
+
+    // initialize from string, e.g.
+    //
+    //  Fortran_Sparse_Vector<T> A(N, 2, "1.0 2.1", "1 3");
+    //
+    Fortran_Sparse_Vector(Subscript N, Subscript nz, char *v,
+        char *ind) : val_(nz, v), index_(nz, ind), dim_(N) {}
+    
+    // assignments
+
+    Fortran_Sparse_Vector<T> & newsize(Subscript N, Subscript nz)
+    {
+        val_.newsize(nz);
+        index_.newsize(nz);
+        dim_ = N;
+        return *this;
+    }
+
+    Fortran_Sparse_Vector<T> & operator=( const Fortran_Sparse_Vector<T> &A)
+    {
+        val_ = A.val_;
+        index_ = A.index_;
+        dim_ = A.dim_;
+
+        return *this;
+    }
+
+    // methods
+
+
+
+};
+
+
+/* ***************************  I/O  ********************************/
+
+template <class T>
+ostream& operator<<(ostream &s, const Fortran_Sparse_Vector<T> &A)
+{
+    // output format is :   N nz val1 ind1 val2 ind2 ... 
+    Subscript nz=A.num_nonzeros();
+
+    s <<  A.dim() << " " << nz << endl;
+
+    for (Subscript i=1; i<=nz; i++)
+        s   << A.val(i) << "  " << A.index(i) << endl;
+    s << endl;
+
+    return s;
+}
+
+
+template <class T>
+istream& operator>>(istream &s, Fortran_Sparse_Vector<T> &A)
+{
+    // output format is :   N nz val1 ind1 val2 ind2 ... 
+
+    Subscript N;
+    Subscript nz;
+
+    s >> N >> nz;
+
+    A.newsize(N, nz);
+
+    for (Subscript i=1; i<=nz; i++)
+            s >>  A.val(i) >> A.index(i);
+
+
+    return s;
+}
+
+} // namespace TNT
+
+#endif
+// FSPVEC_H

intern/iksolver/intern/TNT/index.h

@@ -0,0 +1,115 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Vector/Matrix/Array Index Module  
+
+#ifndef INDEX_H
+#define INDEX_H
+
+#include "subscript.h"
+
+namespace TNT
+{
+
+class Index1D
+{
+    Subscript lbound_;
+    Subscript ubound_;
+
+    public:
+
+    Subscript lbound() const { return lbound_; }
+    Subscript ubound() const { return ubound_; }
+
+    Index1D(const Index1D &D) : lbound_(D.lbound_), ubound_(D.ubound_) {}
+    Index1D(Subscript i1, Subscript i2) : lbound_(i1), ubound_(i2) {}
+
+    Index1D & operator=(const Index1D &D)
+    {
+        lbound_ = D.lbound_;
+        ubound_ = D.ubound_;
+        return *this;
+    }
+
+};
+
+inline Index1D operator+(const Index1D &D, Subscript i)
+{
+    return Index1D(i+D.lbound(), i+D.ubound());
+}
+
+inline Index1D operator+(Subscript i, const Index1D &D)
+{
+    return Index1D(i+D.lbound(), i+D.ubound());
+}
+
+
+
+inline Index1D operator-(Index1D &D, Subscript i)
+{
+    return Index1D(D.lbound()-i, D.ubound()-i);
+}
+
+inline Index1D operator-(Subscript i, Index1D &D)
+{
+    return Index1D(i-D.lbound(), i-D.ubound());
+}
+
+} // namespace TNT
+
+#endif
+

intern/iksolver/intern/TNT/lapack.h

@@ -0,0 +1,225 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Header file for Fortran Lapack
+
+#ifndef LAPACK_H
+#define LAPACK_H
+
+// This file incomplete and included here to only demonstrate the
+// basic framework for linking with the Fortran Lapack routines.
+
+#include "fortran.h"
+#include "vec.h"
+#include "fmat.h"
+
+
+#define F77_DGESV   dgesv_
+#define F77_DGELS   dgels_
+#define F77_DSYEV   dsyev_
+#define F77_DGEEV   dgeev_
+
+extern "C"
+{
+
+    // linear equations (general) using LU factorizaiton
+    //
+    void F77_DGESV(cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda,
+        fia_ ipiv, fda_ b, cfi_ ldb, fi_ info);
+
+    // solve linear least squares using QR or LU factorization
+    //
+    void F77_DGELS(cfch_ trans, cfi_ M, 
+        cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda, fda_ B, cfi_ ldb, fda_ work, 
+            cfi_ lwork, fi_ info);
+
+    // solve symmetric eigenvalues
+    //
+    void F77_DSYEV( cfch_ jobz, cfch_ uplo, cfi_ N, fda_  A, cfi_ lda, 
+        fda_ W, fda_ work, cfi_ lwork, fi_ info);
+
+    // solve unsymmetric eigenvalues
+    //
+    void F77_DGEEV(cfch_ jobvl, cfch_ jobvr, cfi_ N, fda_ A, cfi_ lda,
+        fda_ wr, fda_ wi, fda_ vl, cfi_ ldvl, fda_ vr, 
+        cfi_ ldvr, fda_ work, cfi_ lwork, fi_ info);
+
+}
+
+// solve linear equations using LU factorization
+
+using namespace TNT;
+
+Vector<double> Lapack_LU_linear_solve(const Fortran_Matrix<double> &A,
+    const Vector<double> &b)
+{
+    const Fortran_integer one=1;
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    Fortran_Matrix<double> Tmp(A);
+    Vector<double> x(b);
+    Vector<Fortran_integer> index(M);
+    Fortran_integer info = 0;
+
+    F77_DGESV(&N, &one, &Tmp(1,1), &M, &index(1), &x(1), &M, &info);    
+
+    if (info != 0) return Vector<double>(0);
+    else
+        return x;
+}
+
+// solve linear least squares problem using QR factorization
+//
+Vector<double> Lapack_LLS_QR_linear_solve(const Fortran_Matrix<double> &A,
+    const Vector<double> &b)
+{
+    const Fortran_integer one=1;
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    Fortran_Matrix<double> Tmp(A);
+    Vector<double> x(b);
+    Fortran_integer info = 0;
+
+    char transp = 'N';
+    Fortran_integer lwork = 5 * (M+N);      // temporary work space
+    Vector<double> work(lwork);
+
+    F77_DGELS(&transp, &M, &N, &one, &Tmp(1,1), &M, &x(1), &M,  &work(1),
+        &lwork, &info); 
+
+    if (info != 0) return Vector<double>(0);
+    else
+        return x;
+}
+
+// *********************** Eigenvalue problems *******************
+
+// solve symmetric eigenvalue problem (eigenvalues only)
+//
+Vector<double> Upper_symmetric_eigenvalue_solve(const Fortran_Matrix<double> &A)
+{
+    char jobz = 'N';
+    char uplo = 'U';
+    Subscript N = A.num_rows();
+
+    assert(N == A.num_cols());
+
+    Vector<double> eigvals(N);
+    Fortran_integer worksize = 3*N;
+    Fortran_integer info = 0;
+    Vector<double> work(worksize);
+    Fortran_Matrix<double> Tmp = A;
+
+    F77_DSYEV(&jobz, &uplo, &N, &Tmp(1,1), &N, eigvals.begin(), work.begin(),
+        &worksize, &info);
+
+    if (info != 0) return Vector<double>();
+    else
+        return eigvals;
+}
+
+
+// solve unsymmetric eigenvalue problems 
+//
+int eigenvalue_solve(const Fortran_Matrix<double> &A, 
+        Vector<double> &wr, Vector<double> &wi)
+{
+    char jobvl = 'N';
+    char jobvr = 'N';
+
+    Fortran_integer N = A.num_rows();
+
+
+    assert(N == A.num_cols());
+    
+    if (N<1) return 1;
+
+    Fortran_Matrix<double> vl(1,N);  /* should be NxN ? **** */
+    Fortran_Matrix<double> vr(1,N);  
+    Fortran_integer one = 1;
+
+    Fortran_integer worksize = 5*N;
+    Fortran_integer info = 0;
+    Vector<double> work(worksize, 0.0);
+    Fortran_Matrix<double> Tmp = A;
+
+    wr.newsize(N);
+    wi.newsize(N);
+
+//  void F77_DGEEV(cfch_ jobvl, cfch_ jobvr, cfi_ N, fda_ A, cfi_ lda,
+//      fda_ wr, fda_ wi, fda_ vl, cfi_ ldvl, fda_ vr, 
+//      cfi_ ldvr, fda_ work, cfi_ lwork, fi_ info);
+
+    F77_DGEEV(&jobvl, &jobvr, &N, &Tmp(1,1), &N, &(wr(1)),
+        &(wi(1)), &(vl(1,1)), &one, &(vr(1,1)), &one,
+        &(work(1)), &worksize, &info);
+
+    return (info==0 ? 0: 1);
+}
+
+
+
+
+
+#endif
+// LAPACK_H
+
+
+
+

intern/iksolver/intern/TNT/lu.h

@@ -0,0 +1,236 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+#ifndef LU_H
+#define LU_H
+
+// Solve system of linear equations Ax = b.
+//
+//  Typical usage:
+//
+//    Matrix(double) A;
+//    Vector(Subscript) ipiv;
+//    Vector(double) b;
+//
+//    1)  LU_Factor(A,ipiv);
+//    2)  LU_Solve(A,ipiv,b);
+//
+//   Now b has the solution x.  Note that both A and b
+//   are overwritten.  If these values need to be preserved, 
+//   one can make temporary copies, as in 
+//
+//    O)  Matrix(double) T = A;
+//    1)  LU_Factor(T,ipiv);
+//    1a) Vector(double) x=b;
+//    2)  LU_Solve(T,ipiv,x);
+//
+//   See details below.
+//
+
+
+// for fabs() 
+//
+#include <cmath>
+
+// right-looking LU factorization algorithm (unblocked)
+//
+//   Factors matrix A into lower and upper  triangular matrices 
+//   (L and U respectively) in solving the linear equation Ax=b.  
+//
+//
+// Args:
+//
+// A        (input/output) Matrix(1:n, 1:n)  In input, matrix to be
+//                  factored.  On output, overwritten with lower and 
+//                  upper triangular factors.
+//
+// indx     (output) Vector(1:n)    Pivot vector. Describes how
+//                  the rows of A were reordered to increase
+//                  numerical stability.
+//
+// Return value:
+//
+// int      (0 if successful, 1 otherwise)
+//
+//
+
+
+namespace TNT
+{
+
+template <class MaTRiX, class VecToRSubscript>
+int LU_factor( MaTRiX &A, VecToRSubscript &indx)
+{
+    assert(A.lbound() == 1);                // currently for 1-offset
+    assert(indx.lbound() == 1);             // vectors and matrices
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    if (M == 0 || N==0) return 0;
+    if (indx.dim() != M)
+        indx.newsize(M);
+
+    Subscript i=0,j=0,k=0;
+    Subscript jp=0;
+
+    typename MaTRiX::element_type t;
+
+    Subscript minMN =  (M < N ? M : N) ;        // min(M,N);
+
+    for (j=1; j<= minMN; j++)
+    {
+
+        // find pivot in column j and  test for singularity.
+
+        jp = j;
+        t = fabs(A(j,j));
+        for (i=j+1; i<=M; i++)
+            if ( fabs(A(i,j)) > t)
+            {
+                jp = i;
+                t = fabs(A(i,j));
+            }
+
+        indx(j) = jp;
+
+        // jp now has the index of maximum element 
+        // of column j, below the diagonal
+
+        if ( A(jp,j) == 0 )                 
+            return 1;       // factorization failed because of zero pivot
+
+
+        if (jp != j)            // swap rows j and jp
+            for (k=1; k<=N; k++)
+            {
+                t = A(j,k);
+                A(j,k) = A(jp,k);
+                A(jp,k) =t;
+            }
+
+        if (j<M)                // compute elements j+1:M of jth column
+        {
+            // note A(j,j), was A(jp,p) previously which was
+            // guarranteed not to be zero (Label #1)
+            //
+            typename MaTRiX::element_type recp =  1.0 / A(j,j);
+
+            for (k=j+1; k<=M; k++)
+                A(k,j) *= recp;
+        }
+
+
+        if (j < minMN)
+        {
+            // rank-1 update to trailing submatrix:   E = E - x*y;
+            //
+            // E is the region A(j+1:M, j+1:N)
+            // x is the column vector A(j+1:M,j)
+            // y is row vector A(j,j+1:N)
+
+            Subscript ii,jj;
+
+            for (ii=j+1; ii<=M; ii++)
+                for (jj=j+1; jj<=N; jj++)
+                    A(ii,jj) -= A(ii,j)*A(j,jj);
+        }
+    }
+
+    return 0;
+}   
+
+
+
+
+template <class MaTRiX, class VecToR, class VecToRSubscripts>
+int LU_solve(const MaTRiX &A, const VecToRSubscripts &indx, VecToR &b)
+{
+    assert(A.lbound() == 1);                // currently for 1-offset
+    assert(indx.lbound() == 1);             // vectors and matrices
+    assert(b.lbound() == 1);
+
+    Subscript i,ii=0,ip,j;
+    Subscript n = b.dim();
+    typename MaTRiX::element_type sum = 0.0;
+
+    for (i=1;i<=n;i++) 
+    {
+        ip=indx(i);
+        sum=b(ip);
+        b(ip)=b(i);
+        if (ii)
+            for (j=ii;j<=i-1;j++) 
+                sum -= A(i,j)*b(j);
+        else if (sum) ii=i;
+            b(i)=sum;
+    }
+    for (i=n;i>=1;i--) 
+    {
+        sum=b(i);
+        for (j=i+1;j<=n;j++) 
+            sum -= A(i,j)*b(j);
+        b(i)=sum/A(i,i);
+    }
+
+    return 0;
+}
+
+} // namespace TNT
+
+#endif
+// LU_H

intern/iksolver/intern/TNT/qr.h

@@ -0,0 +1,261 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+#ifndef QR_H
+#define QR_H
+
+// Classical QR factorization example, based on Stewart[1973].
+//
+//
+// This algorithm computes the factorization of a matrix A
+// into a product of an orthognal matrix (Q) and an upper triangular 
+// matrix (R), such that QR = A.
+//
+// Parameters:
+//
+//  A   (in):       Matrix(1:N, 1:N)
+//
+//  Q   (output):   Matrix(1:N, 1:N), collection of Householder
+//                      column vectors Q1, Q2, ... QN
+//
+//  R   (output):   upper triangular Matrix(1:N, 1:N)
+//
+// Returns:  
+//
+//  0 if successful, 1 if A is detected to be singular
+//
+
+
+#include <cmath>      //for sqrt() & fabs()
+#include "tntmath.h"  // for sign()
+
+// Classical QR factorization, based on Stewart[1973].
+//
+//
+// This algorithm computes the factorization of a matrix A
+// into a product of an orthognal matrix (Q) and an upper triangular 
+// matrix (R), such that QR = A.
+//
+// Parameters:
+//
+//  A   (in/out):  On input, A is square, Matrix(1:N, 1:N), that represents
+//                  the matrix to be factored.
+//
+//                 On output, Q and R is encoded in the same Matrix(1:N,1:N)
+//                 in the following manner:
+//
+//                  R is contained in the upper triangular section of A,
+//                  except that R's main diagonal is in D.  The lower
+//                  triangular section of A represents Q, where each
+//                  column j is the vector  Qj = I - uj*uj'/pi_j.
+//
+//  C  (output):    vector of Pi[j]
+//  D  (output):    main diagonal of R, i.e. D(i) is R(i,i)
+//
+// Returns:  
+//
+//  0 if successful, 1 if A is detected to be singular
+//
+
+namespace TNT
+{
+
+template <class MaTRiX, class Vector>
+int QR_factor(MaTRiX &A, Vector& C, Vector &D)
+{
+    assert(A.lbound() == 1);        // ensure these are all 
+    assert(C.lbound() == 1);        // 1-based arrays and vectors
+    assert(D.lbound() == 1);
+
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols(); 
+
+    assert(M == N);                 // make sure A is square
+
+    Subscript i,j,k;
+    typename MaTRiX::element_type eta, sigma, sum;
+
+    // adjust the shape of C and D, if needed...
+
+    if (N != C.size())  C.newsize(N);
+    if (N != D.size())  D.newsize(N);
+
+    for (k=1; k<N; k++)
+    {
+        // eta = max |M(i,k)|,  for k <= i <= n
+        //
+        eta = 0;
+        for (i=k; i<=N; i++)
+        {
+            double absA = fabs(A(i,k));
+            eta = ( absA >  eta ? absA : eta ); 
+        }
+
+        if (eta == 0)           // matrix is singular
+        {
+            cerr << "QR: k=" << k << "\n";
+            return 1;
+        }
+
+        // form Qk and premiltiply M by it
+        //
+        for(i=k; i<=N; i++)
+            A(i,k)  = A(i,k) / eta;
+
+        sum = 0;
+        for (i=k; i<=N; i++)
+            sum = sum + A(i,k)*A(i,k);
+        sigma = sign(A(k,k)) *  sqrt(sum);
+
+
+        A(k,k) = A(k,k) + sigma;
+        C(k) = sigma * A(k,k);
+        D(k) = -eta * sigma;
+
+        for (j=k+1; j<=N; j++)
+        {
+            sum = 0;
+            for (i=k; i<=N; i++)
+                sum = sum + A(i,k)*A(i,j);
+            sum = sum / C(k);
+
+            for (i=k; i<=N; i++)
+                A(i,j) = A(i,j) - sum * A(i,k);
+        }
+
+        D(N) = A(N,N);
+    }
+
+    return 0;
+}
+
+// modified form of upper triangular solve, except that the main diagonal
+// of R (upper portion of A) is in D.
+//
+template <class MaTRiX, class Vector>
+int R_solve(const MaTRiX &A, /*const*/ Vector &D, Vector &b)
+{
+    assert(A.lbound() == 1);        // ensure these are all 
+    assert(D.lbound() == 1);        // 1-based arrays and vectors
+    assert(b.lbound() == 1);
+
+    Subscript i,j;
+    Subscript N = A.num_rows();
+
+    assert(N == A.num_cols());
+    assert(N == D.dim());
+    assert(N == b.dim());
+
+    typename MaTRiX::element_type sum;
+
+    if (D(N) == 0)
+        return 1;
+
+    b(N) = b(N) / 
+            D(N);
+
+    for (i=N-1; i>=1; i--)
+    {
+        if (D(i) == 0)
+            return 1;
+        sum = 0;
+        for (j=i+1; j<=N; j++)
+            sum = sum + A(i,j)*b(j);
+        b(i) = ( b(i) - sum ) / 
+            D(i);
+    }
+
+    return 0;
+}
+
+
+template <class MaTRiX, class Vector>
+int QR_solve(const MaTRiX &A, const Vector &c, /*const*/ Vector &d, 
+        Vector &b)
+{
+    assert(A.lbound() == 1);        // ensure these are all 
+    assert(c.lbound() == 1);        // 1-based arrays and vectors
+    assert(d.lbound() == 1);
+
+    Subscript N=A.num_rows();
+
+    assert(N == A.num_cols());
+    assert(N == c.dim());
+    assert(N == d.dim());
+    assert(N == b.dim());
+
+    Subscript i,j;
+    typename MaTRiX::element_type sum, tau;
+
+    for (j=1; j<N; j++)
+    {
+        // form Q'*b
+        sum = 0;
+        for (i=j; i<=N; i++)
+            sum = sum + A(i,j)*b(i);
+        if (c(j) == 0)
+            return 1;
+        tau = sum / c(j);
+       for (i=j; i<=N; i++)
+            b(i) = b(i) - tau * A(i,j);
+    }
+    return R_solve(A, d, b);        // solve Rx = Q'b
+}
+
+} // namespace TNT
+
+#endif
+// QR_H

intern/iksolver/intern/TNT/region1d.h

@@ -0,0 +1,403 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+
+#ifndef REGION1D_H
+#define REGION1D_H
+
+
+#include "subscript.h"
+#include "index.h"
+#include <iostream>
+#include <cassert>
+
+namespace TNT
+{
+
+template <class Array1D>
+class const_Region1D;
+
+template <class Array1D>
+class Region1D
+{
+    protected:
+
+        Array1D &  A_;
+        Subscript offset_;          // 0-based
+        Subscript dim_;
+
+        typedef typename Array1D::element_type T;
+
+    public:
+        const Array1D & array()  const { return A_; }
+
+        Subscript offset() const { return offset_;}
+        Subscript dim() const { return dim_; }
+
+        Subscript offset(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(i==TNT_BASE_OFFSET);
+#endif
+            return offset_;
+        }
+
+        Subscript dim(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(i== TNT_BASE_OFFSET);
+#endif
+            return offset_;
+        }
+
+
+        Region1D(Array1D &A, Subscript i1, Subscript i2) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1 );
+            assert(i2 <= A.dim() + (TNT_BASE_OFFSET-1));
+            assert(i1 <= i2);
+#endif
+            offset_ = i1 - TNT_BASE_OFFSET;
+            dim_ = i2-i1 + 1;
+        }
+
+        Region1D(Array1D &A, const Index1D &I) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <=I.lbound());
+            assert(I.ubound() <= A.dim() + (TNT_BASE_OFFSET-1));
+            assert(I.lbound() <= I.ubound());
+#endif
+            offset_ = I.lbound() - TNT_BASE_OFFSET;
+            dim_ = I.ubound() - I.lbound() + 1;
+        }
+
+        Region1D(Region1D<Array1D> &A, Subscript i1, Subscript i2) :
+                A_(A.A_)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1 );
+            assert(i2 <= A.dim() + (TNT_BASE_OFFSET - 1));
+            assert(i1 <= i2);
+#endif
+                    //     (old-offset)        (new-offset)
+                    //
+            offset_ =  (i1 - TNT_BASE_OFFSET) + A.offset_;
+            dim_ = i2-i1 + 1;
+        }
+
+        Region1D<Array1D> operator()(Subscript i1, Subscript i2)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1);
+            assert(i2 <= dim() + (TNT_BASE_OFFSET -1));
+            assert(i1 <= i2);
+#endif
+                    // offset_ is 0-based, so no need for
+                    //  ( - TNT_BASE_OFFSET)
+                    //
+            return Region1D<Array1D>(A_, i1+offset_,
+                    offset_ + i2);
+        }
+
+
+        Region1D<Array1D> operator()(const Index1D &I)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET<=I.lbound());
+            assert(I.ubound() <= dim() + (TNT_BASE_OFFSET-1));
+            assert(I.lbound() <= I.ubound());
+#endif
+            return Region1D<Array1D>(A_, I.lbound()+offset_,
+                offset_ + I.ubound());
+        }
+
+
+
+
+        T & operator()(Subscript i)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i);
+            assert(i <=  dim() + (TNT_BASE_OFFSET-1));
+#endif
+            return A_(i+offset_);
+        }
+
+        const T & operator() (Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i);
+            assert(i <= dim() + (TNT_BASE_OFFSET-1));
+#endif
+            return A_(i+offset_);
+        }
+
+
+        Region1D<Array1D> & operator=(const Region1D<Array1D> &R)
+        {
+            // make sure both sides conform
+            assert(dim() == R.dim());
+
+            Subscript N = dim();
+            Subscript i;
+            Subscript istart = TNT_BASE_OFFSET;
+            Subscript iend = istart + N-1;
+
+            for (i=istart; i<=iend; i++)
+                (*this)(i) = R(i);
+
+            return *this;
+        }
+
+
+
+        Region1D<Array1D> & operator=(const const_Region1D<Array1D> &R)
+        {
+            // make sure both sides conform
+            assert(dim() == R.dim());
+
+            Subscript N = dim();
+            Subscript i;
+            Subscript istart = TNT_BASE_OFFSET;
+            Subscript iend = istart + N-1;
+
+            for (i=istart; i<=iend; i++)
+                (*this)(i) = R(i);
+
+            return *this;
+
+        }
+
+
+        Region1D<Array1D> & operator=(const T& t)
+        {
+            Subscript N=dim();
+            Subscript i;
+            Subscript istart = TNT_BASE_OFFSET;
+            Subscript iend = istart + N-1;
+
+            for (i=istart; i<= iend; i++)
+                (*this)(i) = t;
+
+            return *this;
+
+        }
+
+
+        Region1D<Array1D> & operator=(const Array1D &R)
+        {
+            // make sure both sides conform
+            Subscript N = dim();
+            assert(dim() == R.dim());
+
+            Subscript i;
+            Subscript istart = TNT_BASE_OFFSET;
+            Subscript iend = istart + N-1;
+
+            for (i=istart; i<=iend; i++)
+                (*this)(i) = R(i);
+
+            return *this;
+
+        }
+
+};
+
+template <class Array1D>
+std::ostream& operator<<(std::ostream &s, Region1D<Array1D> &A)
+{
+    Subscript N=A.dim();
+    Subscript istart = TNT_BASE_OFFSET;
+    Subscript iend = N - 1 + TNT_BASE_OFFSET;
+
+    for (Subscript i=istart; i<=iend; i++)
+        s << A(i) << endl;
+
+    return s;
+}
+
+
+/*  ---------  class const_Region1D ------------ */
+
+template <class Array1D>
+class const_Region1D
+{
+    protected:
+
+        const Array1D &  A_;
+        Subscript offset_;          // 0-based
+        Subscript dim_;
+       typedef typename Array1D::element_type T;
+
+    public:
+        const Array1D & array()  const { return A_; }
+
+        Subscript offset() const { return offset_;}
+        Subscript dim() const { return dim_; }
+
+        Subscript offset(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(i==TNT_BASE_OFFSET);
+#endif
+            return offset_;
+        }
+
+        Subscript dim(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(i== TNT_BASE_OFFSET);
+#endif
+            return offset_;
+        }
+
+
+        const_Region1D(const Array1D &A, Subscript i1, Subscript i2) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1 );
+            assert(i2 <= A.dim() + (TNT_BASE_OFFSET-1));
+            assert(i1 <= i2);
+#endif
+            offset_ = i1 - TNT_BASE_OFFSET;
+            dim_ = i2-i1 + 1;
+        }
+
+        const_Region1D(const Array1D &A, const Index1D &I) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <=I.lbound());
+            assert(I.ubound() <= A.dim() + (TNT_BASE_OFFSET-1));
+            assert(I.lbound() <= I.ubound());
+#endif
+            offset_ = I.lbound() - TNT_BASE_OFFSET;
+            dim_ = I.ubound() - I.lbound() + 1;
+        }
+
+        const_Region1D(const_Region1D<Array1D> &A, Subscript i1, Subscript i2) :
+                A_(A.A_)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1 );
+            assert(i2 <= A.dim() + (TNT_BASE_OFFSET - 1));
+            assert(i1 <= i2);
+#endif
+                    //     (old-offset)        (new-offset)
+                    //
+            offset_ =  (i1 - TNT_BASE_OFFSET) + A.offset_;
+            dim_ = i2-i1 + 1;
+        }
+
+        const_Region1D<Array1D> operator()(Subscript i1, Subscript i2)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i1);
+            assert(i2 <= dim() + (TNT_BASE_OFFSET -1));
+            assert(i1 <= i2);
+#endif
+                    // offset_ is 0-based, so no need for
+                    //  ( - TNT_BASE_OFFSET)
+                    //
+            return const_Region1D<Array1D>(A_, i1+offset_,
+                    offset_ + i2);
+        }
+
+
+        const_Region1D<Array1D> operator()(const Index1D &I)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET<=I.lbound());
+            assert(I.ubound() <= dim() + (TNT_BASE_OFFSET-1));
+            assert(I.lbound() <= I.ubound());
+#endif
+            return const_Region1D<Array1D>(A_, I.lbound()+offset_,
+                offset_ + I.ubound());
+        }
+
+
+        const T & operator() (Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert(TNT_BASE_OFFSET <= i);
+            assert(i <= dim() + (TNT_BASE_OFFSET-1));
+#endif
+            return A_(i+offset_);
+        }
+
+
+
+
+};
+
+template <class Array1D>
+std::ostream& operator<<(std::ostream &s, const_Region1D<Array1D> &A)
+{
+    Subscript N=A.dim();
+
+    for (Subscript i=1; i<=N; i++)
+        s << A(i) << endl;
+
+    return s;
+}
+
+
+} // namespace TNT
+
+#endif
+// const_Region1D_H

intern/iksolver/intern/TNT/region2d.h

@@ -0,0 +1,499 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+// 2D Regions for arrays and matrices
+
+#ifndef REGION2D_H
+#define REGION2D_H
+
+#include "index.h"
+#include <iostream>
+#include <cassert>
+
+namespace TNT
+{
+
+template <class Array2D>
+class const_Region2D;
+
+
+template <class Array2D>
+class Region2D
+{
+    protected:
+
+        Array2D &  A_;
+        Subscript offset_[2];       // 0-offset internally
+        Subscript dim_[2];
+
+    public:
+        typedef typename Array2D::value_type T;
+        typedef Subscript   size_type;
+        typedef         T   value_type;
+        typedef         T   element_type;
+        typedef         T*  pointer;
+        typedef         T*  iterator;
+        typedef         T&  reference;
+        typedef const   T*  const_iterator;
+        typedef const   T&  const_reference;
+
+        Array2D & array() { return A_; }
+        const Array2D & array()  const { return A_; }
+        Subscript lbound() const { return A_.lbound(); }
+        Subscript num_rows() const { return dim_[0]; }
+        Subscript num_cols() const { return dim_[1]; }
+        Subscript offset(Subscript i) const                 // 1-offset
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( A_.lbound() <= i);
+            assert( i<= dim_[0] + A_.lbound()-1);
+#endif
+            return offset_[i-A_.lbound()];
+        }
+
+        Subscript dim(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( A_.lbound() <= i);
+            assert( i<= dim_[0] + A_.lbound()-1);
+#endif
+            return dim_[i-A_.lbound()];
+        }
+
+
+
+        Region2D(Array2D &A, Subscript i1, Subscript i2, Subscript j1,
+                Subscript j2) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( A.lbound() <= i1);
+            assert( i2<= A.dim(A.lbound()) + A.lbound()-1);
+            assert( A.lbound() <= j1);
+            assert( j2<= A.dim(A.lbound()+1) + A.lbound()-1 );
+#endif
+
+
+            offset_[0] = i1-A.lbound();
+            offset_[1] = j1-A.lbound();
+            dim_[0] = i2-i1+1;
+            dim_[1] = j2-j1+1;
+        }
+
+        Region2D(Array2D &A, const Index1D &I, const Index1D &J) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( I.lbound() <= I.ubound() );
+            assert( J.lbound() <= J.ubound() );
+            assert( A.lbound() <= I.lbound());
+            assert( I.ubound()<= A.dim(A.lbound()) + A.lbound()-1);
+            assert( A.lbound() <= J.lbound());
+            assert( J.ubound() <= A.dim(A.lbound()+1) + A.lbound()-1 );
+#endif
+
+            offset_[0] = I.lbound()-A.lbound();
+            offset_[1] = J.lbound()-A.lbound();
+            dim_[0] = I.ubound() - I.lbound() + 1;
+            dim_[1] = J.ubound() - J.lbound() + 1;
+        }
+
+        Region2D(Region2D<Array2D> &A, Subscript i1, Subscript i2,
+            Subscript j1, Subscript j2) : A_(A.A_)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( A.lbound() <= i1);
+            assert( i2<= A.dim(A.lbound()) + A.lbound()-1);
+            assert( A.lbound() <= j1);
+            assert( j2<= A.dim(A.lbound()+1) + A.lbound()-1 );
+#endif
+            offset_[0] = (i1 - A.lbound()) + A.offset_[0];
+            offset_[1] = (j1 - A.lbound()) + A.offset_[1];
+            dim_[0] = i2-i1 + 1;
+            dim_[1] = j2-j1+1;
+        }
+
+        Region2D<Array2D> operator()(Subscript i1, Subscript i2,
+                Subscript j1, Subscript j2)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( A_.lbound() <= i1);
+            assert( i2<= dim_[0] + A_.lbound()-1);
+            assert( A_.lbound() <= j1);
+            assert( j2<= dim_[1] + A_.lbound()-1 );
+#endif
+            return Region2D<Array2D>(A_, 
+                    i1+offset_[0], offset_[0] + i2, 
+                    j1+offset_[1], offset_[1] + j2);
+        }
+
+
+        Region2D<Array2D> operator()(const Index1D &I,
+                const Index1D &J)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( I.lbound() <= I.ubound() );
+            assert( J.lbound() <= J.ubound() );
+            assert( A_.lbound() <= I.lbound());
+            assert( I.ubound()<= dim_[0] + A_.lbound()-1);
+            assert( A_.lbound() <= J.lbound());
+            assert( J.ubound() <= dim_[1] + A_.lbound()-1 );
+#endif
+
+            return Region2D<Array2D>(A_, I.lbound()+offset_[0],
+                offset_[0] + I.ubound(), offset_[1]+J.lbound(),
+                offset_[1] + J.ubound());
+        }
+
+        inline T & operator()(Subscript i, Subscript j)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( A_.lbound() <= i);
+            assert( i<= dim_[0] + A_.lbound()-1);
+            assert( A_.lbound() <= j);
+            assert( j<= dim_[1] + A_.lbound()-1 );
+#endif
+            return A_(i+offset_[0], j+offset_[1]);
+        }
+
+        inline const T & operator() (Subscript i, Subscript j) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( A_.lbound() <= i);
+            assert( i<= dim_[0] + A_.lbound()-1);
+            assert( A_.lbound() <= j);
+            assert( j<= dim_[1] + A_.lbound()-1 );
+#endif
+            return A_(i+offset_[0], j+offset_[1]);
+        }
+
+
+        Region2D<Array2D> & operator=(const Region2D<Array2D> &R)
+        {
+            Subscript M = num_rows(); 
+            Subscript N = num_cols();
+
+            // make sure both sides conform
+            assert(M == R.num_rows());
+            assert(N == R.num_cols());
+
+
+            Subscript start = R.lbound();
+            Subscript Mend =  start + M - 1;
+            Subscript Nend =  start + N - 1;
+            for (Subscript i=start; i<=Mend; i++)
+              for (Subscript j=start; j<=Nend; j++)
+                (*this)(i,j) = R(i,j);
+
+            return *this;
+        }
+
+        Region2D<Array2D> & operator=(const const_Region2D<Array2D> &R)
+        {
+            Subscript M = num_rows(); 
+            Subscript N = num_cols();
+
+            // make sure both sides conform
+            assert(M == R.num_rows());
+            assert(N == R.num_cols());
+
+
+            Subscript start = R.lbound();
+            Subscript Mend =  start + M - 1;
+            Subscript Nend =  start + N - 1;
+            for (Subscript i=start; i<=Mend; i++)
+              for (Subscript j=start; j<=Nend; j++)
+                (*this)(i,j) = R(i,j);
+
+            return *this;
+        }
+
+        Region2D<Array2D> & operator=(const Array2D &R)
+        {
+            Subscript M = num_rows(); 
+            Subscript N = num_cols();
+
+            // make sure both sides conform
+            assert(M == R.num_rows());
+            assert(N == R.num_cols());
+
+
+            Subscript start = R.lbound();
+            Subscript Mend =  start + M - 1;
+            Subscript Nend =  start + N - 1;
+            for (Subscript i=start; i<=Mend; i++)
+              for (Subscript j=start; j<=Nend; j++)
+                (*this)(i,j) = R(i,j);
+
+            return *this;
+        }
+
+        Region2D<Array2D> & operator=(const  T &scalar)
+        {
+            Subscript start = lbound();
+            Subscript Mend = lbound() + num_rows() - 1;
+            Subscript Nend = lbound() + num_cols() - 1;
+
+
+            for (Subscript i=start; i<=Mend; i++)
+              for (Subscript j=start; j<=Nend; j++)
+                (*this)(i,j) = scalar;
+
+            return *this;
+        }
+
+};
+
+//************************
+
+template <class Array2D>
+class const_Region2D
+{
+    protected:
+
+        const Array2D &  A_;
+        Subscript offset_[2];       // 0-offset internally
+        Subscript dim_[2];
+
+    public:
+    typedef typename Array2D::value_type T;
+        typedef         T   value_type;
+        typedef         T   element_type;
+        typedef const   T*  const_iterator;
+        typedef const   T&  const_reference;
+
+        const Array2D & array() const { return A_; }
+        Subscript lbound() const { return A_.lbound(); }
+        Subscript num_rows() const { return dim_[0]; }
+        Subscript num_cols() const { return dim_[1]; }
+        Subscript offset(Subscript i) const                 // 1-offset
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( TNT_BASE_OFFSET <= i);
+            assert( i<= dim_[0] + TNT_BASE_OFFSET-1);
+#endif
+            return offset_[i-TNT_BASE_OFFSET];
+        }
+
+        Subscript dim(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( TNT_BASE_OFFSET <= i);
+            assert( i<= dim_[0] + TNT_BASE_OFFSET-1);
+#endif
+            return dim_[i-TNT_BASE_OFFSET];
+        }
+
+
+        const_Region2D(const Array2D &A, Subscript i1, Subscript i2, 
+                Subscript j1, Subscript j2) : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( TNT_BASE_OFFSET <= i1);
+            assert( i2<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= j1);
+            assert( j2<= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 );
+#endif
+
+            offset_[0] = i1-TNT_BASE_OFFSET;
+            offset_[1] = j1-TNT_BASE_OFFSET;
+            dim_[0] = i2-i1+1;
+            dim_[1] = j2-j1+1;
+        }
+
+        const_Region2D(const Array2D &A, const Index1D &I, const Index1D &J) 
+                : A_(A)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( I.lbound() <= I.ubound() );
+            assert( J.lbound() <= J.ubound() );
+            assert( TNT_BASE_OFFSET <= I.lbound());
+            assert( I.ubound()<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= J.lbound());
+            assert( J.ubound() <= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 );
+#endif
+
+            offset_[0] = I.lbound()-TNT_BASE_OFFSET;
+            offset_[1] = J.lbound()-TNT_BASE_OFFSET;
+            dim_[0] = I.ubound() - I.lbound() + 1;
+            dim_[1] = J.ubound() - J.lbound() + 1;
+        }
+
+
+        const_Region2D(const_Region2D<Array2D> &A, Subscript i1, 
+                Subscript i2,
+            Subscript j1, Subscript j2) : A_(A.A_)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( TNT_BASE_OFFSET <= i1);
+            assert( i2<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= j1);
+            assert( j2<= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 );
+#endif
+            offset_[0] = (i1 - TNT_BASE_OFFSET) + A.offset_[0];
+            offset_[1] = (j1 - TNT_BASE_OFFSET) + A.offset_[1];
+            dim_[0] = i2-i1 + 1;
+            dim_[1] = j2-j1+1;
+        }
+
+        const_Region2D<Array2D> operator()(Subscript i1, Subscript i2,
+                Subscript j1, Subscript j2)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( i1 <= i2 );
+            assert( j1 <= j2);
+            assert( TNT_BASE_OFFSET <= i1);
+            assert( i2<= dim_[0] + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= j1);
+            assert( j2<= dim_[0] + TNT_BASE_OFFSET-1 );
+#endif
+            return const_Region2D<Array2D>(A_, 
+                    i1+offset_[0], offset_[0] + i2, 
+                    j1+offset_[1], offset_[1] + j2);
+        }
+
+
+        const_Region2D<Array2D> operator()(const Index1D &I,
+                const Index1D &J)
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( I.lbound() <= I.ubound() );
+            assert( J.lbound() <= J.ubound() );
+            assert( TNT_BASE_OFFSET <= I.lbound());
+            assert( I.ubound()<= dim_[0] + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= J.lbound());
+            assert( J.ubound() <= dim_[1] + TNT_BASE_OFFSET-1 );
+#endif
+
+            return const_Region2D<Array2D>(A_, I.lbound()+offset_[0],
+                offset_[0] + I.ubound(), offset_[1]+J.lbound(),
+                offset_[1] + J.ubound());
+        }
+
+
+        inline const T & operator() (Subscript i, Subscript j) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( TNT_BASE_OFFSET <= i);
+            assert( i<= dim_[0] + TNT_BASE_OFFSET-1);
+            assert( TNT_BASE_OFFSET <= j);
+            assert( j<= dim_[1] + TNT_BASE_OFFSET-1 );
+#endif
+            return A_(i+offset_[0], j+offset_[1]);
+        }
+
+};
+
+
+//  ************** std::ostream algorithms *******************************
+
+template <class Array2D>
+std::ostream& operator<<(std::ostream &s, const const_Region2D<Array2D> &A)
+{
+    Subscript start = A.lbound();
+    Subscript Mend=A.lbound()+ A.num_rows() - 1;
+    Subscript Nend=A.lbound() + A.num_cols() - 1;
+
+
+    s << A.num_rows() << "  " << A.num_cols() << "\n";
+    for (Subscript i=start; i<=Mend; i++)
+    {
+        for (Subscript j=start; j<=Nend; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << "\n";
+    }
+
+
+    return s;
+}
+
+template <class Array2D>
+std::ostream& operator<<(std::ostream &s, const Region2D<Array2D> &A)
+{
+    Subscript start = A.lbound();
+    Subscript Mend=A.lbound()+ A.num_rows() - 1;
+    Subscript Nend=A.lbound() + A.num_cols() - 1;
+
+
+    s << A.num_rows() << "  " << A.num_cols() << "\n";
+    for (Subscript i=start; i<=Mend; i++)
+    {
+        for (Subscript j=start; j<=Nend; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << "\n";
+    }
+
+
+    return s;
+
+}
+
+} // namespace TNT
+
+#endif
+// REGION2D_H

intern/iksolver/intern/TNT/stopwatch.h

@@ -0,0 +1,115 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+#ifndef STPWATCH_H
+#define STPWATCH_H
+
+// for clock() and CLOCKS_PER_SEC
+#include <ctime>
+
+namespace TNT
+{
+
+/*  Simple stopwatch object:
+
+        void    start()     : start timing
+        double  stop()      : stop timing
+        void    reset()     : set elapsed time to 0.0
+        double  read()      : read elapsed time (in seconds)
+
+*/
+
+inline double seconds(void)
+{
+    static const double secs_per_tick = 1.0 / CLOCKS_PER_SEC;
+    return ( (double) clock() ) * secs_per_tick;
+}
+
+
+class stopwatch {
+    private:
+        int running;
+        double last_time;
+        double total;
+
+    public:
+        stopwatch() : running(0), last_time(0.0), total(0.0) {}
+        void reset() { running = 0; last_time = 0.0; total=0.0; }
+        void start() { if (!running) { last_time = seconds(); running = 1;}}
+        double stop()  { if (running) 
+                            {
+                                total += seconds() - last_time; 
+                                running = 0;
+                             }
+                          return total; 
+                        }
+        double read()   {  if (running) 
+                            {
+                                total+= seconds() - last_time;
+                                last_time = seconds();
+                            }
+                           return total;
+                        }       
+                            
+};
+
+} // namespace TNT
+
+#endif
+    
+
+            

intern/iksolver/intern/TNT/subscript.h

@@ -0,0 +1,90 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+#ifndef SUBSCRPT_H
+#define SUBSCRPT_H
+
+
+//---------------------------------------------------------------------
+// This definition describes the default TNT data type used for
+// indexing into TNT matrices and vectors.  The data type should
+// be wide enough to index into large arrays.  It defaults to an
+// "int", but can be overriden at compile time redefining TNT_SUBSCRIPT_TYPE,
+// e.g.
+// 
+//      g++ -DTNT_SUBSCRIPT_TYPE='unsigned int'  ...
+//
+//---------------------------------------------------------------------
+//
+
+#ifndef TNT_SUBSCRIPT_TYPE
+#define TNT_SUBSCRIPT_TYPE int
+#endif
+
+namespace TNT
+{
+    typedef TNT_SUBSCRIPT_TYPE Subscript;
+}
+
+
+// () indexing in TNT means 1-offset, i.e. x(1) and A(1,1) are the
+// first elements.  This offset is left as a macro for future
+// purposes, but should not be changed in the current release.
+//
+//
+#define TNT_BASE_OFFSET (1)
+
+#endif

intern/iksolver/intern/TNT/svd.h

@@ -0,0 +1,507 @@
+/**
+ * $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 *****
+ */
+
+#ifndef SVD_H
+
+#define SVD_H
+
+// Compute the Single Value Decomposition of an arbitrary matrix A
+// That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 
+// ,W a diagonal matrix and V an orthogonal square matrix s.t. 
+// A = U.W.Vt. From this decomposition it is trivial to compute the 
+// inverse of A as Ainv = V.Winv.tranpose(U).
+// work_space is a temporary vector used by this class to compute
+// intermediate values during the computation of the SVD. This should
+// be of length a.num_cols. This is not checked
+
+#include "tntmath.h"
+
+namespace TNT
+{
+
+
+template <class MaTRiX, class VecToR >
+void SVD(MaTRiX &a, VecToR &w,  MaTRiX &v, VecToR &work_space) {
+
+		int n = a.num_cols();
+	int m = a.num_rows();
+
+	int flag,i,its,j,jj,k,l(0),nm(0);
+	typename MaTRiX::value_type c,f,h,x,y,z;
+	typename MaTRiX::value_type anorm(0),g(0),scale(0);
+    typename MaTRiX::value_type s(0);
+
+    work_space.newsize(n);
+
+	for (i=1;i<=n;i++) {
+		l=i+1;
+		work_space(i)=scale*g;
+
+		g = (typename MaTRiX::value_type)0;
+
+		s = (typename  MaTRiX::value_type)0;
+        scale = (typename  MaTRiX::value_type)0;
+
+		if (i <= m) {
+			for (k=i;k<=m;k++) scale += TNT::abs(a(k,i));
+			if (scale > (typename  MaTRiX::value_type)0) {
+				for (k=i;k<=m;k++) {
+					a(k,i) /= scale;
+					s += a(k,i)*a(k,i);
+				}
+				f=a(i,i);
+				g = -TNT::sign(sqrt(s),f);
+				h=f*g-s;
+				a(i,i)=f-g;
+				if (i != n) {
+					for (j=l;j<=n;j++) {
+                        s = (typename  MaTRiX::value_type)0;
+						for (k=i;k<=m;k++) s += a(k,i)*a(k,j);
+						f=s/h;
+						for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+					}
+				}
+				for (k=i;k<=m;k++) a(k,i) *= scale;
+			}
+		}
+		w(i)=scale*g;
+        g = (typename  MaTRiX::value_type)0;
+        s = (typename  MaTRiX::value_type)0;
+        scale = (typename  MaTRiX::value_type)0;
+		if (i <= m && i != n) {
+			for (k=l;k<=n;k++) scale += TNT::abs(a(i,k));
+			if (scale > (typename  MaTRiX::value_type)0) {
+				for (k=l;k<=n;k++) {
+					a(i,k) /= scale;
+					s += a(i,k)*a(i,k);
+				}
+				f=a(i,l);
+				g = -TNT::sign(sqrt(s),f);
+				h=f*g-s;
+				a(i,l)=f-g;
+				for (k=l;k<=n;k++) work_space(k)=a(i,k)/h;
+				if (i != m) {
+					for (j=l;j<=m;j++) {
+                        s = (typename  MaTRiX::value_type)0;
+						for (k=l;k<=n;k++) s += a(j,k)*a(i,k);
+						for (k=l;k<=n;k++) a(j,k) += s*work_space(k);
+					}
+				}
+				for (k=l;k<=n;k++) a(i,k) *= scale;
+			}
+		}
+		anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
+	}
+	for (i=n;i>=1;i--) {
+		if (i < n) {
+			if (g != (typename  MaTRiX::value_type)0) {
+				for (j=l;j<=n;j++)
+					v(j,i)=(a(i,j)/a(i,l))/g;
+				for (j=l;j<=n;j++) {
+                    s = (typename  MaTRiX::value_type)0;
+					for (k=l;k<=n;k++) s += a(i,k)*v(k,j);
+					for (k=l;k<=n;k++) v(k,j) += s*v(k,i);
+				}
+			}
+			for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename  MaTRiX::value_type)0;
+		}
+		v(i,i)= (typename  MaTRiX::value_type)1;
+		g=work_space(i);
+		l=i;
+	}
+	for (i=n;i>=1;i--) {
+		l=i+1;
+		g=w(i);
+		if (i < n) {
+			for (j=l;j<=n;j++) a(i,j)= (typename  MaTRiX::value_type)0;
+		}
+		if (g !=  (typename  MaTRiX::value_type)0) {
+			g= ((typename  MaTRiX::value_type)1)/g;
+			if (i != n) {
+				for (j=l;j<=n;j++) {
+                    s =  (typename  MaTRiX::value_type)0;
+					for (k=l;k<=m;k++) s += a(k,i)*a(k,j);
+					f=(s/a(i,i))*g;
+					for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+				}
+			}
+			for (j=i;j<=m;j++) a(j,i) *= g;
+		} else {
+			for (j=i;j<=m;j++) a(j,i)= (typename  MaTRiX::value_type)0;
+		}
+		++a(i,i);
+	}
+	for (k=n;k>=1;k--) {
+		for (its=1;its<=30;its++) {
+			flag=1;
+			for (l=k;l>=1;l--) {
+				nm=l-1;
+				if (TNT::abs(work_space(l))+anorm == anorm) {
+					flag=0;
+					break;
+				}
+				if (TNT::abs(w(nm))+anorm == anorm) break;
+			}
+			if (flag) {
+				c= (typename  MaTRiX::value_type)0;
+				s= (typename  MaTRiX::value_type)1;
+				for (i=l;i<=k;i++) {
+					f=s*work_space(i);
+					if (TNT::abs(f)+anorm != anorm) {
+						g=w(i);
+						h= (typename  MaTRiX::value_type)TNT::pythag(float(f),float(g));
+						w(i)=h;
+						h= ((typename  MaTRiX::value_type)1)/h;
+						c=g*h;
+						s=(-f*h);
+						for (j=1;j<=m;j++) {
+							y=a(j,nm);
+							z=a(j,i);
+							a(j,nm)=y*c+z*s;
+							a(j,i)=z*c-y*s;
+						}
+					}
+				}
+			}
+			z=w(k);
+			if (l == k) {
+				if (z <  (typename  MaTRiX::value_type)0) {
+					w(k) = -z;
+					for (j=1;j<=n;j++) v(j,k)=(-v(j,k));
+				}
+				break;
+			}
+
+
+#if 1
+			if (its == 30)
+			{
+                                TNTException an_exception;
+                                an_exception.i = 0;
+                                throw an_exception;
+
+                                return ;
+				assert(false);
+			}
+#endif
+			x=w(l);
+			nm=k-1;
+			y=w(nm);
+			g=work_space(nm);
+			h=work_space(k);
+			f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename  MaTRiX::value_type)2)*h*y);
+			g=(typename  MaTRiX::value_type)TNT::pythag(float(f), float(1));
+			f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x;
+                        c =  (typename  MaTRiX::value_type)1;
+                        s =  (typename  MaTRiX::value_type)1;
+			for (j=l;j<=nm;j++) {
+				i=j+1;
+				g=work_space(i);
+				y=w(i);
+				h=s*g;
+				g=c*g;
+				z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
+				work_space(j)=z;
+				c=f/z;
+				s=h/z;
+				f=x*c+g*s;
+				g=g*c-x*s;
+				h=y*s;
+				y=y*c;
+				for (jj=1;jj<=n;jj++) {
+					x=v(jj,j);
+					z=v(jj,i);
+					v(jj,j)=x*c+z*s;
+					v(jj,i)=z*c-x*s;
+				}
+				z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
+				w(j)=z;
+				if (z !=  (typename  MaTRiX::value_type)0) {
+					z= ((typename  MaTRiX::value_type)1)/z;
+					c=f*z;
+					s=h*z;
+				}
+				f=(c*g)+(s*y);
+				x=(c*y)-(s*g);
+				for (jj=1;jj<=m;jj++) {
+					y=a(jj,j);
+					z=a(jj,i);
+					a(jj,j)=y*c+z*s;
+					a(jj,i)=z*c-y*s;
+				}
+			}
+			work_space(l)= (typename  MaTRiX::value_type)0;
+			work_space(k)=f;
+			w(k)=x;
+		}
+	}
+};
+
+
+
+// A is replaced by the column orthogonal matrix U 
+
+
+template <class MaTRiX, class VecToR >
+void SVD_a( MaTRiX &a, VecToR &w,  MaTRiX &v) {
+
+	int n = a.num_cols();
+	int m = a.num_rows();
+
+	int flag,i,its,j,jj,k,l,nm;
+	typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
+
+	VecToR work_space;
+	work_space.newsize(n);
+
+	g = scale = anorm = 0.0;
+	
+	for (i=1;i <=n;i++) {
+		l = i+1;
+		work_space(i) = scale*g;
+		g = s=scale=0.0;
+
+		if (i <= m) {
+			for(k=i; k<=m; k++) scale += abs(a(k,i));
+
+			if (scale) {
+				for (k = i; k <=m ; k++) {
+					a(k,i) /= scale;
+					s += a(k,i)*a(k,i);
+				}
+				f = a(i,i);
+				g = -sign(sqrt(s),f);
+				h = f*g -s;
+				a(i,i) = f-g;
+	
+				for (j = l; j <=n; j++) {
+					for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j);
+					f = s/h;
+					for (k = i; k <= m; k++) a(k,j) += f*a(k,i);
+				}
+				for (k = i; k <=m;k++) a(k,i) *= scale;
+			}
+		}
+
+		w(i) = scale*g;
+		g = s = scale = 0.0;
+
+		if (i <=m && i != n) {
+			for (k = l; k <=n;k++) scale += abs(a(i,k));
+			if (scale) {
+				for(k = l;k <=n;k++) {
+					a(i,k) /= scale;
+					s += a(i,k) * a(i,k);
+				}
+
+				f = a(i,l);
+				g = -sign(sqrt(s),f);
+				h= f*g -s;
+				a(i,l) = f-g;
+				for(k=l;k<=n;k++) work_space(k) = a(i,k)/h;
+				for (j=l;j<=m;j++) {
+					for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k);
+					for(k=l;k<=n;k++) a(j,k) += s*work_space(k);
+				}
+				for(k=l;k<=n;k++) a(i,k)*=scale;
+			}
+		}
+		anorm = max(anorm,(abs(w(i)) + abs(work_space(i))));
+	}
+	for (i=n;i>=1;i--) {
+		if (i <n) {
+			if (g) {
+				for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g;
+				for(j=l;j<=n;j++) {
+					for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j);
+					for(k=l; k<=n;k++) v(k,j) +=s*v(k,i);
+				}
+			}
+			for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0;
+		}
+		v(i,i) = 1.0;
+		g = work_space(i);
+		l = i;
+	}
+
+	for (i = min(m,n);i>=1;i--) {
+		l = i+1;
+		g = w(i);
+		for (j=l;j <=n;j++) a(i,j) = 0.0;
+		if (g) {
+			g = 1.0/g;
+			for (j=l;j<=n;j++) {
+				for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j);
+				f = (s/a(i,i))*g;
+				for (k=i;k<=m;k++) a(k,j) += f*a(k,i);	
+			}
+			for (j=i;j<=m;j++) a(j,i)*=g;
+		} else {
+			for (j=i;j<=m;j++) a(j,i) = 0.0;
+		}
+		++a(i,i);
+	}
+
+	for (k=n;k>=1;k--) {
+		for (its=1;its<=30;its++) {
+			flag=1;
+			for(l=k;l>=1;l--) {
+				nm = l-1;
+				if (abs(work_space(l)) + anorm == anorm) {
+					flag = 0;
+					break;
+				}
+				if (abs(w(nm)) + anorm == anorm) break;
+			}
+			if (flag) {
+				c = 0.0;
+				s = 1.0;
+				for (i=l;i<=k;i++) {
+					f = s*work_space(i);
+					work_space(i) = c*work_space(i);
+					if (abs(f) +anorm == anorm) break;
+					g = w(i);
+					h  = pythag(f,g);
+					w(i) = h;
+					h = 1.0/h;
+					c = g*h;
+					s = -f*h;
+					for (j=1;j<=m;j++) {
+						y= a(j,nm);
+						z=a(j,i);
+						a(j,nm) = y*c + z*s;
+						a(j,i) = z*c - y*s;
+					}
+				}
+			}
+			z=w(k);
+			if (l==k) {
+				if (z <0.0) {
+					w(k) = -z;
+					for (j=1;j<=n;j++) v(j,k) = -v(j,k);
+				}
+				break;
+			}
+
+			if (its == 30) assert(false);
+
+			x=w(l);
+			nm=k-1;
+			y=w(nm);
+			g=work_space(nm);
+			h=work_space(k);
+			
+			f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
+			g = pythag(f,1.0);
+			f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x;
+			c=s=1.0;
+
+			for (j=l;j<=nm;j++) {
+				i=j+1;
+				g = work_space(i);
+				y=w(i);
+				h=s*g;
+				g=c*g;
+				z=pythag(f,h);
+				work_space(j) = z;
+				c=f/z;
+				s=h/z;
+				f=x*c + g*s;
+				g= g*c - x*s;
+				h=y*s;
+				y*=c;
+				for(jj=1;jj<=n;jj++) {
+					x=v(jj,j);
+					z=v(jj,i);
+					v(jj,j) = x*c + z*s;
+					v(jj,i) = z*c- x*s;
+				}
+				z=pythag(f,h);
+				w(j)=z;
+				if(z) {
+					z = 1.0/z;
+					c=f*z;
+					s=h*z;
+				}
+				f=c*g + s*y;
+				x= c*y-s*g;
+			
+				for(jj=1;jj<=m;jj++) {
+					y=a(jj,j);
+					z=a(jj,i);
+					a(jj,j) = y*c+z*s;
+					a(jj,i) = z*c - y*s;
+				}
+			}
+
+			work_space(l) = 0.0;
+			work_space(k) = f;
+			w(k) = x;
+		}
+	}
+}
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+
+				
+
+	
+
+
+
+
+
+
+
+
+
+
+
+
+					
+
+
+
+
+
+
+	
+

intern/iksolver/intern/TNT/tnt.h

@@ -0,0 +1,127 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+#ifndef TNT_H
+#define TNT_H
+
+//---------------------------------------------------------------------
+//  tnt.h       TNT general header file.  Defines default types
+//              and conventions.
+//---------------------------------------------------------------------
+
+//---------------------------------------------------------------------
+//  Include current version 
+//---------------------------------------------------------------------
+#include "version.h"
+
+//---------------------------------------------------------------------
+// Define the data type used for matrix and vector Subscripts.
+// This will default to "int", but it can be overriden at compile time,
+// e.g.
+// 
+//      g++ -DTNT_SUBSCRIPT_TYPE='unsinged long' ...
+//
+// See subscript.h for details.
+//---------------------------------------------------------------------
+
+#include "subscript.h"
+
+
+
+//---------------------------------------------------------------------
+// Define this macro if you want  TNT to ensure all refernces
+// are within the bounds of the array.  This encurs a run-time
+// overhead, of course, but is recommended while developing
+// code.  It can be turned off for production runs.
+// 
+//       #define TNT_BOUNDS_CHECK
+//---------------------------------------------------------------------
+//
+#define TNT_BOUNDS_CHECK
+#ifdef TNT_NO_BOUNDS_CHECK
+#undef TNT_BOUNDS_CHECK
+#endif
+
+//---------------------------------------------------------------------
+// Define this macro if you want to utilize matrix and vector
+// regions.  This is typically on, but you can save some
+// compilation time by turning it off.  If you do this and
+// attempt to use regions you will get an error message.
+//
+//       #define TNT_USE_REGIONS
+//---------------------------------------------------------------------
+//
+#define TNT_USE_REGIONS
+
+//---------------------------------------------------------------------
+//  
+//---------------------------------------------------------------------
+// if your system doesn't have abs() min(), and max() uncoment the following
+//---------------------------------------------------------------------
+//
+// 
+//#define __NEED_ABS_MIN_MAX_
+
+#include "tntmath.h"
+
+
+
+
+
+
+
+#endif
+// TNT_H

intern/iksolver/intern/TNT/tntmath.h

@@ -0,0 +1,177 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Header file for scalar math functions
+
+#ifndef TNTMATH_H
+#define TNTMATH_H
+
+// conventional functions required by several matrix algorithms
+
+
+
+namespace TNT 
+{
+
+struct TNTException {
+        int i;
+};
+
+
+inline double abs(double t)
+{
+    return ( t > 0 ? t : -t);
+}
+
+inline double min(double a, double b)
+{
+    return (a < b ? a : b);
+}
+
+inline double max(double a, double b)
+{
+    return (a > b ? a : b);
+}
+
+inline float abs(float t)
+{
+    return ( t > 0 ? t : -t);
+}
+
+inline float min(float a, float b)
+{
+    return (a < b ? a : b);
+}
+
+inline int min(int a,int b) {
+    return (a < b ? a : b);
+}
+
+
+inline float max(float a, float b)
+{
+    return (a > b ? a : b);
+}
+
+inline double sign(double a)
+{
+    return (a > 0 ? 1.0 : -1.0);
+}
+
+inline double sign(double a,double b) {
+	return (b >= 0.0 ? TNT::abs(a) : -TNT::abs(a));
+}
+
+inline float sign(float a,float b) {
+	return (b >= 0.0f ? TNT::abs(a) : -TNT::abs(a));
+}
+
+inline float sign(float a)
+{
+    return (a > 0.0 ? 1.0f : -1.0f);
+}
+
+inline float pythag(float a, float b)
+{
+	float absa,absb;
+	absa = abs(a);
+	absb = abs(b);
+
+	if (absa > absb) {
+		float sqr = absb/absa;
+		sqr *= sqr;
+		return absa * float(sqrt(1 + sqr));
+	} else {
+		if (absb > float(0)) {
+			float sqr = absa/absb;
+			sqr *= sqr;
+			return absb * float(sqrt(1 + sqr));
+		} else {
+			return float(0);
+		}
+	}
+}
+
+inline double pythag(double a, double b)
+{
+	double absa,absb;
+	absa = abs(a);
+	absb = abs(b);
+
+	if (absa > absb) {
+		double sqr = absb/absa;
+		sqr *= sqr;
+		return absa * double(sqrt(1 + sqr));
+	} else {
+
+		if (absb > double(0)) {	
+			double sqr = absa/absb;
+			sqr *= sqr;
+			return absb * double(sqrt(1 + sqr));
+		} else {
+			return double(0);
+		}
+	}
+}
+
+
+} /* namespace TNT */
+
+
+#endif
+/* TNTMATH_H */
+

intern/iksolver/intern/TNT/tntreqs.h

@@ -0,0 +1,102 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// The requirements for a bare-bones vector class:
+//
+//
+//   o) must have 0-based [] indexing for const and
+//          non-const objects  (i.e. operator[] defined)
+//
+//   o) must have size() method to denote the number of
+//          elements
+//   o) must clean up after itself when destructed
+//          (i.e. no memory leaks)
+//
+//   -) must have begin() and end() methods  (The begin()
+//          method is necessary, because relying on 
+//          &v_[0] may not work on a empty vector (i.e. v_ is NULL.)
+//
+//   o) must be templated
+//   o) must have X::value_type defined to be the types of elements
+//   o) must have X::X(const &x) copy constructor (by *value*)
+//   o) must have X::X(int N) constructor to N-length vector
+//          (NOTE: this constructor need *NOT* initalize elements)
+//
+//   -) must have X::X(int N, T scalar) constructor to initalize
+//          elements to value of "scalar".
+//
+//       ( removed, because valarray<> class uses (scalar, N)  rather
+//              than (N, scalar) )
+//   -) must have X::X(int N, const T* scalars) constructor to copy from
+//              any C linear array
+//
+//         ( removed, because of same reverse order of valarray<> )
+//
+//   o) must have assignment A=B, by value
+//
+//  NOTE: this class is *NOT* meant to be derived from,
+//  so its methods (particularly indexing) need not be
+//  declared virtual.
+//
+//
+//  Some things it *DOES NOT* need to do are
+//
+//  o) bounds checking
+//  o) array referencing (e.g. reference counting)
+//  o) support () indexing
+//  o) I/O 
+//

intern/iksolver/intern/TNT/transv.h

@@ -0,0 +1,192 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Matrix Transpose Views
+
+#ifndef TRANSV_H
+#define TRANSV_H
+
+#include <iostream>
+#include <cassert>
+#include "vec.h"
+
+namespace TNT
+{
+
+template <class Array2D>
+class Transpose_View
+{
+    protected:
+
+        const Array2D &  A_;
+
+    public:
+
+        typedef typename Array2D::element_type T;
+        typedef         T   value_type;
+        typedef         T   element_type;
+        typedef         T*  pointer;
+        typedef         T*  iterator;
+        typedef         T&  reference;
+        typedef const   T*  const_iterator;
+        typedef const   T&  const_reference;
+
+
+        const Array2D & array()  const { return A_; }
+        Subscript num_rows() const { return A_.num_cols();}
+        Subscript num_cols() const { return A_.num_rows();}
+        Subscript lbound() const { return A_.lbound(); }
+        Subscript dim(Subscript i) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+            assert( A_.lbound() <= i);
+            assert( i<= A_.lbound()+1);
+#endif
+            if (i== A_.lbound())
+                return num_rows();
+            else
+                return num_cols();
+        }
+
+
+        Transpose_View(const Transpose_View<Array2D> &A) : A_(A.A_) {};
+        Transpose_View(const Array2D &A) : A_(A) {};
+
+
+        inline const typename Array2D::element_type & operator()(
+            Subscript i, Subscript j) const
+        {
+#ifdef TNT_BOUNDS_CHECK
+        assert(lbound()<=i);
+        assert(i<=A_.num_cols() + lbound() - 1);
+        assert(lbound()<=j);
+        assert(j<=A_.num_rows() + lbound() - 1);
+#endif
+
+            return A_(j,i);
+        }
+
+
+};
+
+template <class Matrix>
+Transpose_View<Matrix> Transpose_view(const Matrix &A)
+{
+    return Transpose_View<Matrix>(A);
+}
+
+template <class Matrix, class T>
+Vector<T> matmult(
+    const Transpose_View<Matrix> & A, 
+    const Vector<T> &B)
+{
+    Subscript  M = A.num_rows();
+    Subscript  N = A.num_cols();
+
+    assert(B.dim() == N);
+
+    Vector<T> x(N);
+
+    Subscript i, j;
+    T tmp = 0;
+
+    for (i=1; i<=M; i++)
+    {
+        tmp = 0;
+        for (j=1; j<=N; j++)
+            tmp += A(i,j) * B(j);
+        x(i) = tmp;
+    }
+
+    return x;
+}
+
+template <class Matrix, class T>
+inline Vector<T> operator*(const Transpose_View<Matrix> & A, const Vector<T> &B)
+{
+    return matmult(A,B);
+}
+
+
+template <class Matrix>
+std::ostream& operator<<(std::ostream &s, const Transpose_View<Matrix> &A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    Subscript start = A.lbound();
+    Subscript Mend = M + A.lbound() - 1;
+    Subscript Nend = N + A.lbound() - 1;
+
+    s << M << "  " << N << endl;
+    for (Subscript i=start; i<=Mend; i++)
+    {
+        for (Subscript j=start; j<=Nend; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << endl;
+    }
+
+
+    return s;
+}
+
+} // namespace TNT
+
+#endif
+// TRANSV_H

intern/iksolver/intern/TNT/triang.h

@@ -0,0 +1,670 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Triangular Matrices (Views and Adpators)
+
+#ifndef TRIANG_H
+#define TRIANG_H
+
+// default to use lower-triangular portions of arrays
+// for symmetric matrices.
+
+namespace TNT
+{
+
+template <class MaTRiX>
+class LowerTriangularView
+{
+    protected:
+
+
+        const MaTRiX  &A_;
+        const typename MaTRiX::element_type zero_;
+
+    public:
+
+
+    typedef typename MaTRiX::const_reference const_reference;
+    typedef const typename MaTRiX::element_type element_type;
+    typedef const typename MaTRiX::element_type value_type;
+    typedef element_type T;
+
+    Subscript dim(Subscript d) const {  return A_.dim(d); }
+    Subscript lbound() const { return A_.lbound(); }
+    Subscript num_rows() const { return A_.num_rows(); }
+    Subscript num_cols() const { return A_.num_cols(); }
+    
+    
+    // constructors
+
+    LowerTriangularView(/*const*/ MaTRiX &A) : A_(A),  zero_(0) {}
+
+
+    inline const_reference get(Subscript i, Subscript j) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(lbound()<=i);
+        assert(i<=A_.num_rows() + lbound() - 1);
+        assert(lbound()<=j);
+        assert(j<=A_.num_cols() + lbound() - 1);
+#endif
+        if (i<j) 
+            return zero_;
+        else
+            return A_(i,j);
+    }
+
+
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(lbound()<=i);
+        assert(i<=A_.num_rows() + lbound() - 1);
+        assert(lbound()<=j);
+        assert(j<=A_.num_cols() + lbound() - 1);
+#endif
+        if (i<j) 
+            return zero_;
+        else
+            return A_(i,j);
+    }
+
+#ifdef TNT_USE_REGIONS 
+
+    typedef const_Region2D< LowerTriangularView<MaTRiX> > 
+                    const_Region;
+
+    const_Region operator()(/*const*/ Index1D &I,
+            /*const*/ Index1D &J) const
+    {
+        return const_Region(*this, I, J);
+    }
+
+    const_Region operator()(Subscript i1, Subscript i2,
+            Subscript j1, Subscript j2) const
+    {
+        return const_Region(*this, i1, i2, j1, j2);
+    }
+
+
+
+#endif
+// TNT_USE_REGIONS
+
+};
+
+
+/* *********** Lower_triangular_view() algorithms ****************** */
+
+template <class MaTRiX, class VecToR>
+VecToR matmult(/*const*/ LowerTriangularView<MaTRiX> &A, VecToR &x)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(N == x.dim());
+
+    Subscript i, j;
+    typename MaTRiX::element_type sum=0.0;
+    VecToR result(M);
+
+    Subscript start = A.lbound();
+    Subscript Mend = M + A.lbound() -1 ;
+
+    for (i=start; i<=Mend; i++)
+    {
+        sum = 0.0;
+        for (j=start; j<=i; j++)
+            sum = sum + A(i,j)*x(j);
+        result(i) = sum;
+    }
+
+    return result;
+}
+
+template <class MaTRiX, class VecToR>
+inline VecToR operator*(/*const*/ LowerTriangularView<MaTRiX> &A, VecToR &x)
+{
+    return matmult(A,x);
+}
+
+template <class MaTRiX>
+class UnitLowerTriangularView
+{
+    protected:
+
+        const MaTRiX  &A_;
+        const typename MaTRiX::element_type zero;
+        const typename MaTRiX::element_type one;
+
+    public:
+
+    typedef typename MaTRiX::const_reference const_reference;
+    typedef typename MaTRiX::element_type element_type;
+    typedef typename MaTRiX::element_type value_type;
+    typedef element_type T;
+
+    Subscript lbound() const { return 1; }
+    Subscript dim(Subscript d) const {  return A_.dim(d); }
+    Subscript num_rows() const { return A_.num_rows(); }
+    Subscript num_cols() const { return A_.num_cols(); }
+
+    
+    // constructors
+
+    UnitLowerTriangularView(/*const*/ MaTRiX &A) : A_(A), zero(0), one(1) {}
+
+
+    inline const_reference get(Subscript i, Subscript j) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=A_.dim(1));
+        assert(1<=j);
+        assert(j<=A_.dim(2));
+        assert(0<=i && i<A_.dim(0) && 0<=j && j<A_.dim(1));
+#endif
+        if (i>j)
+            return A_(i,j);
+        else if (i==j)
+            return one;
+        else 
+            return zero;
+    }
+
+
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=A_.dim(1));
+        assert(1<=j);
+        assert(j<=A_.dim(2));
+#endif
+        if (i>j)
+            return A_(i,j);
+        else if (i==j)
+            return one;
+        else 
+            return zero;
+    }
+
+
+#ifdef TNT_USE_REGIONS 
+  // These are the "index-aware" features
+
+    typedef const_Region2D< UnitLowerTriangularView<MaTRiX> > 
+                    const_Region;
+
+    const_Region operator()(/*const*/ Index1D &I,
+            /*const*/ Index1D &J) const
+    {
+        return const_Region(*this, I, J);
+    }
+
+    const_Region operator()(Subscript i1, Subscript i2,
+            Subscript j1, Subscript j2) const
+    {
+        return const_Region(*this, i1, i2, j1, j2);
+    }
+#endif
+// TNT_USE_REGIONS
+};
+
+template <class MaTRiX>
+LowerTriangularView<MaTRiX> Lower_triangular_view(
+    /*const*/ MaTRiX &A)
+{
+    return LowerTriangularView<MaTRiX>(A);
+}
+
+
+template <class MaTRiX>
+UnitLowerTriangularView<MaTRiX> Unit_lower_triangular_view(
+    /*const*/ MaTRiX &A)
+{
+    return UnitLowerTriangularView<MaTRiX>(A);
+}
+
+template <class MaTRiX, class VecToR>
+VecToR matmult(/*const*/ UnitLowerTriangularView<MaTRiX> &A, VecToR &x)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(N == x.dim());
+
+    Subscript i, j;
+    typename MaTRiX::element_type sum=0.0;
+    VecToR result(M);
+
+    Subscript start = A.lbound();
+    Subscript Mend = M + A.lbound() -1 ;
+
+    for (i=start; i<=Mend; i++)
+    {
+        sum = 0.0;
+        for (j=start; j<i; j++)
+            sum = sum + A(i,j)*x(j);
+        result(i) = sum + x(i);
+    }
+
+    return result;
+}
+
+template <class MaTRiX, class VecToR>
+inline VecToR operator*(/*const*/ UnitLowerTriangularView<MaTRiX> &A, VecToR &x)
+{
+    return matmult(A,x);
+}
+
+
+//********************** Algorithms *************************************
+
+
+
+template <class MaTRiX>
+std::ostream& operator<<(std::ostream &s, const LowerTriangularView<MaTRiX>&A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << endl;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript j=1; j<=N; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << endl;
+    }
+
+
+    return s;
+}
+
+template <class MaTRiX>
+std::ostream& operator<<(std::ostream &s, 
+    const UnitLowerTriangularView<MaTRiX>&A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << endl;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript j=1; j<=N; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << endl;
+    }
+
+
+    return s;
+}
+
+
+
+// ******************* Upper Triangular Section **************************
+
+template <class MaTRiX>
+class UpperTriangularView
+{
+    protected:
+
+
+        /*const*/ MaTRiX  &A_;
+        /*const*/ typename MaTRiX::element_type zero_;
+
+    public:
+
+
+    typedef typename MaTRiX::const_reference const_reference;
+    typedef /*const*/ typename MaTRiX::element_type element_type;
+    typedef /*const*/ typename MaTRiX::element_type value_type;
+    typedef element_type T;
+
+    Subscript dim(Subscript d) const {  return A_.dim(d); }
+    Subscript lbound() const { return A_.lbound(); }
+    Subscript num_rows() const { return A_.num_rows(); }
+    Subscript num_cols() const { return A_.num_cols(); }
+    
+    
+    // constructors
+
+    UpperTriangularView(/*const*/ MaTRiX &A) : A_(A),  zero_(0) {}
+
+
+    inline const_reference get(Subscript i, Subscript j) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(lbound()<=i);
+        assert(i<=A_.num_rows() + lbound() - 1);
+        assert(lbound()<=j);
+        assert(j<=A_.num_cols() + lbound() - 1);
+#endif
+        if (i>j) 
+            return zero_;
+        else
+            return A_(i,j);
+    }
+
+
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(lbound()<=i);
+        assert(i<=A_.num_rows() + lbound() - 1);
+        assert(lbound()<=j);
+        assert(j<=A_.num_cols() + lbound() - 1);
+#endif
+        if (i>j) 
+            return zero_;
+        else
+            return A_(i,j);
+    }
+
+#ifdef TNT_USE_REGIONS 
+
+    typedef const_Region2D< UpperTriangularView<MaTRiX> > 
+                    const_Region;
+
+    const_Region operator()(const Index1D &I,
+            const Index1D &J) const
+    {
+        return const_Region(*this, I, J);
+    }
+
+    const_Region operator()(Subscript i1, Subscript i2,
+            Subscript j1, Subscript j2) const
+    {
+        return const_Region(*this, i1, i2, j1, j2);
+    }
+
+
+
+#endif
+// TNT_USE_REGIONS
+
+};
+
+
+/* *********** Upper_triangular_view() algorithms ****************** */
+
+template <class MaTRiX, class VecToR>
+VecToR matmult(/*const*/ UpperTriangularView<MaTRiX> &A, VecToR &x)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(N == x.dim());
+
+    Subscript i, j;
+    typename VecToR::element_type sum=0.0;
+    VecToR result(M);
+
+    Subscript start = A.lbound();
+    Subscript Mend = M + A.lbound() -1 ;
+
+    for (i=start; i<=Mend; i++)
+    {
+        sum = 0.0;
+        for (j=i; j<=N; j++)
+            sum = sum + A(i,j)*x(j);
+        result(i) = sum;
+    }
+
+    return result;
+}
+
+template <class MaTRiX, class VecToR>
+inline VecToR operator*(/*const*/ UpperTriangularView<MaTRiX> &A, VecToR &x)
+{
+    return matmult(A,x);
+}
+
+template <class MaTRiX>
+class UnitUpperTriangularView
+{
+    protected:
+
+        const MaTRiX  &A_;
+        const typename MaTRiX::element_type zero;
+        const typename MaTRiX::element_type one;
+
+    public:
+
+    typedef typename MaTRiX::const_reference const_reference;
+    typedef typename MaTRiX::element_type element_type;
+    typedef typename MaTRiX::element_type value_type;
+    typedef element_type T;
+
+    Subscript lbound() const { return 1; }
+    Subscript dim(Subscript d) const {  return A_.dim(d); }
+    Subscript num_rows() const { return A_.num_rows(); }
+    Subscript num_cols() const { return A_.num_cols(); }
+
+    
+    // constructors
+
+    UnitUpperTriangularView(/*const*/ MaTRiX &A) : A_(A), zero(0), one(1) {}
+
+
+    inline const_reference get(Subscript i, Subscript j) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=A_.dim(1));
+        assert(1<=j);
+        assert(j<=A_.dim(2));
+        assert(0<=i && i<A_.dim(0) && 0<=j && j<A_.dim(1));
+#endif
+        if (i<j)
+            return A_(i,j);
+        else if (i==j)
+            return one;
+        else 
+            return zero;
+    }
+
+
+    inline const_reference operator() (Subscript i, Subscript j) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=A_.dim(1));
+        assert(1<=j);
+        assert(j<=A_.dim(2));
+#endif
+        if (i<j)
+            return A_(i,j);
+        else if (i==j)
+            return one;
+        else 
+            return zero;
+    }
+
+
+#ifdef TNT_USE_REGIONS 
+  // These are the "index-aware" features
+
+    typedef const_Region2D< UnitUpperTriangularView<MaTRiX> > 
+                    const_Region;
+
+    const_Region operator()(const Index1D &I,
+            const Index1D &J) const
+    {
+        return const_Region(*this, I, J);
+    }
+
+    const_Region operator()(Subscript i1, Subscript i2,
+            Subscript j1, Subscript j2) const
+    {
+        return const_Region(*this, i1, i2, j1, j2);
+    }
+#endif
+// TNT_USE_REGIONS
+};
+
+template <class MaTRiX>
+UpperTriangularView<MaTRiX> Upper_triangular_view(
+    /*const*/ MaTRiX &A)
+{
+    return UpperTriangularView<MaTRiX>(A);
+}
+
+
+template <class MaTRiX>
+UnitUpperTriangularView<MaTRiX> Unit_upper_triangular_view(
+    /*const*/ MaTRiX &A)
+{
+    return UnitUpperTriangularView<MaTRiX>(A);
+}
+
+template <class MaTRiX, class VecToR>
+VecToR matmult(/*const*/ UnitUpperTriangularView<MaTRiX> &A, VecToR &x)
+{
+    Subscript M = A.num_rows();
+    Subscript N = A.num_cols();
+
+    assert(N == x.dim());
+
+    Subscript i, j;
+    typename VecToR::element_type sum=0.0;
+    VecToR result(M);
+
+    Subscript start = A.lbound();
+    Subscript Mend = M + A.lbound() -1 ;
+
+    for (i=start; i<=Mend; i++)
+    {
+        sum = x(i);
+        for (j=i+1; j<=N; j++)
+            sum = sum + A(i,j)*x(j);
+        result(i) = sum + x(i);
+    }
+
+    return result;
+}
+
+template <class MaTRiX, class VecToR>
+inline VecToR operator*(/*const*/ UnitUpperTriangularView<MaTRiX> &A, VecToR &x)
+{
+    return matmult(A,x);
+}
+
+
+//********************** Algorithms *************************************
+
+
+
+template <class MaTRiX>
+std::ostream& operator<<(std::ostream &s, 
+    /*const*/ UpperTriangularView<MaTRiX>&A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << endl;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript j=1; j<=N; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << endl;
+    }
+
+
+    return s;
+}
+
+template <class MaTRiX>
+std::ostream& operator<<(std::ostream &s, 
+        /*const*/ UnitUpperTriangularView<MaTRiX>&A)
+{
+    Subscript M=A.num_rows();
+    Subscript N=A.num_cols();
+
+    s << M << " " << N << endl;
+
+    for (Subscript i=1; i<=M; i++)
+    {
+        for (Subscript j=1; j<=N; j++)
+        {
+            s << A(i,j) << " ";
+        }
+        s << endl;
+    }
+
+
+    return s;
+}
+
+} // namespace TNT
+
+
+
+
+
+#endif 
+//TRIANG_H
+

intern/iksolver/intern/TNT/trisolve.h

@@ -0,0 +1,216 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+// Triangular Solves
+
+#ifndef TRISLV_H
+#define TRISLV_H
+
+
+#include "triang.h"
+
+namespace TNT
+{
+
+template <class MaTriX, class VecToR>
+VecToR Lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
+{
+    Subscript N = A.num_rows();
+
+    // make sure matrix sizes agree; A must be square
+
+    assert(A.num_cols() == N);
+    assert(b.dim() == N);
+
+    VecToR x(N);
+
+    Subscript i;
+    for (i=1; i<=N; i++)
+    {
+        typename MaTriX::element_type tmp=0;
+
+        for (Subscript j=1; j<i; j++)
+                tmp = tmp + A(i,j)*x(j);
+
+        x(i) =  (b(i) - tmp)/ A(i,i);
+    }
+
+    return x;
+}
+
+
+template <class MaTriX, class VecToR>
+VecToR Unit_lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
+{
+    Subscript N = A.num_rows();
+
+    // make sure matrix sizes agree; A must be square
+
+    assert(A.num_cols() == N);
+    assert(b.dim() == N);
+
+    VecToR x(N);
+
+    Subscript i;
+    for (i=1; i<=N; i++)
+    {
+
+        typename MaTriX::element_type tmp=0;
+
+        for (Subscript j=1; j<i; j++)
+                tmp = tmp + A(i,j)*x(j);
+
+        x(i) =  b(i) - tmp;
+    }
+
+    return x;
+}
+
+
+template <class MaTriX, class VecToR>
+VecToR linear_solve(/*const*/ LowerTriangularView<MaTriX> &A, 
+            /*const*/ VecToR &b)
+{
+    return Lower_triangular_solve(A, b);
+}
+    
+template <class MaTriX, class VecToR>
+VecToR linear_solve(/*const*/ UnitLowerTriangularView<MaTriX> &A, 
+        /*const*/ VecToR &b)
+{
+    return Unit_lower_triangular_solve(A, b);
+}
+    
+
+
+//********************** Upper triangular section ****************
+
+template <class MaTriX, class VecToR>
+VecToR Upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
+{
+    Subscript N = A.num_rows();
+
+    // make sure matrix sizes agree; A must be square
+
+    assert(A.num_cols() == N);
+    assert(b.dim() == N);
+
+    VecToR x(N);
+
+    Subscript i;
+    for (i=N; i>=1; i--)
+    {
+
+        typename MaTriX::element_type tmp=0;
+
+        for (Subscript j=i+1; j<=N; j++)
+                tmp = tmp + A(i,j)*x(j);
+
+        x(i) =  (b(i) - tmp)/ A(i,i);
+    }
+
+    return x;
+}
+
+
+template <class MaTriX, class VecToR>
+VecToR Unit_upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
+{
+    Subscript N = A.num_rows();
+
+    // make sure matrix sizes agree; A must be square
+
+    assert(A.num_cols() == N);
+    assert(b.dim() == N);
+
+    VecToR x(N);
+
+    Subscript i;
+    for (i=N; i>=1; i--)
+    {
+
+        typename MaTriX::element_type tmp=0;
+
+        for (Subscript j=i+1; j<i; j++)
+                tmp = tmp + A(i,j)*x(j);
+
+        x(i) =  b(i) - tmp;
+    }
+
+    return x;
+}
+
+
+template <class MaTriX, class VecToR>
+VecToR linear_solve(/*const*/ UpperTriangularView<MaTriX> &A, 
+        /*const*/ VecToR &b)
+{
+    return Upper_triangular_solve(A, b);
+}
+    
+template <class MaTriX, class VecToR>
+VecToR linear_solve(/*const*/ UnitUpperTriangularView<MaTriX> &A, 
+    /*const*/ VecToR &b)
+{
+    return Unit_upper_triangular_solve(A, b);
+}
+
+
+} // namespace TNT
+
+#endif
+// TRISLV_H

intern/iksolver/intern/TNT/vec.h

@@ -0,0 +1,535 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+// Basic TNT  numerical vector (0-based [i] AND 1-based (i) indexing )
+//
+
+#ifndef VEC_H
+#define VEC_H
+
+#include "subscript.h"
+#include <stdlib.h>
+#include <assert.h>
+#include <iostream>
+#include <strstream>
+
+namespace TNT
+{
+
+template <class T>
+class Vector 
+{
+
+
+  public:
+
+    typedef Subscript   size_type;
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+
+    Subscript lbound() const { return 1;}
+ 
+  protected:
+    T* v_;                  
+    T* vm1_;        // pointer adjustment for optimzied 1-offset indexing
+    Subscript n_;
+
+    // internal helper function to create the array
+    // of row pointers
+
+    void initialize(Subscript N)
+    {
+        // adjust pointers so that they are 1-offset:
+        // v_[] is the internal contiguous array, it is still 0-offset
+        //
+        assert(v_ == NULL);
+        v_ = new T[N];
+        assert(v_  != NULL);
+        vm1_ = v_-1;
+        n_ = N;
+    }
+   
+    void copy(const T*  v)
+    {
+        Subscript N = n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = v[i];
+            v_[i+1] = v[i+1];
+            v_[i+2] = v[i+2];
+            v_[i+3] = v[i+3];
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = v[i];
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = v[i];
+#endif      
+    }
+
+    void set(const T& val)
+    {
+        Subscript N = n_;
+        Subscript i;
+
+#ifdef TNT_UNROLL_LOOPS
+        Subscript Nmod4 = N & 3;
+        Subscript N4 = N - Nmod4;
+
+        for (i=0; i<N4; i+=4)
+        {
+            v_[i] = val;
+            v_[i+1] = val;
+            v_[i+2] = val;
+            v_[i+3] = val; 
+        }
+
+        for (i=N4; i< N; i++)
+            v_[i] = val;
+#else
+
+        for (i=0; i< N; i++)
+            v_[i] = val;
+        
+#endif      
+    }
+    
+
+
+    void destroy()
+    {     
+        /* do nothing, if no memory has been previously allocated */
+        if (v_ == NULL) return ;
+
+        /* if we are here, then matrix was previously allocated */
+        delete [] (v_);     
+
+        v_ = NULL;
+        vm1_ = NULL;
+    }
+
+
+  public:
+
+    // access
+
+    iterator begin() { return v_;}
+    iterator end()   { return v_ + n_; }
+    const iterator begin() const { return v_;}
+    const iterator end() const  { return v_ + n_; }
+
+    // destructor
+
+    ~Vector() 
+    {
+        destroy();
+    }
+
+    // constructors
+
+    Vector() : v_(0), vm1_(0), n_(0)  {};
+
+    Vector(const Vector<T> &A) : v_(0), vm1_(0), n_(0)
+    {
+        initialize(A.n_);
+        copy(A.v_);
+    }
+
+    Vector(Subscript N, const T& value = T()) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        set(value);
+    }
+
+    Vector(Subscript N, const T* v) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        copy(v);
+    }
+
+    Vector(Subscript N, char *s) :  v_(0), vm1_(0), n_(0)
+    {
+        initialize(N);
+        std::istrstream ins(s);
+
+        Subscript i;
+
+        for (i=0; i<N; i++)
+                ins >> v_[i];
+    }
+
+
+    // methods
+    // 
+    Vector<T>& newsize(Subscript N)
+    {
+        if (n_ == N) return *this;
+
+        destroy();
+        initialize(N);
+
+        return *this;
+    }
+
+
+    // assignments
+    //
+    Vector<T>& operator=(const Vector<T> &A)
+    {
+        if (v_ == A.v_)
+            return *this;
+
+        if (n_ == A.n_)         // no need to re-alloc
+            copy(A.v_);
+
+        else
+        {
+            destroy();
+            initialize(A.n_);
+            copy(A.v_);
+        }
+
+        return *this;
+    }
+        
+    Vector<T>& operator=(const T& scalar)
+    { 
+        set(scalar);  
+        return *this;
+    }
+
+    inline Subscript dim() const 
+    {
+        return  n_; 
+    }
+
+    inline Subscript size() const 
+    {
+        return  n_; 
+    }
+
+
+    inline reference operator()(Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= n_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+    inline const_reference operator() (Subscript i) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i <= n_) ;
+#endif
+        return vm1_[i]; 
+    }
+
+    inline reference operator[](Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i < n_) ;
+#endif
+        return v_[i]; 
+    }
+
+    inline const_reference operator[](Subscript i) const
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i) ;
+        assert(i < n_) ;
+#endif
+        return v_[i]; 
+    }
+
+
+
+};
+
+
+/* ***************************  I/O  ********************************/
+
+template <class T>
+std::ostream& operator<<(std::ostream &s, const Vector<T> &A)
+{
+    Subscript N=A.dim();
+
+    s <<  N << endl;
+
+    for (Subscript i=0; i<N; i++)
+        s   << A[i] << " " << endl;
+    s << endl;
+
+    return s;
+}
+
+template <class T>
+std::istream & operator>>(std::istream &s, Vector<T> &A)
+{
+
+    Subscript N;
+
+    s >> N;
+
+    if ( !(N == A.size() ))
+    {
+        A.newsize(N);
+    }
+
+
+    for (Subscript i=0; i<N; i++)
+            s >>  A[i];
+
+
+    return s;
+}
+
+// *******************[ basic matrix algorithms ]***************************
+
+
+template <class T>
+Vector<T> operator+(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] + B[i];
+
+    return tmp;
+}
+
+template <class T>
+Vector<T> operator-(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] - B[i];
+
+    return tmp;
+}
+
+template <class T>
+Vector<T> operator*(const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    Subscript N = A.dim();
+
+    assert(N==B.dim());
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] * B[i];
+
+    return tmp;
+}
+
+template <class T>
+Vector<T> operator*(const Vector<T> &A, 
+    const T &B)
+{
+    Subscript N = A.dim();
+
+    Vector<T> tmp(N);
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            tmp[i] = A[i] * B;
+
+    return tmp;
+}
+
+
+template <class T>
+T dot_prod(const Vector<T> &A, const Vector<T> &B)
+{
+    Subscript N = A.dim();
+    assert(N == B.dim());
+
+    Subscript i;
+    T sum = 0;
+
+    for (i=0; i<N; i++)
+        sum += A[i] * B[i];
+
+    return sum;
+}
+
+// inplace versions of the above template functions
+
+// A = A + B
+
+template <class T>
+void vectoradd(
+	Vector<T> &A, 
+    const Vector<T> &B)
+{
+    const Subscript N = A.dim();
+    assert(N==B.dim());
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            A[i] += B[i];
+}
+
+// same with seperate output vector
+
+template <class T>
+void vectoradd(
+	Vector<T> &C,
+	const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    const Subscript N = A.dim();
+    assert(N==B.dim());
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            C[i] = A[i] + B[i];
+}
+
+// A = A - B
+
+template <class T>
+void vectorsub(
+	Vector<T> &A, 
+    const Vector<T> &B)
+{
+    const Subscript N = A.dim();
+    assert(N==B.dim());
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            A[i] -= B[i];
+}
+
+template <class T>
+void vectorsub(
+	Vector<T> &C,
+	const Vector<T> &A, 
+    const Vector<T> &B)
+{
+    const Subscript N = A.dim();
+    assert(N==B.dim());
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            C[i] = A[i] - B[i];
+}
+
+template <class T>
+void vectorscale(
+	Vector<T> &C,
+	const Vector<T> &A, 
+    const T &B)
+{
+    const Subscript N = A.dim();
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            C[i] = A[i] * B;
+}
+
+template <class T>
+void vectorscale(
+	Vector<T> &A, 
+    const T &B)
+{
+    const Subscript N = A.dim();
+    Subscript i;
+
+    for (i=0; i<N; i++)
+            A[i] *= B;
+}
+
+
+
+
+
+}   /* namespace TNT */
+
+#endif
+// VEC_H

intern/iksolver/intern/TNT/vecadaptor.h

@@ -0,0 +1,321 @@
+/**
+ * $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 *****
+ */
+
+/*
+
+*
+* Template Numerical Toolkit (TNT): Linear Algebra Module
+*
+* Mathematical and Computational Sciences Division
+* National Institute of Technology,
+* Gaithersburg, MD USA
+*
+*
+* This software was developed at the National Institute of Standards and
+* Technology (NIST) by employees of the Federal Government in the course
+* of their official duties. Pursuant to title 17 Section 105 of the
+* United States Code, this software is not subject to copyright protection
+* and is in the public domain.  The Template Numerical Toolkit (TNT) is
+* an experimental system.  NIST assumes no responsibility whatsoever for
+* its use by other parties, and makes no guarantees, expressed or implied,
+* about its quality, reliability, or any other characteristic.
+*
+* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+* see http://math.nist.gov/tnt for latest updates.
+*
+*/
+
+
+
+#ifndef VECADAPTOR_H
+#define VECADAPTOR_H
+
+#include <cstdlib>
+#include <iostream>
+#include <strstream>
+#include <cassert>
+
+#include "subscript.h"
+
+#ifdef TNT_USE_REGIONS
+#include "region1d.h"
+#endif
+
+namespace TNT
+{
+
+//  see "tntreq.h" for TNT requirements for underlying vector
+//  class.  This need NOT be the STL vector<> class, but a subset
+//  that provides minimal services.
+//
+//  This is a container adaptor that provides the following services.
+//
+//      o)  adds 1-offset operator() access ([] is always 0 offset)
+//      o)  adds TNT_BOUNDS_CHECK to () and []
+//      o)  adds initialization from strings, e.g.  "1.0 2.0 3.0";
+//      o)  adds newsize(N) function (does not preserve previous values)
+//      o)  adds dim() and dim(1)
+//      o)  adds free() function to release memory used by vector
+//      o)  adds regions, e.g. A(Index(1,10)) = ... 
+//      o)  add getVector() method to return adapted container
+//      o)  adds simple I/O for ostreams
+
+template <class BBVec>
+class Vector_Adaptor
+{
+
+  public:
+    typedef   typename BBVec::value_type T;
+    typedef         T   value_type;
+    typedef         T   element_type;
+    typedef         T*  pointer;
+    typedef         T*  iterator;
+    typedef         T&  reference;
+    typedef const   T*  const_iterator;
+    typedef const   T&  const_reference;
+    
+    Subscript lbound() const { return 1; }
+
+  protected:
+    BBVec v_;
+    T* vm1_;
+
+  public:
+
+    Subscript size() const { return v_.size(); }
+
+    // These were removed so that the ANSI C++ valarray class
+    // would work as a possible storage container.
+    //
+    //
+    //iterator begin() { return v_.begin();}
+    //iterator begin() { return &v_[0];}
+    //
+    //iterator end()   { return v_.end(); }
+    //iterator end()   { return &v_[0] + v_.size(); }
+    //
+    //const_iterator begin() const { return v_.begin();}
+    //const_iterator begin() const { return &v_[0];}
+    //
+    //const_iterator end()  const { return v_.end(); }
+    //const_iterator end()  const { return &v_[0] + v_.size(); }
+
+    BBVec& getVector() { return v_; }
+    Subscript dim() const { return v_.size(); }
+    Subscript dim(Subscript i)
+    {
+#ifdef TNT_BOUNDS_CHECK
+        assert(i==TNT_BASE_OFFSET);
+#endif
+        return (i==TNT_BASE_OFFSET ? v_.size() : 0 );
+    }
+    Vector_Adaptor() : v_() {};
+    Vector_Adaptor(const Vector_Adaptor<BBVec> &A) : v_(A.v_) 
+    { 
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+
+    } 
+
+    Vector_Adaptor(Subscript N, /*const*/ char *s) : v_(N) 
+    {
+        istrstream ins(s);
+        for (Subscript i=0; i<N; i++)
+            ins >> v_[i] ;
+
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+    }; 
+
+    Vector_Adaptor(Subscript N, const T& value = T()) : v_(N)
+    {
+        for (Subscript i=0; i<N; i++)
+             v_[i]  = value;
+        
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+    }
+
+    Vector_Adaptor(Subscript N, const T* values) : v_(N)
+    {
+        for (Subscript i=0; i<N; i++)
+             v_[i]  = values[i];
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+    } 
+    Vector_Adaptor(const BBVec & A) : v_(A) 
+    {
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+    }
+
+    // NOTE: this assumes that BBVec(0) constructor creates an 
+    //  null vector that does not take up space...  It would be
+    //  great to require that BBVec have a corresponding free()
+    //  function, but in particular STL vectors do not.
+    //
+    Vector_Adaptor<BBVec>& free()
+    {
+        return *this = Vector_Adaptor<BBVec>(0);
+    }
+
+    Vector_Adaptor<BBVec>& operator=(const Vector_Adaptor<BBVec> &A) 
+    { 
+        v_ = A.v_ ; 
+        vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); 
+        return *this;
+    }
+
+    Vector_Adaptor<BBVec>& newsize(Subscript N)
+    {
+        // NOTE: this is not as efficient as it could be
+        // but to retain compatiblity with STL interface
+        // we cannot assume underlying implementation
+        // has a newsize() function.
+
+        return *this = Vector_Adaptor<BBVec>(N);
+
+    }
+
+    Vector_Adaptor<BBVec>& operator=(const T &a) 
+    {
+        Subscript i;
+        Subscript N = v_.size();    
+        for (i=0; i<N; i++)
+            v_[i] = a;
+
+        return *this;
+    }
+
+    Vector_Adaptor<BBVec>& resize(Subscript N) 
+    { 
+        if (N == size()) return *this;
+
+        Vector_Adaptor<BBVec> tmp(N);
+        Subscript n =  (N < size() ? N : size());  // min(N, size());
+        Subscript i;
+
+        for (i=0; i<n; i++)
+            tmp[i] = v_[i];
+            
+
+        return (*this = tmp);
+
+    }
+
+
+    reference operator()(Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=dim());
+#endif
+        return vm1_[i]; 
+    }
+
+    const_reference operator()(Subscript i) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(1<=i);
+        assert(i<=dim());
+#endif
+        return vm1_[i]; 
+    }
+
+    reference operator[](Subscript i)
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i<dim());
+#endif
+        return v_[i]; 
+    }
+
+    const_reference operator[](Subscript i) const
+    { 
+#ifdef TNT_BOUNDS_CHECK
+        assert(0<=i);
+        assert(i<dim());
+#endif
+        return v_[i]; 
+    }
+
+
+#ifdef TNT_USE_REGIONS
+    // "index-aware" features, all of these are 1-based offsets
+
+    typedef Region1D<Vector_Adaptor<BBVec> > Region;
+
+    typedef const_Region1D< Vector_Adaptor<BBVec> > const_Region;
+
+    Region operator()(const Index1D &I)
+    {   return Region(*this, I); }
+
+    Region operator()(const Subscript i1, Subscript i2)
+    {   return Region(*this, i1, i2); }
+
+    const_Region operator()(const Index1D &I) const
+    {   return const_Region(*this, I); }
+
+    const_Region operator()(const Subscript i1, Subscript i2) const
+    {   return const_Region(*this, i1, i2); }
+#endif
+// TNT_USE_REGIONS
+
+
+};
+
+#include <iostream>
+
+template <class BBVec>
+std::ostream& operator<<(std::ostream &s, const Vector_Adaptor<BBVec> &A)
+{
+    Subscript M=A.size();
+
+    s << M << endl;
+    for (Subscript i=1; i<=M; i++)
+            s << A(i) << endl;
+    return s;
+}
+
+template <class BBVec>
+std::istream& operator>>(std::istream &s, Vector_Adaptor<BBVec> &A)
+{
+    Subscript N;
+    
+    s >> N;
+
+    A.resize(N);
+
+    for (Subscript i=1; i<=N; i++)
+        s >> A(i);
+
+    return s;
+}
+
+} // namespace TNT
+
+#endif

intern/iksolver/intern/TNT/version.h

@@ -0,0 +1,52 @@
+/**
+ * $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 *****
+ */
+
+// Template Numerical Toolkit (TNT) for Linear Algebra
+
+//
+// BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
+// Please see http://math.nist.gov/tnt for updates
+//
+// R. Pozo
+// Mathematical and Computational Sciences Division
+// National Institute of Standards and Technology
+
+
+#ifndef TNT_VERSION_H
+#define TNT_VERSION_H
+
+
+#define TNT_MAJOR_VERSION    '0'
+#define TNT_MINOR_VERSION    '9'
+#define TNT_SUBMINOR_VERSION '4'
+#define TNT_VERSION_STRING "0.9.4"
+
+#endif

intern/iksolver/make/msvc_6_0/iksolver.dsp

@@ -0,0 +1,244 @@
+# Microsoft Developer Studio Project File - Name="iksolver" - Package Owner=<4>
+# Microsoft Developer Studio Generated Build File, Format Version 6.00
+# ** DO NOT EDIT **
+
+# TARGTYPE "Win32 (x86) Static Library" 0x0104
+
+CFG=iksolver - Win32 Debug
+!MESSAGE This is not a valid makefile. To build this project using NMAKE,
+!MESSAGE use the Export Makefile command and run
+!MESSAGE 
+!MESSAGE NMAKE /f "iksolver.mak".
+!MESSAGE 
+!MESSAGE You can specify a configuration when running NMAKE
+!MESSAGE by defining the macro CFG on the command line. For example:
+!MESSAGE 
+!MESSAGE NMAKE /f "iksolver.mak" CFG="iksolver - Win32 Debug"
+!MESSAGE 
+!MESSAGE Possible choices for configuration are:
+!MESSAGE 
+!MESSAGE "iksolver - Win32 Release" (based on "Win32 (x86) Static Library")
+!MESSAGE "iksolver - Win32 Debug" (based on "Win32 (x86) Static Library")
+!MESSAGE 
+
+# Begin Project
+# PROP AllowPerConfigDependencies 0
+# PROP Scc_ProjName ""
+# PROP Scc_LocalPath ""
+CPP=cl.exe
+RSC=rc.exe
+
+!IF  "$(CFG)" == "iksolver - Win32 Release"
+
+# PROP BASE Use_MFC 0
+# PROP BASE Use_Debug_Libraries 0
+# PROP BASE Output_Dir "Release"
+# PROP BASE Intermediate_Dir "Release"
+# PROP BASE Target_Dir ""
+# PROP Use_MFC 0
+# PROP Use_Debug_Libraries 0
+# PROP Output_Dir "Release"
+# PROP Intermediate_Dir "Release"
+# PROP Target_Dir ""
+LINK32=link.exe -lib
+# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c
+# ADD CPP /nologo /W3 /GX /O2 /Ob2 /I "..\..\..\..\intern\moto\include" /I "..\..\..\..\intern\memutil" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /FD /c
+# SUBTRACT CPP /YX
+# ADD BASE RSC /l 0x413 /d "NDEBUG"
+# ADD RSC /l 0x413 /d "NDEBUG"
+BSC32=bscmake.exe
+# ADD BASE BSC32 /nologo
+# ADD BSC32 /nologo
+LIB32=link.exe -lib
+# ADD BASE LIB32 /nologo
+# ADD LIB32 /nologo /out:"..\..\lib\windows\release\iksolver_rmtd.lib"
+
+!ELSEIF  "$(CFG)" == "iksolver - Win32 Debug"
+
+# PROP BASE Use_MFC 0
+# PROP BASE Use_Debug_Libraries 1
+# PROP BASE Output_Dir "Debug"
+# PROP BASE Intermediate_Dir "Debug"
+# PROP BASE Target_Dir ""
+# PROP Use_MFC 0
+# PROP Use_Debug_Libraries 1
+# PROP Output_Dir "Debug"
+# PROP Intermediate_Dir "Debug"
+# PROP Target_Dir ""
+LINK32=link.exe -lib
+# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c
+# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\..\intern\moto\include" /I "..\..\..\..\intern\memutil" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /FD /GZ /c
+# SUBTRACT CPP /YX
+# ADD BASE RSC /l 0x413 /d "_DEBUG"
+# ADD RSC /l 0x413 /d "_DEBUG"
+BSC32=bscmake.exe
+# ADD BASE BSC32 /nologo
+# ADD BSC32 /nologo
+LIB32=link.exe -lib
+# ADD BASE LIB32 /nologo
+# ADD LIB32 /nologo /out:"..\..\lib\windows\debug\iksolver_dmtd.lib"
+
+!ENDIF 
+
+# Begin Target
+
+# Name "iksolver - Win32 Release"
+# Name "iksolver - Win32 Debug"
+# Begin Group "intern"
+
+# PROP Default_Filter ""
+# Begin Group "common"
+
+# PROP Default_Filter ""
+# End Group
+# Begin Group "TNT"
+
+# PROP Default_Filter ""
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\cholesky.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\cmat.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\fcscmat.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\fmat.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\fortran.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\fspvec.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\index.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\lapack.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\lu.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\qr.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\region1d.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\region2d.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\stopwatch.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\subscript.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\svd.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\tnt.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\tntmath.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\tntreqs.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\transv.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\triang.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\trisolve.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\vec.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\vecadaptor.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\TNT\version.h
+# End Source File
+# End Group
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QChain.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QChain.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QJacobianSolver.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QJacobianSolver.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QSegment.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QSegment.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_QSolver_Class.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\IK_Solver.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\MT_ExpMap.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\MT_ExpMap.h
+# End Source File
+# End Group
+# Begin Group "extern"
+
+# PROP Default_Filter ""
+# Begin Source File
+
+SOURCE=..\..\extern\IK_solver.h
+# End Source File
+# End Group
+# End Target
+# End Project

intern/iksolver/make/msvc_6_0/iksolver.dsw

@@ -0,0 +1,35 @@
+Microsoft Developer Studio Workspace File, Format Version 6.00
+# WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE!
+
+###############################################################################
+
+Project: "iksolver"=.\iksolver.dsp - Package Owner=<4>
+
+Package=<5>
+{{{
+}}}
+
+Package=<4>
+{{{
+}}}
+
+###############################################################################
+
+Global:
+
+Package=<5>
+{{{
+}}}
+
+Package=<3>
+{{{
+}}}
+
+###############################################################################
+
+
+
+
+
+
+

intern/iksolver/test/Makefile

@@ -0,0 +1,67 @@
+#
+# $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 *****
+# iksolver test makefile.
+#
+
+LIBNAME = iksolver
+SOURCEDIR = intern/$(LIBNAME)/test
+DIR = $(OCGDIR)/$(SOURCEDIR)
+
+include nan_compile.mk
+
+DIRS = ik_glut_test
+
+include nan_subdirs.mk
+
+include nan_link.mk
+
+LIBS = $(OCGDIR)/intern/$(LIBNAME)/test/ik_glut_test/intern/$(DEBUG_DIR)libintern.a
+LIBS +=  $(OCGDIR)/intern/$(LIBNAME)/test/ik_glut_test/common/$(DEBUG_DIR)libcommon.a
+LIBS += $(OCGDIR)/intern/$(LIBNAME)/$(DEBUG_DIR)libiksolver.a
+
+SLIBS += $(NAN_MOTO)/lib/$(DEBUG_DIR)libmoto.a
+
+ifeq ($(OS),$(findstring $(OS), "beos darwin linux freebsd openbsd"))
+    LLIBS = -L/usr/X11R6/lib -lglut -pthread
+endif
+
+all debug:: $(LIBS) $(DIR)/$(DEBUG_DIR)iksolvertest
+
+$(DIR)/$(DEBUG_DIR)iksolvertest:
+	@echo "****> linking $@ in $(DIR)"
+	$(CCC) $(LDFLAGS) -o $(DIR)/$(DEBUG_DIR)iksolvertest $(LIBS) $(SLIBS) $(LLIBS) $(DADD)
+
+clean::
+	$(RM) $(DIR)/iksolvertest $(DIR)/debug/iksolvertest
+
+test:: all
+	$(DIR)/iksolvertest
+

intern/iksolver/test/ik_glut_test/Makefile

@@ -0,0 +1,42 @@
+#
+# $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 *****
+# iksolver subdir bouncer. Pure waste.
+#
+
+include nan_definitions.mk
+
+LIBNAME = ik_glut_test
+SOURCEDIR = intern/iksolver/test/$(LIBNAME)
+DIR = $(OCGDIR)/$(SOURCEDIR)
+DIRS = common intern
+
+include nan_subdirs.mk
+

intern/iksolver/test/ik_glut_test/common/GlutDrawer.cpp

@@ -0,0 +1,95 @@
+/**
+ * $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 *****
+ */
+
+#include "GlutDrawer.h"
+
+#include "MT_assert.h"
+
+MEM_SmartPtr<GlutDrawManager> GlutDrawManager::m_s_instance = MEM_SmartPtr<GlutDrawManager>();
+
+	GlutDrawManager *
+GlutDrawManager::
+Instance(
+){
+	if (m_s_instance == NULL) {
+		m_s_instance = new GlutDrawManager();
+	}
+
+	return m_s_instance;
+}
+
+
+// this is the function you should pass to glut
+
+	void
+GlutDrawManager::
+Draw(
+){
+	GlutDrawManager *manager = GlutDrawManager::Instance();
+
+	if (manager->m_drawer != NULL) {
+		manager->m_drawer->Draw();
+	}
+}
+
+	void
+GlutDrawManager::
+InstallDrawer(
+	GlutDrawer * drawer
+){
+
+	MT_assert(m_drawer == NULL);
+	m_drawer = drawer;
+}
+
+	void
+GlutDrawManager::
+ReleaseDrawer(
+){
+	m_drawer = NULL;
+}
+
+
+GlutDrawManager::
+~GlutDrawManager(
+){
+
+	delete(m_drawer);
+}
+
+
+
+
+
+
+
+
+

intern/iksolver/test/ik_glut_test/common/GlutDrawer.h

@@ -0,0 +1,100 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_GlutDrawer
+
+#define NAN_INCLUDED_GlutDrawer
+
+#include "MEM_NonCopyable.h"
+#include "MEM_SmartPtr.h"
+
+// So pissed off with Glut callback stuff
+// that is impossible to call objects unless they are global
+
+// inherit from GlutDrawer and installl the drawer in the singleton
+// class GlutDrawManager.
+
+class GlutDrawer {
+public :
+
+	virtual 
+		void
+	Draw(
+	)= 0;
+
+	virtual 
+	~GlutDrawer(
+	){};		
+};
+
+class GlutDrawManager : public MEM_NonCopyable{
+
+public :
+
+	static
+		GlutDrawManager *
+	Instance(
+	);
+
+	// this is the function you should pass to glut
+
+	static
+		void
+	Draw(
+	);
+
+		void
+	InstallDrawer(
+		GlutDrawer *
+	);
+
+		void
+	ReleaseDrawer(
+	);
+
+	~GlutDrawManager(
+	);
+
+private :
+
+	GlutDrawManager (
+	) :
+		m_drawer (0)
+	{
+	};
+	
+	GlutDrawer * m_drawer;
+
+	static MEM_SmartPtr<GlutDrawManager> m_s_instance;
+};	
+
+
+#endif

intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.cpp

@@ -0,0 +1,89 @@
+/**
+ * $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 *****
+ */
+
+#include "GlutKeyboardManager.h"
+
+#include "MT_assert.h"
+
+MEM_SmartPtr<GlutKeyboardManager> GlutKeyboardManager::m_s_instance = MEM_SmartPtr<GlutKeyboardManager>();
+
+	GlutKeyboardManager *
+GlutKeyboardManager::
+Instance(
+){
+	if (m_s_instance == NULL) {
+		m_s_instance = new GlutKeyboardManager();
+	}
+
+	return m_s_instance;
+}
+
+
+// this is the function you should pass to glut
+
+	void
+GlutKeyboardManager::
+HandleKeyboard(
+	unsigned char key,
+	int x,
+	int y
+){
+	GlutKeyboardManager *manager = GlutKeyboardManager::Instance();
+
+	if (manager->m_handler != NULL) {
+		manager->m_handler->HandleKeyboard(key,x,y);
+	}
+}
+
+	void
+GlutKeyboardManager::
+InstallHandler(
+	GlutKeyboardHandler * handler
+){
+
+	MT_assert(m_handler == NULL);
+	m_handler = handler;
+}
+
+	void
+GlutKeyboardManager::
+ReleaseHandler(
+){
+	m_handler = NULL;
+}
+
+
+GlutKeyboardManager::
+~GlutKeyboardManager(
+){
+
+	delete(m_handler);
+}

intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.h

@@ -0,0 +1,106 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_GlutKeyboardManager
+
+#define NAN_INCLUDED_GlutKeyboardManager
+
+#include "MEM_NonCopyable.h"
+#include "MEM_SmartPtr.h"
+
+// So pissed off with Glut callback stuff
+// that is impossible to call objects unless they are global
+
+// inherit from GlutKeyboardHandler and installl the drawer in the singleton
+// class GlutKeyboardManager.
+
+class GlutKeyboardHandler : public MEM_NonCopyable {
+public :
+
+	virtual 
+		void
+	HandleKeyboard(
+		unsigned char key,
+		int x,
+		int y
+	)= 0;
+
+	virtual 
+	~GlutKeyboardHandler(
+	){};		
+};
+
+class GlutKeyboardManager : public MEM_NonCopyable{
+
+public :
+
+	static
+		GlutKeyboardManager *
+	Instance(
+	);
+
+	// this is the function you should pass to glut
+
+	static
+		void
+	HandleKeyboard(
+		unsigned char key,
+		int x,
+		int y
+	);
+
+		void
+	InstallHandler(
+		GlutKeyboardHandler *
+	);
+
+		void
+	ReleaseHandler(
+	);
+
+	~GlutKeyboardManager(
+	);
+
+private :
+
+	GlutKeyboardManager (
+	) :
+		m_handler (0)
+	{
+	};
+	
+	GlutKeyboardHandler * m_handler;
+
+	static MEM_SmartPtr<GlutKeyboardManager> m_s_instance;
+};	
+
+
+#endif

intern/iksolver/test/ik_glut_test/common/GlutMouseManager.cpp

@@ -0,0 +1,104 @@
+/**
+ * $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 *****
+ */
+
+#include "GlutMouseManager.h"
+
+#include "MT_assert.h"
+
+MEM_SmartPtr<GlutMouseManager> GlutMouseManager::m_s_instance = MEM_SmartPtr<GlutMouseManager>();
+
+
+	GlutMouseManager *
+GlutMouseManager::
+Instance(
+){
+	if (m_s_instance == NULL) {
+		m_s_instance = new GlutMouseManager();
+	}
+
+	return m_s_instance;
+}	
+
+// these are the functions you should pass to GLUT	
+
+	void
+GlutMouseManager::
+Mouse(
+	int button,
+	int state,
+	int x,
+	int y
+){
+	GlutMouseManager *manager = GlutMouseManager::Instance();
+
+	if (manager->m_handler != NULL) {
+		manager->m_handler->Mouse(button,state,x,y);
+	}
+}
+
+	void
+GlutMouseManager::
+Motion(
+	int x,
+	int y
+){
+	GlutMouseManager *manager = GlutMouseManager::Instance();
+
+	if (manager->m_handler != NULL) {
+		manager->m_handler->Motion(x,y);
+	}
+}
+
+	void
+GlutMouseManager::
+InstallHandler(
+	GlutMouseHandler *handler
+){
+
+	MT_assert(m_handler == NULL);
+	m_handler = handler;
+}
+
+	void
+GlutMouseManager::
+ReleaseHandler(
+){
+	m_handler = NULL;
+}
+
+GlutMouseManager::
+~GlutMouseManager(
+){
+
+	delete(m_handler);
+}
+
+

intern/iksolver/test/ik_glut_test/common/GlutMouseManager.h

@@ -0,0 +1,116 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_GlutMouseManager_h
+
+#define NAN_INCLUDED_GlutMouseManager_h
+
+#include "MEM_NonCopyable.h"
+#include "MEM_SmartPtr.h"
+
+class GlutMouseHandler {
+public :
+
+	virtual		
+		void
+	Mouse(
+		int button,
+		int state,
+		int x,
+		int y
+	) = 0;
+
+	virtual
+		void
+	Motion(
+		int x,
+		int y
+	) = 0;
+
+	virtual 
+	~GlutMouseHandler(
+	){};		
+};
+
+class GlutMouseManager : public MEM_NonCopyable{
+
+public :
+
+	static
+		GlutMouseManager *
+	Instance(
+	);
+
+	// these are the functions you should pass to GLUT	
+
+	static
+		void
+	Mouse(
+		int button,
+		int state,
+		int x,
+		int y
+	);
+
+	static
+		void
+	Motion(
+		int x,
+		int y
+	);
+
+		void
+	InstallHandler(
+		GlutMouseHandler *
+	);
+
+		void
+	ReleaseHandler(
+	);
+
+	~GlutMouseManager(
+	);
+
+private :
+
+	GlutMouseManager (
+	) :
+		m_handler (0)
+	{
+	};
+	
+	GlutMouseHandler * m_handler;
+
+	static MEM_SmartPtr<GlutMouseManager> m_s_instance;
+};	
+
+
+#endif

intern/iksolver/test/ik_glut_test/common/Makefile

@@ -0,0 +1,44 @@
+#
+# $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 *****
+# iksolver test intern Makefile
+#
+
+LIBNAME = common
+SOURCEDIR = intern/iksolver/test/ik_glut_test/$(LIBNAME)
+DIR = $(OCGDIR)/$(SOURCEDIR)
+
+include nan_compile.mk
+
+CCFLAGS += $(LEVEL_2_CPP_WARNINGS)
+
+CPPFLAGS += -I$(NAN_MOTO)/include
+CPPFLAGS += -I$(NAN_MEMUTIL)/include
+

intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h

@@ -0,0 +1,387 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_ChainDrawer_h
+
+#define NAN_INCLUDED_ChainDrawer_h
+
+#include "../common/GlutDrawer.h"
+#include "MyGlutMouseHandler.h"
+#include "MyGlutKeyHandler.h"
+#include "MT_Transform.h"
+#ifdef USE_QUATERNIONS
+#	include "IK_Qsolver.h"
+#	include "../intern/IK_QChain.h"
+#	include "../intern/IK_QSolver_Class.h"
+#else 
+#	include "IK_solver.h"
+#	include "../intern/IK_Chain.h"
+#	include "../intern/IK_Solver_Class.h"
+#endif
+#include <GL/glut.h>
+
+class ChainDrawer : public GlutDrawer
+{
+public :
+	static
+		ChainDrawer *
+	New(
+	) {
+		return new ChainDrawer();
+	}
+	
+		void
+	SetMouseHandler(
+		MyGlutMouseHandler *mouse_handler
+	) {
+		m_mouse_handler = mouse_handler;
+	}
+
+		void
+	SetKeyHandler (
+		MyGlutKeyHandler *key_handler
+	) {
+		m_key_handler = key_handler;
+	}
+
+		void
+	SetChain(
+		IK_Chain_ExternPtr *chains,int chain_num
+	) {
+		m_chain_num = chain_num;
+		m_chains = chains;
+	}
+
+
+	// inherited from GlutDrawer
+		void
+	Draw(
+	) {
+	  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+		  glPopMatrix();
+		  glPushMatrix();
+		  glRotatef(m_mouse_handler->AngleX(), 0.0, 1.0, 0.0);
+		  glRotatef(m_mouse_handler->AngleY(), 1.0, 0.0, 0.0);
+			
+	  DrawScene();
+	  glutSwapBuffers();
+
+	}
+
+	~ChainDrawer(
+	){
+		// nothing to do
+	};		
+	
+private :
+
+		void
+	DrawScene(
+	){
+
+		// draw a little cross at the position of the key handler
+		// coordinates
+
+		MT_Vector3 line_x(4,0,0);
+		MT_Vector3 line_y(0.0,4,0);
+		MT_Vector3 line_z(0.0,0.0,4);
+
+		MT_Vector3 cross_origin = m_mouse_handler->Position();
+		MT_Vector3 temp;
+		
+		glDisable(GL_LIGHTING);
+
+
+		glBegin(GL_LINES);
+
+		glColor3f (1.0f,1.0f,1.0f);
+
+		temp = cross_origin - line_x;
+		glVertex3f(temp[0],temp[1],temp[2]);
+		temp = cross_origin + line_x;
+		glVertex3f(temp[0],temp[1],temp[2]);
+
+		temp = cross_origin - line_y;
+		glVertex3f(temp[0],temp[1],temp[2]);
+		temp = cross_origin + line_y;
+		glVertex3f(temp[0],temp[1],temp[2]);
+
+		temp = cross_origin - line_z;
+		glVertex3f(temp[0],temp[1],temp[2]);
+		temp = cross_origin + line_z;
+		glVertex3f(temp[0],temp[1],temp[2]);
+
+		glEnd();
+		glEnable(GL_LIGHTING);
+
+
+		IK_Chain_ExternPtr chain;
+
+		int chain_num;
+		for (chain_num = 0; chain_num < m_chain_num; chain_num++) {
+			chain = m_chains[chain_num];
+
+
+			IK_Segment_ExternPtr segs = chain->segments;
+			IK_Segment_ExternPtr seg_start = segs;
+			const IK_Segment_ExternPtr seg_end = segs + chain->num_segments;
+			float ogl_matrix[16];
+
+			glColor3f (0.0f,1.0f,0.0f);
+
+			MT_Vector3 previous_origin(0,0,0);
+
+			MT_Transform global_transform;
+			global_transform.setIdentity();
+			
+			for (; seg_start != seg_end; ++seg_start) {
+				
+				glPushMatrix();
+
+				// fill ogl_matrix with zeros
+
+				std::fill(ogl_matrix,ogl_matrix + 16,float(0));
+
+				// we have to do a bit of work here to compute the chain's
+				// bone values
+					
+				// first compute all the matrices we need
+				
+				MT_Transform translation;
+				translation.setIdentity();
+				translation.translate(MT_Vector3(0,seg_start->length,0));
+
+				MT_Matrix3x3 seg_rot(
+					seg_start->basis_change[0],seg_start->basis_change[1],seg_start->basis_change[2],
+					seg_start->basis_change[3],seg_start->basis_change[4],seg_start->basis_change[5],
+					seg_start->basis_change[6],seg_start->basis_change[7],seg_start->basis_change[8]
+				);
+
+				seg_rot.transpose();
+
+				MT_Matrix3x3 seg_pre_rot(
+					seg_start->basis[0],seg_start->basis[1],seg_start->basis[2],
+					seg_start->basis[3],seg_start->basis[4],seg_start->basis[5],
+					seg_start->basis[6],seg_start->basis[7],seg_start->basis[8]
+				);
+
+		
+				MT_Transform seg_t_pre_rot(
+					MT_Point3(
+						seg_start->seg_start[0],
+						seg_start->seg_start[1],
+						seg_start->seg_start[2]
+					),
+					seg_pre_rot
+				);
+				// start of the bone is just the current global transform
+				// multiplied by the seg_start vector
+
+				
+
+				MT_Transform seg_t_rot(MT_Point3(0,0,0),seg_rot);
+				MT_Transform seg_local = seg_t_pre_rot * seg_t_rot * translation;
+
+				MT_Vector3 bone_start = global_transform * 	
+					MT_Point3(
+						seg_start->seg_start[0],
+						seg_start->seg_start[1],
+						seg_start->seg_start[2]
+					);
+
+
+				global_transform = global_transform * seg_local;
+
+				global_transform.getValue(ogl_matrix);
+				MT_Vector3 bone_end = global_transform.getOrigin();
+
+				glMultMatrixf(ogl_matrix);
+//				glutSolidSphere(0.5,5,5);
+
+				glPopMatrix();
+		
+				glDisable(GL_LIGHTING);
+
+				glBegin(GL_LINES);
+
+				// draw lines of the principle axis of the local transform
+
+				MT_Vector3 x_axis(1,0,0);
+				MT_Vector3 y_axis(0,1,0);
+				MT_Vector3 z_axis(0,0,1);
+
+				x_axis = global_transform.getBasis() * x_axis * 5;
+				y_axis = global_transform.getBasis() * y_axis * 5;
+				z_axis = global_transform.getBasis() * z_axis * 5;
+
+
+				x_axis = x_axis + bone_start;
+				y_axis = y_axis + bone_start;
+				z_axis = z_axis + bone_start;
+
+				glColor3f(1,0,0);
+
+				glVertex3f(x_axis.x(),x_axis.y(),x_axis.z());
+				glVertex3f(
+						bone_start.x(),
+						bone_start.y(),
+						bone_start.z()
+				);
+
+				glColor3f(0,1,0);
+
+				glVertex3f(y_axis.x(),y_axis.y(),y_axis.z());
+				glVertex3f(
+						bone_start.x(),
+						bone_start.y(),
+						bone_start.z()
+				);
+
+				glColor3f(0,1,1);
+
+				glVertex3f(z_axis.x(),z_axis.y(),z_axis.z());
+				glVertex3f(
+						bone_start.x(),
+						bone_start.y(),
+						bone_start.z()
+				);
+
+				glColor3f(0,0,1);
+
+				glVertex3f(
+						bone_start.x(),
+						bone_start.y(),
+						bone_start.z()
+				);
+				glVertex3f(bone_end[0],bone_end[1],bone_end[2]);
+
+				glEnd();
+				glEnable(GL_LIGHTING);
+			}
+#if 0
+			// draw jacobian column vectors
+
+			// hack access to internals
+
+			IK_Solver_Class * internals = static_cast<IK_Solver_Class *>(chain->intern);
+
+			glDisable(GL_LIGHTING);
+
+			glBegin(GL_LINES);
+
+			const TNT::Matrix<MT_Scalar> & jac = internals->Chain().TransposedJacobian();
+
+			int i = 0;
+			for (i=0; i < jac.num_rows(); i++) {
+				glColor3f(1,1,1);
+
+				previous_origin = internals->Chain().Segments()[i/3].GlobalSegmentStart();
+
+				glVertex3f(previous_origin[0],previous_origin[1],previous_origin[2]);
+				glVertex3f(jac[i][0] + previous_origin[0],jac[i][1] + previous_origin[1],jac[i][2] + previous_origin[2]);
+			
+				
+			}
+			glEnd();
+			glEnable(GL_LIGHTING);
+#endif
+			
+		}
+
+		glColor3f(1.0,1.0,1.0);
+
+		glDisable(GL_LIGHTING);
+		glBegin(GL_LINES);
+
+		MT_Scalar cube_size = 50;
+		glVertex3f(cube_size,cube_size,cube_size);
+		glVertex3f(-cube_size,cube_size,cube_size);
+
+		glVertex3f(cube_size,-cube_size,cube_size);
+		glVertex3f(-cube_size,-cube_size,cube_size);
+		
+		glVertex3f(cube_size,cube_size,-cube_size);
+		glVertex3f(-cube_size,cube_size,-cube_size);
+
+		glVertex3f(cube_size,-cube_size,-cube_size);
+		glVertex3f(-cube_size,-cube_size,-cube_size);
+
+
+		glVertex3f(-cube_size,cube_size,cube_size);
+		glVertex3f(-cube_size,-cube_size,cube_size);
+
+		glVertex3f(cube_size,cube_size,-cube_size);
+		glVertex3f(cube_size,-cube_size,-cube_size);
+
+		glVertex3f(cube_size,cube_size,cube_size);
+		glVertex3f(cube_size,-cube_size,cube_size);
+
+		glVertex3f(-cube_size,cube_size,-cube_size);
+		glVertex3f(-cube_size,-cube_size,-cube_size);
+
+
+		glVertex3f(cube_size,cube_size,cube_size);
+		glVertex3f(cube_size,cube_size,-cube_size);
+
+		glVertex3f(cube_size,-cube_size,cube_size);
+		glVertex3f(cube_size,-cube_size,-cube_size);
+
+		glVertex3f(-cube_size,cube_size,cube_size);
+		glVertex3f(-cube_size,cube_size,-cube_size);
+
+		glVertex3f(-cube_size,-cube_size,cube_size);
+		glVertex3f(-cube_size,-cube_size,-cube_size);
+		glEnd();
+		glEnable(GL_LIGHTING);
+
+	};
+
+
+
+private :
+
+	MyGlutMouseHandler * m_mouse_handler;
+	MyGlutKeyHandler *m_key_handler;
+	IK_Chain_ExternPtr *m_chains;
+
+	int m_chain_num;
+	ChainDrawer (
+	) : m_chains (NULL),
+		m_mouse_handler (NULL),
+		m_chain_num (0)
+	{
+	};
+	
+};
+
+
+
+#endif

intern/iksolver/test/ik_glut_test/intern/Makefile

@@ -0,0 +1,51 @@
+#
+# $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 *****
+# iksolver test intern Makefile
+#
+
+LIBNAME = intern
+SOURCEDIR = intern/iksolver/test/ik_glut_test/$(LIBNAME)
+DIR = $(OCGDIR)/$(SOURCEDIR)
+
+include nan_compile.mk
+
+CCFLAGS += $(LEVEL_2_CPP_WARNINGS)
+
+CPPFLAGS += -I$(OPENGL_HEADERS)
+CPPFLAGS += -I../../../extern
+CPPFLAGS += -I../common
+CPPFLAGS += -I$(NAN_MOTO)/include
+CPPFLAGS += -I$(NAN_MEMUTIL)/include
+
+ifeq ($(OS),windows)
+    CPPFLAGS += -I$(NAN_LIBDIR)/windows/glut-3.7/include
+endif
+

intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h

@@ -0,0 +1,84 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_MyGlutKeyHandler_h
+
+#define NAN_INCLUDED_MyGlutKeyHandler_h
+
+#include "../common/GlutKeyboardManager.h"
+
+class MyGlutKeyHandler : public GlutKeyboardHandler
+{
+public :
+	static
+		MyGlutKeyHandler *
+	New(
+	) {
+		MEM_SmartPtr<MyGlutKeyHandler> output = new MyGlutKeyHandler();
+
+		if (output == NULL
+		) {
+			return NULL;
+		}
+		return output.Release();
+		
+	}	
+
+		void
+	HandleKeyboard(
+		unsigned char key,
+		int x,
+		int y
+	){
+
+		switch (key) {
+
+			case 27 :
+
+				exit(0);
+		}	
+	}
+		
+	~MyGlutKeyHandler(
+	) 
+	{
+	};
+
+private :
+
+	MyGlutKeyHandler(
+	)
+	{
+	}
+
+};
+#endif
+

intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h

@@ -0,0 +1,211 @@
+/**
+ * $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 *****
+ */
+
+#ifndef NAN_INCLUDED_MyGlutMouseHandler_h
+
+#define NAN_INCLUDED_MyGlutMouseHandler_h
+
+#include "../common/GlutMouseManager.h"
+#include <GL/glut.h>
+#include "IK_solver.h"
+
+class MyGlutMouseHandler : public GlutMouseHandler
+{
+ 
+public :
+ 
+	static 
+		MyGlutMouseHandler *
+	New(
+	) {
+		MEM_SmartPtr<MyGlutMouseHandler> output = new MyGlutMouseHandler();
+		if (output == NULL
+		) {
+			return NULL;
+		}
+		return output.Release();
+		
+	}
+
+		void
+	SetChain(
+		IK_Chain_ExternPtr *chains, int num_chains
+	){
+		m_chains = chains;
+		m_num_chains = num_chains;
+	}
+
+		void
+	Mouse(
+		int button,
+		int state,
+		int x,
+		int y
+	){
+		if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
+			m_moving = true;
+			m_begin_x = x;
+			m_begin_y = y;	
+		}
+		if (button == GLUT_LEFT_BUTTON && state == GLUT_UP) {
+			m_moving = false;
+		}
+
+		if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) {
+			m_tracking = true;
+		}
+		if (button == GLUT_RIGHT_BUTTON && state == GLUT_UP) {
+			m_tracking = false;
+		}
+
+		if (button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN) {
+			m_cg_on = true;
+		}
+		if (button == GLUT_MIDDLE_BUTTON && state == GLUT_UP) {
+			m_cg_on = false;
+		}
+
+	}
+
+
+		void
+	Motion(
+		int x,
+		int y
+	){
+		if (m_moving) {
+			m_angle_x = m_angle_x + (x - m_begin_x);
+			m_begin_x = x;
+
+			m_angle_y = m_angle_y + (y - m_begin_y);
+			m_begin_y = y;
+
+			glutPostRedisplay();
+		}
+		if (m_tracking) {
+
+			int w_h = glutGet((GLenum)GLUT_WINDOW_HEIGHT);
+
+			y = w_h - y;
+
+			double mvmatrix[16];
+			double projmatrix[16];
+			GLint viewport[4];
+
+			double px, py, pz,sz;
+
+			/* Get the matrices needed for gluUnProject */
+			glGetIntegerv(GL_VIEWPORT, viewport);
+			glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix);
+			glGetDoublev(GL_PROJECTION_MATRIX, projmatrix);
+
+			// work out the position of the end effector in screen space
+
+			GLdouble ex,ey,ez;
+			ex = m_pos.x();
+			ey = m_pos.y();
+			ez = m_pos.z();
+
+			gluProject(ex, ey, ez, mvmatrix, projmatrix, viewport, &px, &py, &sz);
+			gluUnProject((GLdouble) x, (GLdouble) y, sz, mvmatrix, projmatrix, viewport, &px, &py, &pz);
+
+			m_pos = MT_Vector3(px,py,pz);
+
+		}
+		if (m_tracking || m_cg_on) {
+			float temp[3];
+			m_pos.getValue(temp);
+
+			IK_SolveChain(m_chains[0],temp,0.01,200,0.1,m_chains[1]->segments);
+			IK_LoadChain(m_chains[0],m_chains[0]->segments,m_chains[0]->num_segments);
+	
+			glutPostRedisplay();
+		}			
+
+
+	}
+
+	const 
+		float
+	AngleX(
+	) const {
+		return m_angle_x;
+	}
+
+	const 
+		float
+	AngleY(
+	) const {
+		return m_angle_y;
+	}
+
+	const
+		MT_Vector3	
+	Position(
+	) const {
+		return m_pos;
+	}
+
+
+private :
+
+	MyGlutMouseHandler (
+	) :  
+		m_angle_x(0),
+		m_angle_y(0),
+		m_begin_x(0),
+		m_begin_y(0),
+		m_moving (false),
+		m_tracking (false),
+		m_pos(0,0,0),
+		m_cg_on (false),
+		m_chains(NULL),
+		m_num_chains(0)
+	{
+	};
+		
+	float m_angle_x;
+	float m_angle_y;
+	float m_begin_x;
+	float m_begin_y;
+
+	bool m_moving;
+	bool m_tracking;
+	bool m_cg_on;
+	MT_Vector3 m_pos;
+	
+	IK_Chain_ExternPtr *m_chains;
+	int m_num_chains;
+
+};
+
+
+#endif

intern/iksolver/test/ik_glut_test/intern/main.cpp

@@ -0,0 +1,361 @@
+/**
+ * $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$
+ * ***** 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 *****
+ */
+
+
+#include "MEM_SmartPtr.h"
+
+#ifdef USE_QUATERNIONS
+#include "IK_Qsolver.h"
+#else 
+#include "IK_solver.h"
+#endif
+
+#include <GL/glut.h>
+#include "MT_Vector3.h"
+#include "MT_Quaternion.h"
+#include "MT_Matrix3x3.h"
+#include "MyGlutMouseHandler.h"	
+#include "MyGlutKeyHandler.h"
+#include "ChainDrawer.h"
+ 
+void
+init(MT_Vector3 min,MT_Vector3 max)
+{
+ 
+	GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 1.0};  /* Red diffuse light. */
+	GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0};  /* Infinite light location. */
+
+	GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 1.0};  /* Red diffuse light. */
+	GLfloat light_position1[] = {1.0, 0, 0, 0.0};  /* Infinite light location. */
+
+  /* Enable a single OpenGL light. */
+  glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0);
+  glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
+
+  glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1);
+  glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
+
+  glEnable(GL_LIGHT0);
+  glEnable(GL_LIGHT1);
+  glEnable(GL_LIGHTING);
+
+  /* Use depth buffering for hidden surface elimination. */
+  glEnable(GL_DEPTH_TEST);
+
+  /* Setup the view of the cube. */
+  glMatrixMode(GL_PROJECTION);
+
+	// centre of the box + 3* depth of box
+
+  MT_Vector3 centre = (min + max) * 0.5;
+  MT_Vector3 diag = max - min;
+
+	float depth = diag.length();
+	float distance = 2;
+
+  gluPerspective( 
+	/* field of view in degree */ 40.0,
+    /* aspect ratio */ 1.0,
+    /* Z near */ 1.0, 
+	/* Z far */ distance * depth * 2
+  );
+  glMatrixMode(GL_MODELVIEW);	
+
+
+  gluLookAt(
+	centre.x(), centre.y(), centre.z() + distance*depth,  /* eye is at (0,0,5) */
+    centre.x(), centre.y(), centre.z(),      /* center is at (0,0,0) */
+    0.0, 1.0, 0.);      /* up is in positive Y direction */
+
+  glPushMatrix();	
+
+ 
+}
+int
+main(int argc, char **argv)
+{
+
+
+	const int seg_num = 5;
+	const MT_Scalar seg_length = 15;
+
+	const float seg_startA[3] = {0,0,0};
+	const float seg_startB[3] = {0,-20,0};
+
+	// create some segments to solve with
+
+	// First chain
+	//////////////
+
+
+	IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num]; 	
+	IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num]; 	
+
+	IK_Segment_ExternPtr seg_it = segmentsA; 	
+	IK_Segment_ExternPtr seg_itB = segmentsB; 	
+
+	
+	{
+
+//		MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2);
+		MT_Quaternion qmat(MT_Vector3(0,0,1),0);
+		MT_Matrix3x3 mat(qmat);
+
+		seg_it->seg_start[0] = seg_startA[0];
+		seg_it->seg_start[1] = seg_startA[1];
+		seg_it->seg_start[2] = seg_startA[2];
+
+		float temp[12];
+		mat.getValue(temp);
+ 
+		seg_it->basis[0] = temp[0];
+		seg_it->basis[1] = temp[1];
+		seg_it->basis[2] = temp[2];
+
+		seg_it->basis[3] = temp[4];
+		seg_it->basis[4] = temp[5];
+		seg_it->basis[5] = temp[6];
+
+		seg_it->basis[6] = temp[8];
+		seg_it->basis[7] = temp[9];
+		seg_it->basis[8] = temp[10];
+ 
+		seg_it->length = seg_length;
+
+		MT_Quaternion q;
+		q.setEuler(0,0,0);
+
+			
+		MT_Matrix3x3 qrot(q);
+
+		seg_it->basis_change[0] = 1;
+		seg_it->basis_change[1] = 0;
+		seg_it->basis_change[2] = 0;
+		seg_it->basis_change[3] = 0;
+		seg_it->basis_change[4] = 1;
+		seg_it->basis_change[5] = 0;
+		seg_it->basis_change[6] = 0;
+		seg_it->basis_change[7] = 0;
+		seg_it->basis_change[8] = 1;
+
+
+		seg_it ++;			
+
+		seg_itB->seg_start[0] = seg_startA[0];
+		seg_itB->seg_start[1] = seg_startA[1];
+		seg_itB->seg_start[2] = seg_startA[2];
+ 
+		seg_itB->basis[0] = temp[0];
+		seg_itB->basis[1] = temp[1];
+		seg_itB->basis[2] = temp[2];
+
+		seg_itB->basis[3] = temp[4];
+		seg_itB->basis[4] = temp[5];
+		seg_itB->basis[5] = temp[6];
+
+		seg_itB->basis[6] = temp[8];
+		seg_itB->basis[7] = temp[9];
+		seg_itB->basis[8] = temp[10];
+ 
+		seg_itB->length = seg_length;
+
+		seg_itB->basis_change[0] = 1;
+		seg_itB->basis_change[1] = 0;
+		seg_itB->basis_change[2] = 0;
+		seg_itB->basis_change[3] = 0;
+		seg_itB->basis_change[4] = 1;
+		seg_itB->basis_change[5] = 0;
+		seg_itB->basis_change[6] = 0;
+		seg_itB->basis_change[7] = 0;
+		seg_itB->basis_change[8] = 1;
+
+
+		seg_itB ++;			
+
+
+	}
+
+
+	int i;
+	for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) {
+
+		MT_Quaternion qmat(MT_Vector3(0,0,1),0.3);
+		MT_Matrix3x3 mat(qmat);
+
+		seg_it->seg_start[0] = 0;
+		seg_it->seg_start[1] = 0;
+		seg_it->seg_start[2] = 0;
+
+		float temp[12];
+		mat.getValue(temp);
+ 
+		seg_it->basis[0] = temp[0];
+		seg_it->basis[1] = temp[1];
+		seg_it->basis[2] = temp[2];
+
+		seg_it->basis[3] = temp[4];
+		seg_it->basis[4] = temp[5];
+		seg_it->basis[5] = temp[6];
+
+		seg_it->basis[6] = temp[8];
+		seg_it->basis[7] = temp[9];
+		seg_it->basis[8] = temp[10];
+ 
+		seg_it->length = seg_length;
+
+		MT_Quaternion q;
+		q.setEuler(0,0,0);
+
+			
+		MT_Matrix3x3 qrot(q);
+
+		seg_it->basis_change[0] = 1;
+		seg_it->basis_change[1] = 0;
+		seg_it->basis_change[2] = 0;
+		seg_it->basis_change[3] = 0;
+		seg_it->basis_change[4] = 1;
+		seg_it->basis_change[5] = 0;
+		seg_it->basis_change[6] = 0;
+		seg_it->basis_change[7] = 0;
+		seg_it->basis_change[8] = 1;
+
+
+		///////////////////////////////
+
+		seg_itB->seg_start[0] = 0;
+		seg_itB->seg_start[1] = 0;
+		seg_itB->seg_start[2] = 0;
+ 
+		seg_itB->basis[0] = temp[0];
+		seg_itB->basis[1] = temp[1];
+		seg_itB->basis[2] = temp[2];
+
+		seg_itB->basis[3] = temp[4];
+		seg_itB->basis[4] = temp[5];
+		seg_itB->basis[5] = temp[6];
+
+		seg_itB->basis[6] = temp[8];
+		seg_itB->basis[7] = temp[9];
+		seg_itB->basis[8] = temp[10];
+ 
+		seg_itB->length = seg_length;
+
+		seg_itB->basis_change[0] = 1;
+		seg_itB->basis_change[1] = 0;
+		seg_itB->basis_change[2] = 0;
+		seg_itB->basis_change[3] = 0;
+		seg_itB->basis_change[4] = 1;
+		seg_itB->basis_change[5] = 0;
+		seg_itB->basis_change[6] = 0;
+		seg_itB->basis_change[7] = 0;
+		seg_itB->basis_change[8] = 1;
+
+
+
+	}
+
+	// create the chains
+
+	const int num_chains = 2;
+
+	IK_Chain_ExternPtr chains[num_chains];
+
+	chains[0] = IK_CreateChain();
+	chains[1] = IK_CreateChain();
+
+	// load segments into chain
+
+	IK_LoadChain(chains[0],segmentsA,seg_num);
+	IK_LoadChain(chains[1],segmentsB,seg_num);
+
+	// make and install a mouse handler
+
+	MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New());
+	GlutMouseManager::Instance()->InstallHandler(mouse_handler);
+
+	mouse_handler->SetChain(chains,num_chains);
+
+	// make and install a keyhandler
+	MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New());
+	GlutKeyboardManager::Instance()->InstallHandler(key_handler);
+
+	// instantiate the drawing class	
+
+	MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New());
+	GlutDrawManager::Instance()->InstallDrawer(drawer);
+
+	drawer->SetMouseHandler(mouse_handler);
+	drawer->SetChain(chains,num_chains);
+	drawer->SetKeyHandler(key_handler);
+
+	glutInit(&argc, argv);
+	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
+	glutCreateWindow("ik");
+	glutDisplayFunc(GlutDrawManager::Draw);
+	glutMouseFunc(GlutMouseManager::Mouse);
+	glutMotionFunc(GlutMouseManager::Motion);
+	glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard);
+
+	init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50));
+	glutMainLoop();
+	return 0;             /* ANSI C requires main to return int. */
+}

intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsp

@@ -0,0 +1,130 @@
+# Microsoft Developer Studio Project File - Name="ik_glut_test" - Package Owner=<4>
+# Microsoft Developer Studio Generated Build File, Format Version 6.00
+# ** DO NOT EDIT **
+
+# TARGTYPE "Win32 (x86) Console Application" 0x0103
+
+CFG=ik_glut_test - Win32 Debug
+!MESSAGE This is not a valid makefile. To build this project using NMAKE,
+!MESSAGE use the Export Makefile command and run
+!MESSAGE 
+!MESSAGE NMAKE /f "ik_glut_test.mak".
+!MESSAGE 
+!MESSAGE You can specify a configuration when running NMAKE
+!MESSAGE by defining the macro CFG on the command line. For example:
+!MESSAGE 
+!MESSAGE NMAKE /f "ik_glut_test.mak" CFG="ik_glut_test - Win32 Debug"
+!MESSAGE 
+!MESSAGE Possible choices for configuration are:
+!MESSAGE 
+!MESSAGE "ik_glut_test - Win32 Release" (based on "Win32 (x86) Console Application")
+!MESSAGE "ik_glut_test - Win32 Debug" (based on "Win32 (x86) Console Application")
+!MESSAGE 
+
+# Begin Project
+# PROP AllowPerConfigDependencies 0
+# PROP Scc_ProjName ""
+# PROP Scc_LocalPath ""
+CPP=cl.exe
+RSC=rc.exe
+
+!IF  "$(CFG)" == "ik_glut_test - Win32 Release"
+
+# PROP BASE Use_MFC 0
+# PROP BASE Use_Debug_Libraries 0
+# PROP BASE Output_Dir "Release"
+# PROP BASE Intermediate_Dir "Release"
+# PROP BASE Target_Dir ""
+# PROP Use_MFC 0
+# PROP Use_Debug_Libraries 0
+# PROP Output_Dir "Release"
+# PROP Intermediate_Dir "Release"
+# PROP Ignore_Export_Lib 0
+# PROP Target_Dir ""
+# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /c
+# ADD CPP /nologo /MT /W3 /GX /O2 /I "..\..\..\..\extern" /I "..\..\..\..\..\..\lib\windows\glut-3.7\include" /I "..\..\..\..\..\..\lib\windows\moto\include" /I "..\..\..\..\..\..\lib\windows\memutil\include" /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /c
+# ADD BASE RSC /l 0x413 /d "NDEBUG"
+# ADD RSC /l 0x413 /d "NDEBUG"
+BSC32=bscmake.exe
+# ADD BASE BSC32 /nologo
+# ADD BSC32 /nologo
+LINK32=link.exe
+# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /machine:I386
+# ADD LINK32 iksolver_rmtd.lib libmoto.a /nologo /subsystem:console /machine:I386 /libpath:"..\..\..\..\lib\windows\release" /libpath:"..\..\..\..\..\..\lib\windows\glut-3.7\lib" /libpath:"..\..\..\..\..\..\lib\windows\moto\lib"
+
+!ELSEIF  "$(CFG)" == "ik_glut_test - Win32 Debug"
+
+# PROP BASE Use_MFC 0
+# PROP BASE Use_Debug_Libraries 1
+# PROP BASE Output_Dir "Debug"
+# PROP BASE Intermediate_Dir "Debug"
+# PROP BASE Target_Dir ""
+# PROP Use_MFC 0
+# PROP Use_Debug_Libraries 1
+# PROP Output_Dir "Debug"
+# PROP Intermediate_Dir "Debug"
+# PROP Ignore_Export_Lib 0
+# PROP Target_Dir ""
+# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /GZ /c
+# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\..\extern" /I "..\..\..\..\..\..\lib\windows\glut-3.7\include" /I "..\..\..\..\..\..\lib\windows\moto\include" /I "..\..\..\..\..\..\lib\windows\memutil\include" /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /GZ /c
+# ADD BASE RSC /l 0x413 /d "_DEBUG"
+# ADD RSC /l 0x413 /d "_DEBUG"
+BSC32=bscmake.exe
+# ADD BASE BSC32 /nologo
+# ADD BSC32 /nologo
+LINK32=link.exe
+# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /debug /machine:I386 /pdbtype:sept
+# ADD LINK32 iksolver_dmtd.lib libmoto.a /nologo /subsystem:console /debug /machine:I386 /nodefaultlib:"LIBCMTD.lib" /pdbtype:sept /libpath:"..\..\..\..\lib\windows\debug" /libpath:"..\..\..\..\..\..\lib\windows\glut-3.7\lib" /libpath:"..\..\..\..\..\..\lib\windows\moto\lib\debug"
+
+!ENDIF 
+
+# Begin Target
+
+# Name "ik_glut_test - Win32 Release"
+# Name "ik_glut_test - Win32 Debug"
+# Begin Group "common"
+
+# PROP Default_Filter ""
+# Begin Source File
+
+SOURCE=..\..\common\GlutDrawer.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\common\GlutDrawer.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\common\GlutKeyboardManager.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\common\GlutKeyboardManager.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\common\GlutMouseManager.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\common\GlutMouseManager.h
+# End Source File
+# End Group
+# Begin Source File
+
+SOURCE=..\..\intern\ChainDrawer.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\main.cpp
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\MyGlutKeyHandler.h
+# End Source File
+# Begin Source File
+
+SOURCE=..\..\intern\MyGlutMouseHandler.h
+# End Source File
+# End Target
+# End Project

intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsw

@@ -0,0 +1,49 @@
+Microsoft Developer Studio Workspace File, Format Version 6.00
+# WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE!
+
+###############################################################################
+
+Project: "ik_glut_test"=.\ik_glut_test.dsp - Package Owner=<4>
+
+Package=<5>
+{{{
+}}}
+
+Package=<4>
+{{{
+    Begin Project Dependency
+    Project_Dep_Name iksolver
+    End Project Dependency
+}}}
+
+###############################################################################
+
+Project: "iksolver"=..\..\..\..\make\msvc_6_0\iksolver.dsp - Package Owner=<4>
+
+Package=<5>
+{{{
+}}}
+
+Package=<4>
+{{{
+}}}
+
+###############################################################################
+
+Global:
+
+Package=<5>
+{{{
+}}}
+
+Package=<3>
+{{{
+}}}
+
+###############################################################################
+
+
+
+
+
+