Update SConscript.

Fix some warnings.
Merge with latest soc code.

What changed in IK lib:

Fully restructured, with components now as follows:
  - IK_Solver: C <=> C++ interface
  - IK_QSegment: base class for bone/segment with 0
    to 3 DOF
  - IK_QTask: base class for a task (currently there's
    a position and a rotation task)
  - IK_QJacobian: the Jacobian matrix, with SVD
    decomposition, damping, etc
  - IK_QJacobianSolver: the iterative solver

The exponential map parametrization is no longer used,
instead we have now:
  - 3DOF and 2DOF XZ segments: directly update matrix
    with Rodrigues' formula
  - Other: Euler angles (no worries about singularities
    here)

Computation of the Jacobian inverse has also changed:
  - The SVD algorithm is now based on LAPACK code,
    instead of NR, to avoid some problems with rounding
    errors.
  - When the problem is underconstrained (as is the case
    most of the time), the SVD is computed for the transpose
    of the Jacobian (faster).
  - A new damping algorithm called the Selectively Damped
    Least Squares is used, result in faster and more
    stable convergence.
  - Stiffness is implemented as if a weighted psuedo-inverse
    was used.

Tree structure support.

Rotation limits:
  - 3DOF and 2DOF XZ segments limits are based on a swing
    (direct axis-angle over XZ) and twist/roll (rotation
    over Y) decomposition. The swing region is an ellipse
    on a sphere.
  - Rotation limits are implemented using an inner clamping
    loop: as long as there is a violation, a violating DOF
    is clamped and removed from the Jacobian, and the solution
    is recomputed.

Convergence checking is based now on the max norm of angle
change, or the maximum number of iterations.

10 files changed, 378 insertions, 491 deletions

diff --git a/intern/iksolver/SConscript b/intern/iksolver/SConscript
index aa96002b5f0..7ef8a9b9d11 100644
--- a/intern/iksolver/SConscript
+++ b/intern/iksolver/SConscript
@@ -4,11 +4,11 @@ Import ('library_env')
 
 iksolver_env = library_env.Copy ()
 
