First commit of Brecht's new IK work. This only does the IK module.

Don't start compiling yet!

11 files changed, 1618 insertions, 1731 deletions

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