-source_files = ['intern/IK_QChain.cpp',
+source_files = ['intern/IK_QTask.cpp',
                 'intern/IK_QJacobianSolver.cpp',
                 'intern/IK_QSegment.cpp',
-                'intern/IK_Solver.cpp',
-                'intern/MT_ExpMap.cpp']
+                'intern/IK_QJacobian.cpp',
+                'intern/IK_Solver.cpp']
 
 iksolver_env.Append (CPPPATH = ['intern',
                                 '../moto/include',
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp
index c9690e68d4c..cfa764454d4 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -33,9 +33,6 @@
 #include "IK_QJacobian.h"
 #include "TNT/svd.h"
 
-#include <iostream>
-using namespace std;
-
 IK_QJacobian::IK_QJacobian()
 : m_sdls(true), m_min_damp(1.0)
 {
@@ -72,8 +69,6 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
 	m_weight = 1.0;
 	m_weight_sqrt = 1.0;
 
-	// m_svd_work_space.newsize(dof); // TNT resizes this?
-
 	if (task_size >= dof) {
 		m_transpose = false;
 
@@ -81,6 +76,9 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
 		m_svd_v.newsize(dof, dof);
 		m_svd_w.newsize(dof);
 
+		m_work1.newsize(task_size);
+		m_work2.newsize(dof);
+
 		m_svd_u_t.newsize(dof, task_size);
 		m_svd_u_beta.newsize(dof);
 	}
@@ -89,10 +87,15 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
 		// as the original, and often allows using smaller matrices.
 		m_transpose = true;
 
+		m_jacobian_t.newsize(dof, task_size);
+
 		m_svd_u.newsize(task_size, task_size);
 		m_svd_v.newsize(dof, task_size);
 		m_svd_w.newsize(task_size);
 
+		m_work1.newsize(dof);
+		m_work2.newsize(task_size);
+
 		m_svd_u_t.newsize(task_size, task_size);
 		m_svd_u_beta.newsize(task_size);
 	}
@@ -121,51 +124,19 @@ void IK_QJacobian::Invert()
 	if (m_transpose) {
 		// SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
 		// so J = U*W*Vt and Jinv = V*Winv*Ut
-		TNT::transpose(m_jacobian, m_svd_v);
-
-		TNT::SVD(m_svd_v, m_svd_w, m_svd_u, m_svd_work_space);
+		TNT::transpose(m_jacobian, m_jacobian_t);
+		TNT::SVD(m_jacobian_t, m_svd_v, m_svd_w, m_svd_u, m_work1, m_work2);
 	}
 	else {
 		// SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
 		// so Jinv = V*Winv*Ut
-		m_svd_u = m_jacobian;
-
-		TNT::SVD(m_svd_u, m_svd_w, m_svd_v, m_svd_work_space);
+		TNT::SVD(m_jacobian, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2);
 	}
 
 	if (m_sdls)
 		InvertSDLS();
 	else
 		InvertDLS();
-	
-#if 0
-	if (!ComputeNullProjection())
-		return;
-	
-	int i, j;
-	for (i = 0; i < m_weight.size(); i++)
-		m_weight[i] = 1.0 - m_weight[i];
-
-	TNT::matmultdiag(m_null, m_null, m_weight);
-
-	for (i = 0; i < m_null.num_rows(); i++)
-		for (j = 0; j < m_null.num_cols(); j++)
-			if (i == j)
-				m_null[i][j] = 1.0 - m_null[i][j];
-			else
-				m_null[i][j] = -m_null[i][j];
-	
-	TVector ntheta(m_d_theta);
-	TNT::matmult(ntheta, m_null, m_d_theta);
-
-	cout << "#" << endl;
-	for (i = 0; i < m_d_theta.size(); i++)
-		printf("%f >> %f (%f)\n", m_d_theta[i], ntheta[i], m_weight[i]);
-	m_d_theta = ntheta;
-
-	for (i = 0; i < m_weight.size(); i++)
-		m_weight[i] = 1.0 - m_weight[i];
-#endif
 }
 
 bool IK_QJacobian::ComputeNullProjection()
@@ -209,36 +180,6 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
 {
 	if (!ComputeNullProjection())
 		return;
-	
-#if 0
-	int i, j;
-
-	m_null.newsize(m_d_theta.size(), m_d_theta.size());
-
-	for (i = 0; i < m_d_theta.size(); i++)
-		for (j = 0; j < m_d_theta.size(); j++)
-			if (i == j)
-				m_null[i][j] = 1.0;
-			else
-				m_null[i][j] = 0.0;
-
-	// restrict lower priority jacobian
-	//jacobian.Restrict(m_d_theta, m_null);
-
-	// add angle update from lower priority
-	jacobian.Invert();
-
-	TVector d2(m_d_theta.size());
-	TVector n2(m_d_theta.size());
-
-	for (i = 0; i < m_d_theta.size(); i++)
-		d2[i] = jacobian.AngleUpdate(i);
-	
-	TNT::matmult(n2, m_null, d2);
-
-	m_d_theta = m_d_theta + n2;
-#else
-	int i;
 
 	// restrict lower priority jacobian
 	jacobian.Restrict(m_d_theta, m_null);
@@ -249,9 +190,9 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
 	// note: now damps secondary angles with minimum damping value from
 	// SDLS, to avoid shaking when the primary task is near singularities,
 	// doesn't work well at all
+	int i;
 	for (i = 0; i < m_d_theta.size(); i++)
 		m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(i);
-#endif
 }
 
 void IK_QJacobian::Restrict(TVector& d_theta, TMatrix& null)
@@ -472,23 +413,6 @@ void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
 	m_d_theta[dof_id] = 0.0;
 }
 
-#if 0
-void IK_QJacobian::SetSecondary(int dof_id, MT_Scalar d)
-{
-	m_alpha[dof_id] = d;
-}
-
-void IK_QJacobian::SolveSecondary()
-{
-	if (!ComputeNullProjection())
-		return;
-	
-	TNT::matmult(m_d_theta, m_null, m_alpha);
-
-	m_alpha = 0;
-}
-#endif
-
 MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
 {
 	return m_d_theta[dof_id];
diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h
index b2d990d5489..a16d7a7a941 100644
--- a/intern/iksolver/intern/IK_QJacobian.h
+++ b/intern/iksolver/intern/IK_QJacobian.h
@@ -36,7 +36,6 @@
 #define NAN_INCLUDED_IK_QJacobian_h
 
 #include "TNT/cmat.h"
-#include "TNT/fmat.h"
 #include <vector>
 #include "MT_Vector3.h"
 
@@ -44,7 +43,7 @@ class IK_QJacobian
 {
 public:
 	typedef TNT::Matrix<MT_Scalar> TMatrix;
-	typedef TNT::Vector<MT_Scalar>TVector;
+	typedef TNT::Vector<MT_Scalar> TVector;
 
 	IK_QJacobian();
 	~IK_QJacobian();
@@ -71,11 +70,6 @@ public:
 	void Restrict(TVector& d_theta, TMatrix& null);
 	void SubTask(IK_QJacobian& jacobian);
 
-#if 0
-	void SetSecondary(int dof_id, MT_Scalar d);
-	void SolveSecondary();
-#endif
-
 private:
 	
 	void InvertSDLS();
@@ -85,7 +79,7 @@ private:
 	bool m_transpose;
 
 	// the jacobian matrix and it's null space projector
-	TMatrix m_jacobian;
+	TMatrix m_jacobian, m_jacobian_t;
 	TMatrix m_null;
 
 	/// the vector of intermediate betas
@@ -97,9 +91,10 @@ private:
 	/// space required for SVD computation
 
 	TVector m_svd_w;
-    TVector m_svd_work_space;        
 	TMatrix m_svd_v;
 	TMatrix m_svd_u;
+    TVector m_work1;
+    TVector m_work2;
 
 	TMatrix m_svd_u_t;
 	TVector m_svd_u_beta;
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index 4bd3d28ce69..312b3146619 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -34,9 +34,6 @@
 
 //#include "analyze.h"
 
-#include <iostream>
-using namespace std;
-
 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
 {
 	m_segments.push_back(seg);
@@ -130,6 +127,9 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 	bool locked = false, clamp[3];
 	int i, mindof = 0;
 
+	// here we check if any angle limits were violated. angles whose clamped
+	// position is the same as it was before, are locked immediate. of the
+	// other violation angles the most violating angle is rememberd
 	for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
 		qseg = *seg;
 		if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
@@ -152,6 +152,7 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 		}
 	}
 
+	// lock most violating angle
 	if (minseg) {
 		minseg->Lock(mindof, m_jacobian, mindelta);
 		locked = true;
@@ -161,11 +162,13 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 	}
 
 	if (locked == false)
+		// no locking done, last inner iteration, apply the angles
 		for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
 			(*seg)->UnLock();
 			(*seg)->UpdateAngleApply();
 		}
 	
+	// signal if another inner iteration is needed
 	return locked;
 }
 
@@ -188,25 +191,14 @@ bool IK_QJacobianSolver::Solve(
 
 		std::list<IK_QTask*>::iterator task;
 
-		//bool done = true;
-
 		// compute jacobian
 		for (task = tasks.begin(); task != tasks.end(); task++) {
 			if ((*task)->Primary())
 				(*task)->ComputeJacobian(m_jacobian);
 			else
 				(*task)->ComputeJacobian(m_jacobian_sub);
-
-			//printf("#> %f\n", (*task)->Distance());
-			//if ((*task)->Distance() > 1e-4)
-			//	done = false;
 		}
 
-		/*if (done) {
-			//analyze_add_run(iterations, analyze_time()-dt);
-			return true;
-		}*/
-
 		MT_Scalar norm = 0.0;
 
 		do {
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h
index caa47b77539..adf95eb82dc 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.h
+++ b/intern/iksolver/intern/IK_QJacobianSolver.h
@@ -53,16 +53,7 @@ public:
 	IK_QJacobianSolver() {};
 	~IK_QJacobianSolver() {};
 
-	/**
-	 * Compute a solution for a chain.
-     * @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) 
-	 *
-     * @return True iff goal position reached.
-     */
+	// returns true if converged, false if max number of iterations was used
 
 	bool Solve(
 		IK_QSegment *root,
@@ -77,10 +68,12 @@ private:
 	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;
 };
 
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index a380e29e6fa..8d6655b1531 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -32,9 +32,6 @@
 
 #include "IK_QSegment.h"
 
-#include <iostream>
-using namespace std;
-
 // Utility functions
 
 static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis)
diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp
index 781a2712885..b0e51aec371 100644
--- a/intern/iksolver/intern/IK_QTask.cpp
+++ b/intern/iksolver/intern/IK_QTask.cpp
@@ -34,9 +34,6 @@
 
 // IK_QTask
 
-#include <iostream>
-using namespace std;
-
 IK_QTask::IK_QTask(
 	int size,
 	bool primary,
@@ -85,6 +82,7 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
 	
 	jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
 
+
 	// compute derivatives
 	int i;
 	const IK_QSegment *seg;
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index a335a946166..b1806a955b5 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -37,7 +37,6 @@
 #include "IK_QTask.h"
 
 #include <list>
-#include <iostream>
 using namespace std;
 
 typedef struct {
@@ -241,7 +240,7 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[
 	                 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);
+	IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
 	orient->SetWeight(weight);
 	qsolver->tasks.push_back(orient);
 }
diff --git a/intern/iksolver/intern/TNT/svd.h b/intern/iksolver/intern/TNT/svd.h
index 22cb1d7a088..1b25361811e 100644
--- a/intern/iksolver/intern/TNT/svd.h
+++ b/intern/iksolver/intern/TNT/svd.h
@@ -11,430 +11,414 @@
 // ,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
+//
+// s = diagonal elements of W
+// work1 = workspace, length must be A.num_rows
+// work2 = workspace, length must be A.num_cols
 
 #include "tntmath.h"
 
 namespace TNT
 {
 
-
 template <class MaTRiX, class VecToR >
-void SVD(MaTRiX &a, VecToR &w,  MaTRiX &v, VecToR &work_space) {
+void SVD(MaTRiX &A, MaTRiX &U, VecToR &s, MaTRiX &V, VecToR &work1, VecToR &work2) {
 
-		int n = a.num_cols();
-	int m = a.num_rows();
+	int m = A.num_rows();
+	int n = A.num_cols();
+	int nu = min(m,n);
 
-	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);
+	VecToR& work = work1;
+	VecToR& e = work2;
 
-    work_space.newsize(n);
+	U = 0;
+	s = 0;
 
-	for (i=1;i<=n;i++) {
-		l=i+1;
-		work_space(i)=scale*g;
+	int i=0, j=0, k=0;
 
-		g = (typename MaTRiX::value_type)0;
+	// Reduce A to bidiagonal form, storing the diagonal elements
+	// in s and the super-diagonal elements in e.
 
-		s = (typename  MaTRiX::value_type)0;
-        scale = (typename  MaTRiX::value_type)0;
+	int nct = min(m-1,n);
+	int nrt = max(0,min(n-2,m));
+	for (k = 0; k < max(nct,nrt); k++) {
+		if (k < nct) {
 
-		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;
+			// Compute the transformation for the k-th column and
+			// place the k-th diagonal in s[k].
+			// Compute 2-norm of k-th column without under/overflow.
+			s[k] = 0;
+			for (i = k; i < m; i++) {
+				s[k] = hypot(s[k],A[i][k]);
 			}
-		}
-		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);
+			if (s[k] != 0.0) {
+				if (A[k][k] < 0.0) {
+					s[k] = -s[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 (i = k; i < m; i++) {
+					A[i][k] /= s[k];
 				}
-				for (k=l;k<=n;k++) a(i,k) *= scale;
+				A[k][k] += 1.0;
 			}
+			s[k] = -s[k];
 		}
-		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 = k+1; j < n; j++) {
+			if ((k < nct) && (s[k] != 0.0))  {
+
+			// Apply the transformation.
+
+				typename MaTRiX::value_type t = 0;
+				for (i = k; i < m; i++) {
+					t += A[i][k]*A[i][j];
 				}
-			}
-			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);
+				t = -t/A[k][k];
+				for (i = k; i < m; i++) {
+					A[i][j] += t*A[i][k];
 				}
 			}
-			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;
+
+			// Place the k-th row of A into e for the
+			// subsequent calculation of the row transformation.
+
+			e[j] = A[k][j];
 		}
-		++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 (k < nct) {
+
+			// Place the transformation in U for subsequent back
+			// multiplication.
+
+			for (i = k; i < m; i++)
+				U[i][k] = A[i][k];
+		}
+		if (k < nrt) {
+
+			// Compute the k-th row transformation and place the
+			// k-th super-diagonal in e[k].
+			// Compute 2-norm without under/overflow.
+			e[k] = 0;
+			for (i = k+1; i < n; i++) {
+				e[k] = hypot(e[k],e[i]);
 			}
-			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;
-						}
-					}
+			if (e[k] != 0.0) {
+				if (e[k+1] < 0.0) {
+					e[k] = -e[k];
 				}
-			}
-			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));
+				for (i = k+1; i < n; i++) {
+					e[i] /= e[k];
 				}
-				break;
+				e[k+1] += 1.0;
 			}
+			e[k] = -e[k];
+			if ((k+1 < m) & (e[k] != 0.0)) {
 
+			// Apply the transformation.
 
-#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;
+				for (i = k+1; i < m; i++) {
+					work[i] = 0.0;
 				}
-				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;
+				for (j = k+1; j < n; j++) {
+					for (i = k+1; i < m; i++) {
+						work[i] += e[j]*A[i][j];
+					}
 				}
-				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;
+				for (j = k+1; j < n; j++) {
+					typename MaTRiX::value_type t = -e[j]/e[k+1];
+					for (i = k+1; i < m; i++) {
+						A[i][j] += t*work[i];
+					}
 				}
 			}
-			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();
+			// Place the transformation in V for subsequent
+			// back multiplication.
 
-	int flag,i,its,j,jj,k,l,nm;
-	typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
+			for (i = k+1; i < n; i++)
+				V[i][k] = e[i];
+		}
+	}
 
-	VecToR work_space;
-	work_space.newsize(n);
+	// Set up the final bidiagonal matrix or order p.
 
-	g = scale = anorm = 0.0;
-	
-	for (i=1;i <=n;i++) {
-		l = i+1;
-		work_space(i) = scale*g;
-		g = s=scale=0.0;
+	int p = min(n,m+1);
+	if (nct < n) {
+		s[nct] = A[nct][nct];
+	}
+	if (m < p) {
+		s[p-1] = 0.0;
+	}
+	if (nrt+1 < p) {
+		e[nrt] = A[nrt][p-1];
+	}
+	e[p-1] = 0.0;
 
-		if (i <= m) {
-			for(k=i; k<=m; k++) scale += abs(a(k,i));
+	// If required, generate U.
 
-			if (scale) {
-				for (k = i; k <=m ; k++) {
-					a(k,i) /= scale;
-					s += a(k,i)*a(k,i);
+	for (j = nct; j < nu; j++) {
+		for (i = 0; i < m; i++) {
+			U[i][j] = 0.0;
+		}
+		U[j][j] = 1.0;
+	}
+	for (k = nct-1; k >= 0; k--) {
+		if (s[k] != 0.0) {
+			for (j = k+1; j < nu; j++) {
+				typename MaTRiX::value_type t = 0;
+				for (i = k; i < m; i++) {
+					t += U[i][k]*U[i][j];
 				}
-				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);
+				t = -t/U[k][k];
+				for (i = k; i < m; i++) {
+					U[i][j] += t*U[i][k];
 				}
-				for (k = i; k <=m;k++) a(k,i) *= scale;
 			}
+			for (i = k; i < m; i++ ) {
+				U[i][k] = -U[i][k];
+			}
+			U[k][k] = 1.0 + U[k][k];
+			for (i = 0; i < k-1; i++) {
+				U[i][k] = 0.0;
+			}
+		} else {
+			for (i = 0; i < m; i++) {
+				U[i][k] = 0.0;
+			}
+			U[k][k] = 1.0;
 		}
+	}
 
-		w(i) = scale*g;
-		g = s = scale = 0.0;
+	// If required, generate V.
 
-		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);
+	for (k = n-1; k >= 0; k--) {
+		if ((k < nrt) & (e[k] != 0.0)) {
+			for (j = k+1; j < nu; j++) {
+				typename MaTRiX::value_type t = 0;
+				for (i = k+1; i < n; i++) {
+					t += V[i][k]*V[i][j];
 				}
-
-				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);
+				t = -t/V[k+1][k];
+				for (i = k+1; i < n; i++) {
+					V[i][j] += t*V[i][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;
+		for (i = 0; i < n; i++) {
+			V[i][k] = 0.0;
 		}
-		v(i,i) = 1.0;
-		g = work_space(i);
-		l = i;
+		V[k][k] = 1.0;
 	}
 
-	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);	
+	// Main iteration loop for the singular values.
+
+	int pp = p-1;
+	int iter = 0;
+	typename MaTRiX::value_type eps = pow(2.0,-52.0);
+	while (p > 0) {
+		int kase=0;
+		k=0;
+
+		// Here is where a test for too many iterations would go.
+
+		// This section of the program inspects for
+		// negligible elements in the s and e arrays.  On
+		// completion the variables kase and k are set as follows.
+
+		// kase = 1	  if s(p) and e[k-1] are negligible and k<p
+		// kase = 2	  if s(k) is negligible and k<p
+		// kase = 3	  if e[k-1] is negligible, k<p, and
+		//				  s(k), ..., s(p) are not negligible (qr step).
+		// kase = 4	  if e(p-1) is negligible (convergence).
+
+		for (k = p-2; k >= -1; k--) {
+			if (k == -1) {
+				break;
+			}
+			if (abs(e[k]) <= eps*(abs(s[k]) + abs(s[k+1]))) {
+				e[k] = 0.0;
+				break;
 			}
-			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;
+		if (k == p-2) {
+			kase = 4;
+		} else {
+			int ks;
+			for (ks = p-1; ks >= k; ks--) {
+				if (ks == k) {
+					break;
+				}
+				typename MaTRiX::value_type t = (ks != p ? abs(e[ks]) : 0.) + 
+							  (ks != k+1 ? abs(e[ks-1]) : 0.);
+				if (abs(s[ks]) <= eps*t)  {
+					s[ks] = 0.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;
+			if (ks == k) {
+				kase = 3;
+			} else if (ks == p-1) {
+				kase = 1;
+			} else {
+				kase = 2;
+				k = ks;
+			}
+		}
+		k++;
+
+		// Perform the task indicated by kase.
+
+		switch (kase) {
+
+			// Deflate negligible s(p).
+
+			case 1: {
+				typename MaTRiX::value_type f = e[p-2];
+				e[p-2] = 0.0;
+				for (j = p-2; j >= k; j--) {
+					typename MaTRiX::value_type t = hypot(s[j],f);
+					typename MaTRiX::value_type cs = s[j]/t;
+					typename MaTRiX::value_type sn = f/t;
+					s[j] = t;
+					if (j != k) {
+						f = -sn*e[j-1];
+						e[j-1] = cs*e[j-1];
+					}
+
+					for (i = 0; i < n; i++) {
+						t = cs*V[i][j] + sn*V[i][p-1];
+						V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1];
+						V[i][j] = t;
 					}
 				}
 			}
-			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;
+
+			// Split at negligible s(k).
+
+			case 2: {
+				typename MaTRiX::value_type f = e[k-1];
+				e[k-1] = 0.0;
+				for (j = k; j < p; j++) {
+					typename MaTRiX::value_type t = hypot(s[j],f);
+					typename MaTRiX::value_type cs = s[j]/t;
+					typename MaTRiX::value_type sn = f/t;
+					s[j] = t;
+					f = -sn*e[j];
+					e[j] = cs*e[j];
+
+					for (i = 0; i < m; i++) {
+						t = cs*U[i][j] + sn*U[i][k-1];
+						U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1];
+						U[i][j] = t;
+					}
 				}
-				break;
 			}
+			break;
+
+			// Perform one qr step.
+
+			case 3: {
+
+				// Calculate the shift.
+
+				typename MaTRiX::value_type scale = max(max(max(max(
+						  abs(s[p-1]),abs(s[p-2])),abs(e[p-2])), 
+						  abs(s[k])),abs(e[k]));
+				typename MaTRiX::value_type sp = s[p-1]/scale;
+				typename MaTRiX::value_type spm1 = s[p-2]/scale;
+				typename MaTRiX::value_type epm1 = e[p-2]/scale;
+				typename MaTRiX::value_type sk = s[k]/scale;
+				typename MaTRiX::value_type ek = e[k]/scale;
+				typename MaTRiX::value_type b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
+				typename MaTRiX::value_type c = (sp*epm1)*(sp*epm1);
+				typename MaTRiX::value_type shift = 0.0;
+				if ((b != 0.0) || (c != 0.0)) {
+					shift = sqrt(b*b + c);
+					if (b < 0.0) {
+						shift = -shift;
+					}
+					shift = c/(b + shift);
+				}
+				typename MaTRiX::value_type f = (sk + sp)*(sk - sp) + shift;
+				typename MaTRiX::value_type g = sk*ek;
+
+				// Chase zeros.
+
+				for (j = k; j < p-1; j++) {
+					typename MaTRiX::value_type t = hypot(f,g);
+					typename MaTRiX::value_type cs = f/t;
+					typename MaTRiX::value_type sn = g/t;
+					if (j != k) {
+						e[j-1] = t;
+					}
+					f = cs*s[j] + sn*e[j];
+					e[j] = cs*e[j] - sn*s[j];
+					g = sn*s[j+1];
+					s[j+1] = cs*s[j+1];
+
+					for (i = 0; i < n; i++) {
+						t = cs*V[i][j] + sn*V[i][j+1];
+						V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1];
+						V[i][j] = t;
+					}
 
-			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;
+					t = hypot(f,g);
+					cs = f/t;
+					sn = g/t;
+					s[j] = t;
+					f = cs*e[j] + sn*s[j+1];
+					s[j+1] = -sn*e[j] + cs*s[j+1];
+					g = sn*e[j+1];
+					e[j+1] = cs*e[j+1];
+					if (j < m-1) {
+						for (i = 0; i < m; i++) {
+							t = cs*U[i][j] + sn*U[i][j+1];
+							U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1];
+							U[i][j] = t;
+						}
+					}
 				}
-				z=pythag(f,h);
-				w(j)=z;
-				if(z) {
-					z = 1.0/z;
-					c=f*z;
-					s=h*z;
+				e[p-2] = f;
+				iter = iter + 1;
+			}
+			break;
+
+			// Convergence.
+
+			case 4: {
+
+				// Make the singular values positive.
+
+				if (s[k] <= 0.0) {
+					s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
+
+					for (i = 0; i <= pp; i++)
+						V[i][k] = -V[i][k];
 				}
-				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;
+
+				// Order the singular values.
+
+				while (k < pp) {
+					if (s[k] >= s[k+1]) {
+						break;
+					}
+					typename MaTRiX::value_type t = s[k];
+					s[k] = s[k+1];
+					s[k+1] = t;
+					if (k < n-1) {
+						for (i = 0; i < n; i++) {
+							t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t;
+						}
+					}
+					if (k < m-1) {
+						for (i = 0; i < m; i++) {
+							t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t;
+						}
+					}
+					k++;
 				}
+				iter = 0;
+				p--;
 			}
-
-			work_space(l) = 0.0;
-			work_space(k) = f;
-			w(k) = x;
+			break;
 		}
 	}
 }
diff --git a/intern/iksolver/intern/TNT/tntmath.h b/intern/iksolver/intern/TNT/tntmath.h
index 9e79c07fa67..5773900caf9 100644
--- a/intern/iksolver/intern/TNT/tntmath.h
+++ b/intern/iksolver/intern/TNT/tntmath.h
@@ -70,10 +70,15 @@ inline float min(float a, float b)
     return (a < b ? a : b);
 }
 
-inline int min(int a,int b) {
+inline int min(int a, int b)
+{
     return (a < b ? a : b);
 }
 
+inline int max(int a, int b)
+{
+    return (a > b ? a : b);
+}
 
 inline float max(float a, float b)
 {