IK solver: replace Moto math library with Eigen.

14 files changed, 642 insertions, 1077 deletions

diff --git a/intern/iksolver/CMakeLists.txt b/intern/iksolver/CMakeLists.txt
index 9476e0379e9..4e5699f1abd 100644
--- a/intern/iksolver/CMakeLists.txt
+++ b/intern/iksolver/CMakeLists.txt
@@ -29,7 +29,7 @@ set(INC
 )
 
 set(INC_SYS
-	../moto/include
+	${EIGEN3_INCLUDE_DIRS}
 )
 
 set(SRC
@@ -38,14 +38,12 @@ set(SRC
 	intern/IK_QSegment.cpp
 	intern/IK_QTask.cpp
 	intern/IK_Solver.cpp
-	intern/MT_ExpMap.cpp
 
 	extern/IK_solver.h
 	intern/IK_QJacobian.h
 	intern/IK_QJacobianSolver.h
 	intern/IK_QSegment.h
 	intern/IK_QTask.h
-	intern/MT_ExpMap.h
 	intern/TNT/cholesky.h
 	intern/TNT/cmat.h
 	intern/TNT/fcscmat.h
diff --git a/intern/iksolver/SConscript b/intern/iksolver/SConscript
index ba973ad5fd5..12d9d14712b 100644
--- a/intern/iksolver/SConscript
+++ b/intern/iksolver/SConscript
@@ -29,7 +29,7 @@ Import ('env')
 
 sources = env.Glob('intern/*.cpp')
 
-incs = 'intern ../moto/include ../memutil'
+incs = 'intern #/extern/Eigen3'
 
 env.BlenderLib ('bf_intern_iksolver', sources, Split(incs), [], libtype=['intern','player'], priority=[100,90] )
 
diff --git a/intern/iksolver/intern/IK_Math.h b/intern/iksolver/intern/IK_Math.h
new file mode 100644
index 00000000000..ba0eb047c8a
--- /dev/null
+++ b/intern/iksolver/intern/IK_Math.h
@@ -0,0 +1,259 @@
+/*
+ * ***** BEGIN GPL 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.
+ *
+ * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Original author: Laurence
+ * Contributor(s): Brecht
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <cmath>
+
+using Eigen::Matrix3d;
+using Eigen::Vector3d;
+using Eigen::Affine3d;
+
+static const double IK_EPSILON = 1e-20;
+
+static inline bool FuzzyZero(double x)
+{
+	return fabs(x) < IK_EPSILON;
+}
+
+static inline double Clamp(const double x, const double min, const double max)
+{
+	return (x < min) ? min : (x > max) ? max : x;
+}
+
+static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz,
+                                           double yx, double yy, double yz,
+                                           double zx, double zy, double zz)
+{
+	Eigen::Matrix3d M;
+	M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz;
+	M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz;
+	M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz;
+	return M;
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
+{
+	if (axis == 0)
+		return CreateMatrix(1.0, 0.0, 0.0,
+		                    0.0, cosine, -sine,
+		                    0.0, sine, cosine);
+	else if (axis == 1)
+		return CreateMatrix(cosine, 0.0, sine,
+		                    0.0, 1.0, 0.0,
+		                    -sine, 0.0, cosine);
+	else
+		return CreateMatrix(cosine, -sine, 0.0,
+		                    sine, cosine, 0.0,
+		                    0.0, 0.0, 1.0);
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
+{
+	return RotationMatrix(sin(angle), cos(angle), axis);
+}
+
+
+static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis)
+{
+	double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
+
+	if (t > 16.0 * IK_EPSILON) {
+		if      (axis == 0) return -atan2(R(1, 2), R(2, 2));
+		else if (axis == 1) return  atan2(-R(0, 2), t);
+		else                return -atan2(R(0, 1), R(0, 0));
+	}
+	else {
+		if      (axis == 0) return -atan2(-R(2, 1), R(1, 1));
+		else if (axis == 1) return  atan2(-R(0, 2), t);
+		else                return 0.0f;
+	}
+}
+
+static inline double safe_acos(double f)
+{
+	// acos that does not return NaN with rounding errors
+	if (f <= -1.0)
+		return M_PI;
+	else if (f >= 1.0)
+		return 0.0;
+	else
+		return acos(f);
+}
+
+static inline Eigen::Vector3d normalize(const Eigen::Vector3d& v)
+{
+	// a sane normalize function that doesn't give (1, 0, 0) in case
+	// of a zero length vector
+	double len = v.norm();
+	return FuzzyZero(len) ?  Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
+}
+
+static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
+{
+	return safe_acos(v1.dot(v2));
+}
+
+static inline double ComputeTwist(const Eigen::Matrix3d& R)
+{
+	// qy and qw are the y and w components of the quaternion from R
+	double qy = R(0, 2) - R(2, 0);
+	double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
+
+	double tau = 2.0 * atan2(qy, qw);
+
+	return tau;
+}
+
+static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
+{
+	return RotationMatrix(tau, 1);
+}
+
+static inline void RemoveTwist(Eigen::Matrix3d& R)
+{
+	// compute twist parameter
+	double tau = ComputeTwist(R);
+
+	// compute twist matrix
+	Eigen::Matrix3d T = ComputeTwistMatrix(tau);
+
+	// remove twist
+	R = R * T.transpose();
+}
+
+static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d& R)
+{
+	// compute twist parameter
+	double tau = ComputeTwist(R);
+
+	// compute swing parameters
+	double num = 2.0 * (1.0 + R(1, 1));
+
+	// singularity at pi
+	if (fabs(num) < IK_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 Eigen::Vector3d(0.0, tau, 1.0);
+
+	num = 1.0 / sqrt(num);
+	double ax = -R(2, 1) * num;
+	double az =  R(0, 1) * num;
+
+	return Eigen::Vector3d(ax, tau, az);
+}
+
+static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
+{
+	// length of (ax, 0, az) = sin(theta/2)
+	double sine2 = ax * ax + az * az;
+	double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
+
+	// compute swing matrix
+	Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
+
+	return S;
+}
+
+static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R)
+{
+	Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2),
+	                              R(0, 2) - R(2, 0),
+	                              R(1, 0) - R(0, 1));
+
+	double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
+	double l = delta.norm();
+	
+	if (!FuzzyZero(l))
+		delta *= c / l;
+	
+	return delta;
+}
+
+static inline bool EllipseClamp(double& ax, double& az, double *amin, double *amax)
+{
+	double xlim, zlim, x, z;
+
+	if (ax < 0.0) {
+		x = -ax;
+		xlim = -amin[0];
+	}
+	else {
+		x = ax;
+		xlim = amax[0];
+	}
+
+	if (az < 0.0) {
+		z = -az;
+		zlim = -amin[1];
+	}
+	else {
+		z = az;
+		zlim = amax[1];
+	}
+
+	if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
+		if (x <= xlim && z <= zlim)
+			return false;
+
+		if (x > xlim)
+			x = xlim;
+		if (z > zlim)
+			z = zlim;
+	}
+	else {
+		double invx = 1.0 / (xlim * xlim);
+		double invz = 1.0 / (zlim * zlim);
+
+		if ((x * x * invx + z * z * invz) <= 1.0)
+			return false;
+
+		if (FuzzyZero(x)) {
+			x = 0.0;
+			z = zlim;
+		}
+		else {
+			double rico = z / x;
+			double old_x = x;
+			x = sqrt(1.0 / (invx + invz * rico * rico));
+			if (old_x < 0.0)
+				x = -x;
+			z = rico * x;
+		}
+	}
+
+	ax = (ax < 0.0) ? -x : x;
+	az = (az < 0.0) ? -z : z;
+
+	return true;
+}
+
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp
index bb7b7c5c0b8..cd77d5d662d 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -104,14 +104,14 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size)
 	}
 }
 
-void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v)
+void IK_QJacobian::SetBetas(int id, int, const Vector3d& v)
 {
 	m_beta[id + 0] = v.x();
 	m_beta[id + 1] = v.y();
 	m_beta[id + 2] = v.z();
 }
 
-void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight)
+void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight)
 {
 	m_jacobian[id + 0][dof_id] = v.x() * m_weight_sqrt[dof_id];
 	m_jacobian[id + 1][dof_id] = v.y() * m_weight_sqrt[dof_id];
@@ -143,7 +143,7 @@ void IK_QJacobian::Invert()
 
 bool IK_QJacobian::ComputeNullProjection()
 {
-	MT_Scalar epsilon = 1e-10;
+	double epsilon = 1e-10;
 	
 	// compute null space projection based on V
 	int i, j, rank = 0;
@@ -230,8 +230,8 @@ void IK_QJacobian::InvertSDLS()
 	// DLS. The SDLS damps individual singular values, instead of using a single
 	// damping term.
 
-	MT_Scalar max_angle_change = MT_PI / 4.0;
-	MT_Scalar epsilon = 1e-10;
+	double max_angle_change = M_PI / 4.0;
+	double epsilon = 1e-10;
 	int i, j;
 
 	m_d_theta = 0;
@@ -240,7 +240,7 @@ void IK_QJacobian::InvertSDLS()
 	for (i = 0; i < m_dof; i++) {
 		m_norm[i] = 0.0;
 		for (j = 0; j < m_task_size; j += 3) {
-			MT_Scalar n = 0.0;
+			double n = 0.0;
 			n += m_jacobian[j][i] * m_jacobian[j][i];
 			n += m_jacobian[j + 1][i] * m_jacobian[j + 1][i];
 			n += m_jacobian[j + 2][i] * m_jacobian[j + 2][i];
@@ -252,9 +252,9 @@ void IK_QJacobian::InvertSDLS()
 		if (m_svd_w[i] <= epsilon)
 			continue;
 
-		MT_Scalar wInv = 1.0 / m_svd_w[i];
-		MT_Scalar alpha = 0.0;
-		MT_Scalar N = 0.0;
+		double wInv = 1.0 / m_svd_w[i];
+		double alpha = 0.0;
+		double N = 0.0;
 
 		// compute alpha and N
 		for (j = 0; j < m_svd_u.num_rows(); j += 3) {
@@ -264,7 +264,7 @@ void IK_QJacobian::InvertSDLS()
 
 			// note: for 1 end effector, N will always be 1, since U is
 			// orthogonal, .. so could be optimized
-			MT_Scalar tmp;
+			double tmp;
 			tmp = m_svd_u[j][i] * m_svd_u[j][i];
 			tmp += m_svd_u[j + 1][i] * m_svd_u[j + 1][i];
 			tmp += m_svd_u[j + 2][i] * m_svd_u[j + 2][i];
@@ -273,19 +273,19 @@ void IK_QJacobian::InvertSDLS()
 		alpha *= wInv;
 
 		// compute M, dTheta and max_dtheta
-		MT_Scalar M = 0.0;
-		MT_Scalar max_dtheta = 0.0, abs_dtheta;
+		double M = 0.0;
+		double max_dtheta = 0.0, abs_dtheta;
 
 		for (j = 0; j < m_d_theta.size(); j++) {
-			MT_Scalar v = m_svd_v[j][i];
-			M += MT_abs(v) * m_norm[j];
+			double v = m_svd_v[j][i];
+			M += fabs(v) * m_norm[j];
 
 			// compute tmporary dTheta's
 			m_d_theta_tmp[j] = v * alpha;
 
 			// find largest absolute dTheta
 			// multiply with weight to prevent unnecessary damping
-			abs_dtheta = MT_abs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
+			abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
 			if (abs_dtheta > max_dtheta)
 				max_dtheta = abs_dtheta;
 		}
@@ -293,18 +293,18 @@ void IK_QJacobian::InvertSDLS()
 		M *= wInv;
 
 		// compute damping term and damp the dTheta's
-		MT_Scalar gamma = max_angle_change;
+		double gamma = max_angle_change;
 		if (N < M)
 			gamma *= N / M;
 
-		MT_Scalar damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
+		double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
 
 		for (j = 0; j < m_d_theta.size(); j++) {
 			// slight hack: we do 0.80*, so that if there is some oscillation,
 			// the system can still converge (for joint limits). also, it's
 			// better to go a little to slow than to far
 			
-			MT_Scalar dofdamp = damp / m_weight[j];
+			double dofdamp = damp / m_weight[j];
 			if (dofdamp > 1.0) dofdamp = 1.0;
 			
 			m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
@@ -315,19 +315,19 @@ void IK_QJacobian::InvertSDLS()
 	}
 
 	// weight + prevent from doing angle updates with angles > max_angle_change
-	MT_Scalar max_angle = 0.0, abs_angle;
+	double max_angle = 0.0, abs_angle;
 
 	for (j = 0; j < m_dof; j++) {
 		m_d_theta[j] *= m_weight[j];
 
-		abs_angle = MT_abs(m_d_theta[j]);
+		abs_angle = fabs(m_d_theta[j]);
 
 		if (abs_angle > max_angle)
 			max_angle = abs_angle;
 	}
 	
 	if (max_angle > max_angle_change) {
-		MT_Scalar damp = (max_angle_change) / (max_angle_change + max_angle);
+		double damp = (max_angle_change) / (max_angle_change + max_angle);
 
 		for (j = 0; j < m_dof; j++)
 			m_d_theta[j] *= damp;
@@ -353,12 +353,12 @@ void IK_QJacobian::InvertDLS()
 	// find the smallest non-zero W value, anything below epsilon is
 	// treated as zero
 
-	MT_Scalar epsilon = 1e-10;
-	MT_Scalar max_angle_change = 0.1;
-	MT_Scalar x_length = sqrt(TNT::dot_prod(m_beta, m_beta));
+	double epsilon = 1e-10;
+	double max_angle_change = 0.1;
+	double x_length = sqrt(TNT::dot_prod(m_beta, m_beta));
 
 	int i, j;
-	MT_Scalar w_min = MT_INFINITY;
+	double w_min = std::numeric_limits<double>::max();
 
 	for (i = 0; i < m_svd_w.size(); i++) {
 		if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
@@ -367,8 +367,8 @@ void IK_QJacobian::InvertDLS()
 	
 	// compute lambda damping term
 
-	MT_Scalar d = x_length / max_angle_change;
-	MT_Scalar lambda;
+	double d = x_length / max_angle_change;
+	double lambda;
 
 	if (w_min <= d / 2)
 		lambda = d / 2;
@@ -393,7 +393,7 @@ void IK_QJacobian::InvertDLS()
 
 	for (i = 0; i < m_svd_w.size(); i++) {
 		if (m_svd_w[i] > epsilon) {
-			MT_Scalar wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
+			double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
 
 			// compute V*Winv*Ut*Beta
 			m_svd_u_beta[i] *= wInv;
@@ -407,7 +407,7 @@ void IK_QJacobian::InvertDLS()
 		m_d_theta[j] *= m_weight[j];
 }
 
-void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
+void IK_QJacobian::Lock(int dof_id, double delta)
 {
 	int i;
 
@@ -420,18 +420,18 @@ void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
 	m_d_theta[dof_id] = 0.0;
 }
 
-MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
+double IK_QJacobian::AngleUpdate(int dof_id) const
 {
 	return m_d_theta[dof_id];
 }
 
-MT_Scalar IK_QJacobian::AngleUpdateNorm() const
+double IK_QJacobian::AngleUpdateNorm() const
 {
 	int i;
-	MT_Scalar mx = 0.0, dtheta_abs;
+	double mx = 0.0, dtheta_abs;
 
 	for (i = 0; i < m_d_theta.size(); i++) {
-		dtheta_abs = MT_abs(m_d_theta[i] * m_d_norm_weight[i]);
+		dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
 		if (dtheta_abs > mx)
 			mx = dtheta_abs;
 	}
@@ -439,7 +439,7 @@ MT_Scalar IK_QJacobian::AngleUpdateNorm() const
 	return mx;
 }
 
-void IK_QJacobian::SetDoFWeight(int dof, MT_Scalar weight)
+void IK_QJacobian::SetDoFWeight(int dof, double weight)
 {
 	m_weight[dof] = weight;
 	m_weight_sqrt[dof] = sqrt(weight);
diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h
index b4b5a0402e6..2a5bac49373 100644
--- a/intern/iksolver/intern/IK_QJacobian.h
+++ b/intern/iksolver/intern/IK_QJacobian.h
@@ -31,39 +31,36 @@
  *  \ingroup iksolver
  */
 
-
-#ifndef __IK_QJACOBIAN_H__
-
-#define __IK_QJACOBIAN_H__
+#pragma once
 
 #include "TNT/cmat.h"
 #include <vector>
-#include "MT_Vector3.h"
+#include "IK_Math.h"
 
 class IK_QJacobian
 {
 public:
-	typedef TNT::Matrix<MT_Scalar> TMatrix;
-	typedef TNT::Vector<MT_Scalar> TVector;
+	typedef TNT::Matrix<double> TMatrix;
+	typedef TNT::Vector<double> TVector;
 
 	IK_QJacobian();
 	~IK_QJacobian();
 
 	// Call once to initialize
 	void ArmMatrices(int dof, int task_size);
-	void SetDoFWeight(int dof, MT_Scalar weight);
+	void SetDoFWeight(int dof, double weight);
 
 	// Iteratively called
-	void SetBetas(int id, int size, const MT_Vector3& v);
-	void SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight);
+	void SetBetas(int id, int size, const Vector3d& v);
+	void SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight);
 
 	void Invert();
 
-	MT_Scalar AngleUpdate(int dof_id) const;
-	MT_Scalar AngleUpdateNorm() const;
+	double AngleUpdate(int dof_id) const;
+	double AngleUpdateNorm() const;
 
 	// DoF locking for inner clamping loop
-	void Lock(int dof_id, MT_Scalar delta);
+	void Lock(int dof_id, double delta);
 
 	// Secondary task
 	bool ComputeNullProjection();
@@ -106,7 +103,7 @@ private:
 	bool m_sdls;
 	TVector m_norm;
 	TVector m_d_theta_tmp;
-	MT_Scalar m_min_damp;
+	double m_min_damp;
 
 	// null space task vector
 	TVector m_alpha;
@@ -116,5 +113,3 @@ private:
 	TVector m_weight_sqrt;
 };
 
-#endif
-
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index 75f51f566c9..b78270eb87f 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -32,8 +32,8 @@
 
 
 #include <stdio.h>
+
 #include "IK_QJacobianSolver.h"
-#include "MT_Quaternion.h"
 
 //#include "analyze.h"
 IK_QJacobianSolver::IK_QJacobianSolver()
@@ -43,10 +43,10 @@ IK_QJacobianSolver::IK_QJacobianSolver()
 	m_rootmatrix.setIdentity();
 }
 
-MT_Scalar IK_QJacobianSolver::ComputeScale()
+double IK_QJacobianSolver::ComputeScale()
 {
 	std::vector<IK_QSegment *>::iterator seg;
-	MT_Scalar length = 0.0f;
+	double length = 0.0f;
 
 	for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
 		length += (*seg)->MaxExtension();
@@ -57,7 +57,7 @@ MT_Scalar IK_QJacobianSolver::ComputeScale()
 		return 1.0 / length;
 }
 
-void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list<IK_QTask *>& tasks)
+void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *>& tasks)
 {
 	std::list<IK_QTask *>::iterator task;
 	std::vector<IK_QSegment *>::iterator seg;
@@ -68,7 +68,7 @@ void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list<IK_QTask *>& tasks)
 	for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
 		(*seg)->Scale(scale);
 	
-	m_rootmatrix.getOrigin() *= scale;
+	m_rootmatrix.translation() *= scale;
 	m_goal *= scale;
 	m_polegoal *= scale;
 }
@@ -102,7 +102,7 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
 	// 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;
+	double primary_weight = 0.0, secondary_weight = 0.0;
 	std::list<IK_QTask *>::iterator task;
 
 	for (task = tasks.begin(); task != tasks.end(); task++) {
@@ -122,15 +122,15 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
 		}
 	}
 
-	if (primary_size == 0 || MT_fuzzyZero(primary_weight))
+	if (primary_size == 0 || FuzzyZero(primary_weight))
 		return false;
 
 	m_secondary_enabled = (secondary > 0);
 	
 	// 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))
+	double primary_rescale = 1.0 / primary_weight;
+	double secondary_rescale;
+	if (FuzzyZero(secondary_weight))
 		secondary_rescale = 0.0;
 	else
 		secondary_rescale = 1.0 / secondary_weight;
@@ -159,7 +159,7 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
 	return true;
 }
 
-void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& goal, MT_Vector3& polegoal, float poleangle, bool getangle)
+void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal, Vector3d& polegoal, float poleangle, bool getangle)
 {
 	m_poleconstraint = true;
 	m_poletip = tip;
@@ -169,27 +169,6 @@ void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& g
 	m_getpoleangle = getangle;
 }
 
-static MT_Scalar safe_acos(MT_Scalar f)
-{
-	// acos that does not return NaN with rounding errors
-	if (f <= -1.0) return MT_PI;
-	else if (f >= 1.0) return 0.0;
-	else return acos(f);
-}
-
-static MT_Vector3 normalize(const MT_Vector3& v)
-{
-	// a sane normalize function that doesn't give (1, 0, 0) in case
-	// of a zero length vector, like MT_Vector3.normalize
-	MT_Scalar len = v.length();
-	return MT_fuzzyZero(len) ?  MT_Vector3(0, 0, 0) : v / len;
-}
-
-static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
-{
-	return safe_acos(v1.dot(v2));
-}
-
 void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks)
 {
 	// this function will be called before and after solving. calling it before
@@ -215,37 +194,38 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
 	// get positions and rotations
 	root->UpdateTransform(m_rootmatrix);
 
-	const MT_Vector3 rootpos = root->GlobalStart();
-	const MT_Vector3 endpos = m_poletip->GlobalEnd();
-	const MT_Matrix3x3& rootbasis = root->GlobalTransform().getBasis();
+	const Vector3d rootpos = root->GlobalStart();
+	const Vector3d endpos = m_poletip->GlobalEnd();
+	const Matrix3d& rootbasis = root->GlobalTransform().linear();
 
 	// construct "lookat" matrices (like gluLookAt), based on a direction and
 	// an up vector, with the direction going from the root to the end effector
 	// and the up vector going from the root to the pole constraint position.
-	MT_Vector3 dir = normalize(endpos - rootpos);
-	MT_Vector3 rootx = rootbasis.getColumn(0);
-	MT_Vector3 rootz = rootbasis.getColumn(2);
-	MT_Vector3 up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
+	Vector3d dir = normalize(endpos - rootpos);
+	Vector3d rootx = rootbasis.col(0);
+	Vector3d rootz = rootbasis.col(2);
+	Vector3d up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
 
 	// in post, don't rotate towards the goal but only correct the pole up
-	MT_Vector3 poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
-	MT_Vector3 poleup = normalize(m_polegoal - rootpos);
+	Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
+	Vector3d poleup = normalize(m_polegoal - rootpos);
 
-	MT_Matrix3x3 mat, polemat;
+	Matrix3d mat, polemat;
 
-	mat[0] = normalize(MT_cross(dir, up));
-	mat[1] = MT_cross(mat[0], dir);
-	mat[2] = -dir;
+	mat.row(0) = normalize(dir.cross(up));
+	mat.row(1) = mat.row(0).cross(dir);
+	mat.row(2) = -dir;
 
-	polemat[0] = normalize(MT_cross(poledir, poleup));
-	polemat[1] = MT_cross(polemat[0], poledir);
-	polemat[2] = -poledir;
+	polemat.row(0) = normalize(poledir.cross(poleup));
+	polemat.row(1) = polemat.row(0).cross(poledir);
+	polemat.row(2) = -poledir;
 
 	if (m_getpoleangle) {
 		// we compute the pole angle that to rotate towards the target
-		m_poleangle = angle(mat[1], polemat[1]);
+		m_poleangle = angle(mat.row(1), polemat.row(1));
 
-		if (rootz.dot(mat[1] * cos(m_poleangle) + mat[0] * sin(m_poleangle)) > 0.0)
+		double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
+		if (dt > 0.0)
 			m_poleangle = -m_poleangle;
 
 		// solve again, with the pole angle we just computed
@@ -257,18 +237,20 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
 		// desired rotation based on the pole vector constraint. we use
 		// transpose instead of inverse because we have orthogonal matrices
 		// anyway, and in case of a singular matrix we don't get NaN's.
-		MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed() * mat);
+		Affine3d trans;
+		trans.linear() = polemat.transpose() * mat;
+		trans.translation() = Vector3d(0, 0, 0);
 		m_rootmatrix = trans * m_rootmatrix;
 	}
 }
 
-bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
+bool IK_QJacobianSolver::UpdateAngles(double& 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;
+	double minabsdelta = 1e10, absdelta;
+	Vector3d delta, mindelta;
 	bool locked = false, clamp[3];
 	int i, mindof = 0;
 
@@ -280,9 +262,9 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 		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]);
+					absdelta = fabs(delta[i]);
 
-					if (absdelta < MT_EPSILON) {
+					if (absdelta < IK_EPSILON) {
 						qseg->Lock(i, m_jacobian, delta);
 						locked = true;
 					}
@@ -320,7 +302,7 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 bool IK_QJacobianSolver::Solve(
     IK_QSegment *root,
     std::list<IK_QTask *> tasks,
-    const MT_Scalar,
+    const double,
     const int max_iterations
     )
 {
@@ -349,7 +331,7 @@ bool IK_QJacobianSolver::Solve(
 				(*task)->ComputeJacobian(m_jacobian_sub);
 		}
 
-		MT_Scalar norm = 0.0;
+		double norm = 0.0;
 
 		do {
 			// invert jacobian
@@ -372,7 +354,7 @@ bool IK_QJacobianSolver::Solve(
 			(*seg)->UnLock();
 
 		// compute angle update norm
-		MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
+		double maxnorm = m_jacobian.AngleUpdateNorm();
 		if (maxnorm > norm)
 			norm = maxnorm;
 
@@ -384,7 +366,7 @@ bool IK_QJacobianSolver::Solve(
 	}
 
 	if (m_poleconstraint)
-		root->PrependBasis(m_rootmatrix.getBasis());
+		root->PrependBasis(m_rootmatrix.linear());
 
 	Scale(1.0f / scale, tasks);
 
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h
index 646f952b9ff..545ef91c710 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.h
+++ b/intern/iksolver/intern/IK_QJacobianSolver.h
@@ -30,10 +30,7 @@
  *  \ingroup iksolver
  */
 
-
-#ifndef __IK_QJACOBIANSOLVER_H__
-
-#define __IK_QJACOBIANSOLVER_H__
+#pragma once
 
 /**
  * @author Laurence Bourn
@@ -43,8 +40,7 @@
 #include <vector>
 #include <list>
 
-#include "MT_Vector3.h"
-#include "MT_Transform.h"
+#include "IK_Math.h"
 #include "IK_QJacobian.h"
 #include "IK_QSegment.h"
 #include "IK_QTask.h"
@@ -56,8 +52,8 @@ public:
 	~IK_QJacobianSolver() {}
 
 	// setup pole vector constraint
-	void SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& goal,
-		MT_Vector3& polegoal, float poleangle, bool getangle);
+	void SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal,
+		Vector3d& polegoal, float poleangle, bool getangle);
 	float GetPoleAngle() { return m_poleangle; }
 
 	// call setup once before solving, if it fails don't solve
@@ -67,17 +63,17 @@ public:
 	bool Solve(
 		IK_QSegment *root,
 		std::list<IK_QTask*> tasks,
-		const MT_Scalar tolerance,
+		const double tolerance,
 		const int max_iterations
 	);
 
 private:
 	void AddSegmentList(IK_QSegment *seg);
-	bool UpdateAngles(MT_Scalar& norm);
+	bool UpdateAngles(double& norm);
 	void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks);
 
-	MT_Scalar ComputeScale();
-	void Scale(MT_Scalar scale, std::list<IK_QTask*>& tasks);
+	double ComputeScale();
+	void Scale(double scale, std::list<IK_QTask*>& tasks);
 
 private:
 
@@ -88,15 +84,13 @@ private:
 
 	std::vector<IK_QSegment*> m_segments;
 
-	MT_Transform m_rootmatrix;
+	Affine3d m_rootmatrix;
 
 	bool m_poleconstraint;
 	bool m_getpoleangle;
-	MT_Vector3 m_goal;
-	MT_Vector3 m_polegoal;
+	Vector3d m_goal;
+	Vector3d m_polegoal;
 	float m_poleangle;
 	IK_QSegment *m_poletip;
 };
 
-#endif
-
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index e511d8233a2..23b094db279 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -32,192 +32,6 @@
 
 
 #include "IK_QSegment.h"
-#include <cmath>
-
-// Utility functions
-
-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);
-}
-
-static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis)
-{
-	return RotationMatrix(sin(angle), cos(angle), axis);
-}
-
-
-static MT_Scalar EulerAngleFromMatrix(const MT_Matrix3x3& R, int axis)
-{
-	MT_Scalar t = sqrt(R[0][0] * R[0][0] + R[0][1] * R[0][1]);
-
-	if (t > 16.0 * MT_EPSILON) {
-		if      (axis == 0) return -atan2(R[1][2], R[2][2]);
-		else if (axis == 1) return  atan2(-R[0][2], t);
-		else                return -atan2(R[0][1], R[0][0]);
-	}
-	else {
-		if      (axis == 0) return -atan2(-R[2][1], R[1][1]);
-		else if (axis == 1) return  atan2(-R[0][2], t);
-		else                return 0.0f;
-	}
-}
-
-static MT_Scalar safe_acos(MT_Scalar f)
-{
-	if (f <= -1.0)
-		return MT_PI;
-	else if (f >= 1.0)
-		return 0.0;
-	else
-		return acos(f);
-}
-
-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.0 * atan2(qy, qw);
-
-	return tau;
-}
-
-static MT_Matrix3x3 ComputeTwistMatrix(MT_Scalar tau)
-{
-	return RotationMatrix(tau, 1);
-}
-
-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;
-}
-
-static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scalar *amax)
-{
-	MT_Scalar xlim, zlim, x, z;
-
-	if (ax < 0.0) {
-		x = -ax;
-		xlim = -amin[0];
-	}
-	else {
-		x = ax;
-		xlim = amax[0];
-	}
-
-	if (az < 0.0) {
-		z = -az;
-		zlim = -amin[1];
-	}
-	else {
-		z = az;
-		zlim = amax[1];
-	}
-
-	if (MT_fuzzyZero(xlim) || MT_fuzzyZero(zlim)) {
-		if (x <= xlim && z <= zlim)
-			return false;
-
-		if (x > xlim)
-			x = xlim;
-		if (z > zlim)
-			z = zlim;
-	}
-	else {
-		MT_Scalar invx = 1.0 / (xlim * xlim);
-		MT_Scalar invz = 1.0 / (zlim * zlim);
-
-		if ((x * x * invx + z * z * invz) <= 1.0)
-			return false;
-
-		if (MT_fuzzyZero(x)) {
-			x = 0.0;
-			z = zlim;
-		}
-		else {
-			MT_Scalar rico = z / x;
-			MT_Scalar old_x = x;
-			x = sqrt(1.0 / (invx + invz * rico * rico));
-			if (old_x < 0.0)
-				x = -x;
-			z = rico * x;
-		}
-	}
-
-	ax = (ax < 0.0) ? -x : x;
-	az = (az < 0.0) ? -z : z;
-
-	return true;
-}
 
 // IK_QSegment
 
@@ -230,10 +44,10 @@ IK_QSegment::IK_QSegment(int num_DoF, bool translational)
 
 	m_max_extension = 0.0;
 
-	m_start = MT_Vector3(0, 0, 0);
+	m_start = Vector3d(0, 0, 0);
 	m_rest_basis.setIdentity();
 	m_basis.setIdentity();
-	m_translation = MT_Vector3(0, 0, 0);
+	m_translation = Vector3d(0, 0, 0);
 
 	m_orig_basis = m_basis;
 	m_orig_translation = m_translation;
@@ -252,13 +66,13 @@ void IK_QSegment::Reset()
 }
 
 void IK_QSegment::SetTransform(
-    const MT_Vector3& start,
-    const MT_Matrix3x3& rest_basis,
-    const MT_Matrix3x3& basis,
-    const MT_Scalar length
+    const Vector3d& start,
+    const Matrix3d& rest_basis,
+    const Matrix3d& basis,
+    const double length
     )
 {
-	m_max_extension = start.length() + length;	
+	m_max_extension = start.norm() + length;	
 
 	m_start = start;
 	m_rest_basis = rest_basis;
@@ -266,16 +80,16 @@ void IK_QSegment::SetTransform(
 	m_orig_basis = basis;
 	SetBasis(basis);
 
-	m_translation = MT_Vector3(0, length, 0);
+	m_translation = Vector3d(0, length, 0);
 	m_orig_translation = m_translation;
 }
 
-MT_Matrix3x3 IK_QSegment::BasisChange() const
+Matrix3d IK_QSegment::BasisChange() const
 {
-	return m_orig_basis.transposed() * m_basis;
+	return m_orig_basis.transpose() * m_basis;
 }
 
-MT_Vector3 IK_QSegment::TranslationChange() const
+Vector3d IK_QSegment::TranslationChange() const
 {
 	return m_translation - m_orig_translation;
 }
@@ -327,13 +141,13 @@ void IK_QSegment::RemoveChild(IK_QSegment *child)
 	}
 }
 
-void IK_QSegment::UpdateTransform(const MT_Transform& global)
+void IK_QSegment::UpdateTransform(const Affine3d& global)
 {
 	// compute the global transform at the end of the segment
-	m_global_start = global.getOrigin() + global.getBasis() * m_start;
+	m_global_start = global.translation() + global.linear() * m_start;
 
-	m_global_transform.setOrigin(m_global_start);
-	m_global_transform.setBasis(global.getBasis() * m_rest_basis * m_basis);
+	m_global_transform.translation() = m_global_start;
+	m_global_transform.linear() = global.linear() * m_rest_basis * m_basis;
 	m_global_transform.translate(m_translation);
 
 	// update child transforms
@@ -341,18 +155,18 @@ void IK_QSegment::UpdateTransform(const MT_Transform& global)
 		seg->UpdateTransform(m_global_transform);
 }
 
-void IK_QSegment::PrependBasis(const MT_Matrix3x3& mat)
+void IK_QSegment::PrependBasis(const Matrix3d& mat)
 {
 	m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
 }
 
-void IK_QSegment::Scale(MT_Scalar scale)
+void IK_QSegment::Scale(double scale)
 {
 	m_start *= scale;
 	m_translation *= scale;
 	m_orig_translation *= scale;
 	m_global_start *= scale;
-	m_global_transform.getOrigin() *= scale;
+	m_global_transform.translation() *= scale;
 	m_max_extension *= scale;
 }
 
@@ -363,19 +177,19 @@ IK_QSphericalSegment::IK_QSphericalSegment()
 {
 }
 
-MT_Vector3 IK_QSphericalSegment::Axis(int dof) const
+Vector3d IK_QSphericalSegment::Axis(int dof) const
 {
-	return m_global_transform.getBasis().getColumn(dof);
+	return m_global_transform.linear().col(dof);
 }
 
-void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax)
 {
 	if (lmin > lmax)
 		return;
 	
 	if (axis == 1) {
-		lmin = MT_clamp(lmin, -MT_PI, MT_PI);
-		lmax = MT_clamp(lmax, -MT_PI, MT_PI);
+		lmin = Clamp(lmin, -M_PI, M_PI);
+		lmax = Clamp(lmax, -M_PI, M_PI);
 
 		m_min_y = lmin;
 		m_max_y = lmax;
@@ -384,8 +198,8 @@ void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	}
 	else {
 		// clamp and convert to axis angle parameters
-		lmin = MT_clamp(lmin, -MT_PI, MT_PI);
-		lmax = MT_clamp(lmax, -MT_PI, MT_PI);
+		lmin = Clamp(lmin, -M_PI, M_PI);
+		lmax = Clamp(lmax, -M_PI, M_PI);
 
 		lmin = sin(lmin * 0.5);
 		lmax = sin(lmax * 0.5);
@@ -403,17 +217,17 @@ void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	}
 }
 
-void IK_QSphericalSegment::SetWeight(int axis, MT_Scalar weight)
+void IK_QSphericalSegment::SetWeight(int axis, double weight)
 {
 	m_weight[axis] = weight;
 }
 
-bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
 {
 	if (m_locked[0] && m_locked[1] && m_locked[2])
 		return false;
 
-	MT_Vector3 dq;
+	Vector3d dq;
 	dq.x() = jacobian.AngleUpdate(m_DoF_id);
 	dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
 	dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
@@ -421,27 +235,27 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	// Directly update the rotation matrix, with Rodrigues' rotation formula,
 	// to avoid singularities and allow smooth integration.
 
-	MT_Scalar theta = dq.length();
+	double theta = dq.norm();
 
-	if (!MT_fuzzyZero(theta)) {
-		MT_Vector3 w = dq * (1.0 / theta);
+	if (!FuzzyZero(theta)) {
+		Vector3d w = dq * (1.0 / theta);
 
-		MT_Scalar sine = sin(theta);
-		MT_Scalar cosine = cos(theta);
-		MT_Scalar cosineInv = 1 - cosine;
+		double sine = sin(theta);
+		double cosine = cos(theta);
+		double cosineInv = 1 - cosine;
 
-		MT_Scalar xsine = w.x() * sine;
-		MT_Scalar ysine = w.y() * sine;
-		MT_Scalar zsine = w.z() * sine;
+		double xsine = w.x() * sine;
+		double ysine = w.y() * sine;
+		double zsine = w.z() * sine;
 
-		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;
+		double xxcosine = w.x() * w.x() * cosineInv;
+		double xycosine = w.x() * w.y() * cosineInv;
+		double xzcosine = w.x() * w.z() * cosineInv;
+		double yycosine = w.y() * w.y() * cosineInv;
+		double yzcosine = w.y() * w.z() * cosineInv;
+		double zzcosine = w.z() * w.z() * cosineInv;
 
-		MT_Matrix3x3 M(
+		Matrix3d M = CreateMatrix(
 		    cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
 		    zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
 		    -ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
@@ -455,7 +269,7 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
 		return false;
 
-	MT_Vector3 a = SphericalRangeParameters(m_new_basis);
+	Vector3d a = SphericalRangeParameters(m_new_basis);
 
 	if (m_locked[0])
 		a.x() = m_locked_ax;
@@ -464,7 +278,7 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	if (m_locked[2])
 		a.z() = m_locked_az;
 
-	MT_Scalar ax = a.x(), ay = a.y(), az = a.z();
+	double ax = a.x(), ay = a.y(), az = a.z();
 
 	clamp[0] = clamp[1] = clamp[2] =  false;
 	
@@ -512,7 +326,7 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	
 	m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
 
-	delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
+	delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
 
 	if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
 		m_locked_ax = ax;
@@ -525,7 +339,7 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	return true;
 }
 
-void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
+void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
 {
 	if (dof == 1) {
 		m_locked[1] = true;
@@ -557,7 +371,7 @@ IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
 {
 }
 
-void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis)
+void IK_QRevoluteSegment::SetBasis(const Matrix3d& basis)
 {
 	if (m_axis == 1) {
 		m_angle = ComputeTwist(basis);
@@ -569,12 +383,12 @@ void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis)
 	}
 }
 
-MT_Vector3 IK_QRevoluteSegment::Axis(int) const
+Vector3d IK_QRevoluteSegment::Axis(int) const
 {
-	return m_global_transform.getBasis().getColumn(m_axis);
+	return m_global_transform.linear().col(m_axis);
 }
 
-bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
 {
 	if (m_locked[0])
 		return false;
@@ -599,7 +413,7 @@ bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
 	return true;
 }
 
-void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
+void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta)
 {
 	m_locked[0] = true;
 	jacobian.Lock(m_DoF_id, delta[0]);
@@ -611,14 +425,14 @@ void IK_QRevoluteSegment::UpdateAngleApply()
 	m_basis = RotationMatrix(m_angle, m_axis);
 }
 
-void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
 {
 	if (lmin > lmax || m_axis != axis)
 		return;
 	
 	// clamp and convert to axis angle parameters
-	lmin = MT_clamp(lmin, -MT_PI, MT_PI);
-	lmax = MT_clamp(lmax, -MT_PI, MT_PI);
+	lmin = Clamp(lmin, -M_PI, M_PI);
+	lmax = Clamp(lmax, -M_PI, M_PI);
 
 	m_min = lmin;
 	m_max = lmax;
@@ -626,7 +440,7 @@ void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	m_limit = true;
 }
 
-void IK_QRevoluteSegment::SetWeight(int axis, MT_Scalar weight)
+void IK_QRevoluteSegment::SetWeight(int axis, double weight)
 {
 	if (axis == m_axis)
 		m_weight[0] = weight;
@@ -639,23 +453,23 @@ IK_QSwingSegment::IK_QSwingSegment()
 {
 }
 
-void IK_QSwingSegment::SetBasis(const MT_Matrix3x3& basis)
+void IK_QSwingSegment::SetBasis(const Matrix3d& basis)
 {
 	m_basis = basis;
 	RemoveTwist(m_basis);
 }
 
-MT_Vector3 IK_QSwingSegment::Axis(int dof) const
+Vector3d IK_QSwingSegment::Axis(int dof) const
 {
-	return m_global_transform.getBasis().getColumn((dof == 0) ? 0 : 2);
+	return m_global_transform.linear().col((dof == 0) ? 0 : 2);
 }
 
-bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
 {
 	if (m_locked[0] && m_locked[1])
 		return false;
 
-	MT_Vector3 dq;
+	Vector3d dq;
 	dq.x() = jacobian.AngleUpdate(m_DoF_id);
 	dq.y() = 0.0;
 	dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
@@ -663,23 +477,23 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
 	// Directly update the rotation matrix, with Rodrigues' rotation formula,
 	// to avoid singularities and allow smooth integration.
 
-	MT_Scalar theta = dq.length();
+	double theta = dq.norm();
 
-	if (!MT_fuzzyZero(theta)) {
-		MT_Vector3 w = dq * (1.0 / theta);
+	if (!FuzzyZero(theta)) {
+		Vector3d w = dq * (1.0 / theta);
 
-		MT_Scalar sine = sin(theta);
-		MT_Scalar cosine = cos(theta);
-		MT_Scalar cosineInv = 1 - cosine;
+		double sine = sin(theta);
+		double cosine = cos(theta);
+		double cosineInv = 1 - cosine;
 
-		MT_Scalar xsine = w.x() * sine;
-		MT_Scalar zsine = w.z() * sine;
+		double xsine = w.x() * sine;
+		double 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;
+		double xxcosine = w.x() * w.x() * cosineInv;
+		double xzcosine = w.x() * w.z() * cosineInv;
+		double zzcosine = w.z() * w.z() * cosineInv;
 
-		MT_Matrix3x3 M(
+		Matrix3d M = CreateMatrix(
 		    cosine + xxcosine, -zsine, xzcosine,
 		    zsine, cosine, -xsine,
 		    xzcosine, xsine, cosine + zzcosine);
@@ -694,8 +508,8 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
 	if (m_limit_x == false && m_limit_z == false)
 		return false;
 
-	MT_Vector3 a = SphericalRangeParameters(m_new_basis);
-	MT_Scalar ax = 0, az = 0;
+	Vector3d a = SphericalRangeParameters(m_new_basis);
+	double ax = 0, az = 0;
 
 	clamp[0] = clamp[1] = false;
 	
@@ -732,13 +546,13 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
 
 	m_new_basis = ComputeSwingMatrix(ax, az);
 
-	delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
+	delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
 	delta[1] = delta[2]; delta[2] = 0.0;
 
 	return true;
 }
 
-void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
+void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta)
 {
 	m_locked[0] = m_locked[1] = true;
 	jacobian.Lock(m_DoF_id, delta[0]);
@@ -750,20 +564,20 @@ void IK_QSwingSegment::UpdateAngleApply()
 	m_basis = m_new_basis;
 }
 
-void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
 {
 	if (lmin > lmax)
 		return;
 	
 	// clamp and convert to axis angle parameters
-	lmin = MT_clamp(lmin, -MT_PI, MT_PI);
-	lmax = MT_clamp(lmax, -MT_PI, MT_PI);
+	lmin = Clamp(lmin, -M_PI, M_PI);
+	lmax = Clamp(lmax, -M_PI, M_PI);
 
 	lmin = sin(lmin * 0.5);
 	lmax = sin(lmax * 0.5);
 
 	// put center of ellispe in the middle between min and max
-	MT_Scalar offset = 0.5 * (lmin + lmax);
+	double offset = 0.5 * (lmin + lmax);
 	//lmax = lmax - offset;
 
 	if (axis == 0) {
@@ -784,7 +598,7 @@ void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	}
 }
 
-void IK_QSwingSegment::SetWeight(int axis, MT_Scalar weight)
+void IK_QSwingSegment::SetWeight(int axis, double weight)
 {
 	if (axis == 0)
 		m_weight[0] = weight;
@@ -800,7 +614,7 @@ IK_QElbowSegment::IK_QElbowSegment(int axis)
 {
 }
 
-void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis)
+void IK_QElbowSegment::SetBasis(const Matrix3d& basis)
 {
 	m_basis = basis;
 
@@ -811,22 +625,22 @@ void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis)
 	m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
 }
 
-MT_Vector3 IK_QElbowSegment::Axis(int dof) const
+Vector3d IK_QElbowSegment::Axis(int dof) const
 {
 	if (dof == 0) {
-		MT_Vector3 v;
+		Vector3d v;
 		if (m_axis == 0)
-			v = MT_Vector3(m_cos_twist, 0, m_sin_twist);
+			v = Vector3d(m_cos_twist, 0, m_sin_twist);
 		else
-			v = MT_Vector3(-m_sin_twist, 0, m_cos_twist);
+			v = Vector3d(-m_sin_twist, 0, m_cos_twist);
 
-		return m_global_transform.getBasis() * v;
+		return m_global_transform.linear() * v;
 	}
 	else
-		return m_global_transform.getBasis().getColumn(1);
+		return m_global_transform.linear().col(1);
 }
 
-bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
 {
 	if (m_locked[0] && m_locked[1])
 		return false;
@@ -870,7 +684,7 @@ bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
 	return (clamp[0] || clamp[1]);
 }
 
-void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
+void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
 {
 	if (dof == 0) {
 		m_locked[0] = true;
@@ -890,20 +704,20 @@ void IK_QElbowSegment::UpdateAngleApply()
 	m_sin_twist = sin(m_twist);
 	m_cos_twist = cos(m_twist);
 
-	MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis);
-	MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
+	Matrix3d A = RotationMatrix(m_angle, m_axis);
+	Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
 
 	m_basis = A * T;
 }
 
-void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
 {
 	if (lmin > lmax)
 		return;
 
 	// clamp and convert to axis angle parameters
-	lmin = MT_clamp(lmin, -MT_PI, MT_PI);
-	lmax = MT_clamp(lmax, -MT_PI, MT_PI);
+	lmin = Clamp(lmin, -M_PI, M_PI);
+	lmax = Clamp(lmax, -M_PI, M_PI);
 
 	if (axis == 1) {
 		m_min_twist = lmin;
@@ -917,7 +731,7 @@ void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	}
 }
 
-void IK_QElbowSegment::SetWeight(int axis, MT_Scalar weight)
+void IK_QElbowSegment::SetWeight(int axis, double weight)
 {
 	if (axis == m_axis)
 		m_weight[0] = weight;
@@ -963,16 +777,16 @@ IK_QTranslateSegment::IK_QTranslateSegment()
 	m_limit[0] = m_limit[1] = m_limit[2] = false;
 }
 
-MT_Vector3 IK_QTranslateSegment::Axis(int dof) const
+Vector3d IK_QTranslateSegment::Axis(int dof) const
 {
-	return m_global_transform.getBasis().getColumn(m_axis[dof]);
+	return m_global_transform.linear().col(m_axis[dof]);
 }
 
-bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
+bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
 {
 	int dof_id = m_DoF_id, dof = 0, i, clamped = false;
 
-	MT_Vector3 dx(0.0, 0.0, 0.0);
+	Vector3d dx(0.0, 0.0, 0.0);
 
 	for (i = 0; i < 3; i++) {
 		if (!m_axis_enabled[i]) {
@@ -1011,13 +825,13 @@ void IK_QTranslateSegment::UpdateAngleApply()
 	m_translation = m_new_translation;
 }
 
-void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
+void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
 {
 	m_locked[dof] = true;
 	jacobian.Lock(m_DoF_id + dof, delta[dof]);
 }
 
-void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight)
+void IK_QTranslateSegment::SetWeight(int axis, double weight)
 {
 	int i;
 
@@ -1026,7 +840,7 @@ void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight)
 			m_weight[i] = weight;
 }
 
-void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
+void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax)
 {
 	if (lmax < lmin)
 		return;
@@ -1036,7 +850,7 @@ void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	m_limit[axis] = true;
 }
 
-void IK_QTranslateSegment::Scale(MT_Scalar scale)
+void IK_QTranslateSegment::Scale(double scale)
 {
 	int i;
 
diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h
index b40bf3739ff..74f157aa763 100644
--- a/intern/iksolver/intern/IK_QSegment.h
+++ b/intern/iksolver/intern/IK_QSegment.h
@@ -30,13 +30,9 @@
  *  \ingroup iksolver
  */
 
+#pragma once
 
-#ifndef __IK_QSEGMENT_H__
-#define __IK_QSEGMENT_H__
-
-#include "MT_Vector3.h"
-#include "MT_Transform.h"
-#include "MT_Matrix4x4.h"
+#include "IK_Math.h"
 #include "IK_QJacobian.h"
 
 #include <vector>
@@ -50,8 +46,7 @@
  * 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
- * this as:
+ * You can read this as:
  * - first translate by (0,length,0)
  * - multiply by the rotation matrix derived from the current
  * angle parameterization q.
@@ -73,10 +68,10 @@ public:
 	// length: length of this segment
 
 	void SetTransform(
-		const MT_Vector3& start,
-		const MT_Matrix3x3& rest_basis,
-		const MT_Matrix3x3& basis,
-		const MT_Scalar length
+		const Vector3d& start,
+		const Matrix3d& rest_basis,
+		const Matrix3d& basis,
+		const double length
 	);
 
 	// tree structure access
@@ -109,22 +104,22 @@ public:
 	{ m_DoF_id = dof_id; }
 
 	// the max distance of the end of this bone from the local origin.
-	const MT_Scalar MaxExtension() const
+	const double 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;
+	Matrix3d BasisChange() const;
+	Vector3d TranslationChange() const;
 
 	// the start and end of the segment
-	const MT_Point3 &GlobalStart() const
+	const Vector3d GlobalStart() const
 	{ return m_global_start; }
 
-	const MT_Point3 &GlobalEnd() const
-	{ return m_global_transform.getOrigin(); }
+	const Vector3d GlobalEnd() const
+	{ return m_global_transform.translation(); }
 
 	// the global transformation at the end of the segment
-	const MT_Transform &GlobalTransform() const
+	const Affine3d &GlobalTransform() const
 	{ return m_global_transform; }
 
 	// is a translational segment?
@@ -139,38 +134,38 @@ public:
 	{ m_locked[0] = m_locked[1] = m_locked[2] = false; }
 
 	// per dof joint weighting
-	MT_Scalar Weight(int dof) const
+	double Weight(int dof) const
 	{ return m_weight[dof]; }
 
-	void ScaleWeight(int dof, MT_Scalar scale)
+	void ScaleWeight(int dof, double 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);
+	void UpdateTransform(const Affine3d &global);
 
 	// get axis from rotation matrix for derivative computation
-	virtual MT_Vector3 Axis(int dof) const=0;
+	virtual Vector3d Axis(int dof) const=0;
 
 	// 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 bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*)=0;
+	virtual void Lock(int, IK_QJacobian&, Vector3d&) {}
 	virtual void UpdateAngleApply()=0;
 
 	// set joint limits
-	virtual void SetLimit(int, MT_Scalar, MT_Scalar) {}
+	virtual void SetLimit(int, double, double) {}
 
 	// set joint weights (per axis)
-	virtual void SetWeight(int, MT_Scalar) {}
+	virtual void SetWeight(int, double) {}
 
-	virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
+	virtual void SetBasis(const Matrix3d& basis) { m_basis = basis; }
 
 	// functions needed for pole vector constraint
-	void PrependBasis(const MT_Matrix3x3& mat);
+	void PrependBasis(const Matrix3d& mat);
 	void Reset();
 
 	// scale
-	virtual void Scale(MT_Scalar scale);
+	virtual void Scale(double scale);
 
 protected:
 
@@ -188,28 +183,28 @@ protected:
 
 	// full transform = 
 	// start * rest_basis * basis * translation
-	MT_Vector3 m_start;
-	MT_Matrix3x3 m_rest_basis;
-	MT_Matrix3x3 m_basis;
-	MT_Vector3 m_translation;
+	Vector3d m_start;
+	Matrix3d m_rest_basis;
+	Matrix3d m_basis;
+	Vector3d m_translation;
 
 	// original basis
-	MT_Matrix3x3 m_orig_basis;
-	MT_Vector3 m_orig_translation;
+	Matrix3d m_orig_basis;
+	Vector3d m_orig_translation;
 
 	// maximum extension of this segment
-	MT_Scalar m_max_extension;
+	double m_max_extension;
 
 	// accumulated transformations starting from root
-	MT_Point3 m_global_start;
-	MT_Transform m_global_transform;
+	Vector3d m_global_start;
+	Affine3d m_global_transform;
 
 	// 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];
+	double m_weight[3];
 };
 
 class IK_QSphericalSegment : public IK_QSegment
@@ -217,23 +212,23 @@ class IK_QSphericalSegment : public IK_QSegment
 public:
 	IK_QSphericalSegment();
 
-	MT_Vector3 Axis(int dof) const;
+	Vector3d 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);
+	bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
+	void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
 	void UpdateAngleApply();
 
-	bool ComputeClampRotation(MT_Vector3& clamp);
+	bool ComputeClampRotation(Vector3d& clamp);
 
-	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
-	void SetWeight(int axis, MT_Scalar weight);
+	void SetLimit(int axis, double lmin, double lmax);
+	void SetWeight(int axis, double weight);
 
 private:
-	MT_Matrix3x3 m_new_basis;
+	Matrix3d m_new_basis;
 	bool m_limit_x, m_limit_y, m_limit_z;
-	MT_Scalar m_min[2], m_max[2];
-	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;
+	double m_min[2], m_max[2];
+	double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
+	double m_locked_ax, m_locked_ay, m_locked_az;
 };
 
 class IK_QNullSegment : public IK_QSegment
@@ -241,11 +236,11 @@ class IK_QNullSegment : public IK_QSegment
 public:
 	IK_QNullSegment();
 
-	bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
+	bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*) { return false; }
 	void UpdateAngleApply() {}
 
-	MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
-	void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
+	Vector3d Axis(int) const { return Vector3d(0, 0, 0); }
+	void SetBasis(const Matrix3d&) { m_basis.setIdentity(); }
 };
 
 class IK_QRevoluteSegment : public IK_QSegment
@@ -254,21 +249,21 @@ public:
 	// axis: the axis of the DoF, in range 0..2
 	IK_QRevoluteSegment(int axis);
 
-	MT_Vector3 Axis(int dof) const;
+	Vector3d 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);
+	bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
+	void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
 	void UpdateAngleApply();
 
-	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
-	void SetWeight(int axis, MT_Scalar weight);
-	void SetBasis(const MT_Matrix3x3& basis);
+	void SetLimit(int axis, double lmin, double lmax);
+	void SetWeight(int axis, double weight);
+	void SetBasis(const Matrix3d& basis);
 
 private:
 	int m_axis;
-	MT_Scalar m_angle, m_new_angle;
+	double m_angle, m_new_angle;
 	bool m_limit;
-	MT_Scalar m_min, m_max;
+	double m_min, m_max;
 };
 
 class IK_QSwingSegment : public IK_QSegment
@@ -277,21 +272,21 @@ public:
 	// XZ DOF, uses one direct rotation
 	IK_QSwingSegment();
 
-	MT_Vector3 Axis(int dof) const;
+	Vector3d 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);
+	bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
+	void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
 	void UpdateAngleApply();
 
-	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
-	void SetWeight(int axis, MT_Scalar weight);
-	void SetBasis(const MT_Matrix3x3& basis);
+	void SetLimit(int axis, double lmin, double lmax);
+	void SetWeight(int axis, double weight);
+	void SetBasis(const Matrix3d& basis);
 
 private:
-	MT_Matrix3x3 m_new_basis;
+	Matrix3d m_new_basis;
 	bool m_limit_x, m_limit_z;
-	MT_Scalar m_min[2], m_max[2];
-	MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
+	double m_min[2], m_max[2];
+	double m_max_x, m_max_z, m_offset_x, m_offset_z;
 };
 
 class IK_QElbowSegment : public IK_QSegment
@@ -301,24 +296,24 @@ public:
 	// X or Z, then rotate around Y (twist)
 	IK_QElbowSegment(int axis);
 
-	MT_Vector3 Axis(int dof) const;
+	Vector3d 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);
+	bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
+	void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
 	void UpdateAngleApply();
 
-	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
-	void SetWeight(int axis, MT_Scalar weight);
-	void SetBasis(const MT_Matrix3x3& basis);
+	void SetLimit(int axis, double lmin, double lmax);
+	void SetWeight(int axis, double weight);
+	void SetBasis(const Matrix3d& basis);
 
 private:
 	int m_axis;
 
-	MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
-	MT_Scalar m_cos_twist, m_sin_twist;
+	double m_twist, m_angle, m_new_twist, m_new_angle;
+	double m_cos_twist, m_sin_twist;
 
 	bool m_limit, m_limit_twist;
-	MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
+	double m_min, m_max, m_min_twist, m_max_twist;
 };
 
 class IK_QTranslateSegment : public IK_QSegment
@@ -329,23 +324,21 @@ public:
 	IK_QTranslateSegment(int axis1, int axis2);
 	IK_QTranslateSegment();
 
-	MT_Vector3 Axis(int dof) const;
+	Vector3d Axis(int dof) const;
 
-	bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
-	void Lock(int, IK_QJacobian&, MT_Vector3&);
+	bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
+	void Lock(int, IK_QJacobian&, Vector3d&);
 	void UpdateAngleApply();
 
-	void SetWeight(int axis, MT_Scalar weight);
-	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
+	void SetWeight(int axis, double weight);
+	void SetLimit(int axis, double lmin, double lmax);
 
-	void Scale(MT_Scalar scale);
+	void Scale(double scale);
 
 private:
 	int m_axis[3];
 	bool m_axis_enabled[3], m_limit[3];
-	MT_Vector3 m_new_translation;
-	MT_Scalar m_min[3], m_max[3];
+	Vector3d m_new_translation;
+	double m_min[3], m_max[3];
 };
 
-#endif
-
diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp
index 0ba716ff59d..d7c73865789 100644
--- a/intern/iksolver/intern/IK_QTask.cpp
+++ b/intern/iksolver/intern/IK_QTask.cpp
@@ -51,7 +51,7 @@ IK_QTask::IK_QTask(
 IK_QPositionTask::IK_QPositionTask(
     bool primary,
     const IK_QSegment *segment,
-    const MT_Vector3& goal
+    const Vector3d& goal
     ) :
 	IK_QTask(3, primary, true, segment), m_goal(goal)
 {
@@ -73,10 +73,10 @@ IK_QPositionTask::IK_QPositionTask(
 void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
 {
 	// compute beta
-	const MT_Vector3& pos = m_segment->GlobalEnd();
+	const Vector3d& pos = m_segment->GlobalEnd();
 
-	MT_Vector3 d_pos = m_goal - pos;
-	MT_Scalar length = d_pos.length();
+	Vector3d d_pos = m_goal - pos;
+	double length = d_pos.norm();
 
 	if (length > m_clamp_length)
 		d_pos = (m_clamp_length / length) * d_pos;
@@ -88,26 +88,26 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
 	const IK_QSegment *seg;
 
 	for (seg = m_segment; seg; seg = seg->Parent()) {
-		MT_Vector3 p = seg->GlobalStart() - pos;
+		Vector3d p = seg->GlobalStart() - pos;
 
 		for (i = 0; i < seg->NumberOfDoF(); i++) {
-			MT_Vector3 axis = seg->Axis(i) * m_weight;
+			Vector3d axis = seg->Axis(i) * m_weight;
 
 			if (seg->Translational())
 				jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
 			else {
-				MT_Vector3 pa = p.cross(axis);
+				Vector3d pa = p.cross(axis);
 				jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
 			}
 		}
 	}
 }
 
-MT_Scalar IK_QPositionTask::Distance() const
+double IK_QPositionTask::Distance() const
 {
-	const MT_Vector3& pos = m_segment->GlobalEnd();
-	MT_Vector3 d_pos = m_goal - pos;
-	return d_pos.length();
+	const Vector3d& pos = m_segment->GlobalEnd();
+	Vector3d d_pos = m_goal - pos;
+	return d_pos.norm();
 }
 
 // IK_QOrientationTask
@@ -115,7 +115,7 @@ MT_Scalar IK_QPositionTask::Distance() const
 IK_QOrientationTask::IK_QOrientationTask(
     bool primary,
     const IK_QSegment *segment,
-    const MT_Matrix3x3& goal
+    const Matrix3d& goal
     ) :
 	IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
 {
@@ -124,17 +124,16 @@ IK_QOrientationTask::IK_QOrientationTask(
 void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
 {
 	// compute betas
-	const MT_Matrix3x3& rot = m_segment->GlobalTransform().getBasis();
+	const Matrix3d& rot = m_segment->GlobalTransform().linear();
 
-	MT_Matrix3x3 d_rotm = m_goal * rot.transposed();
-	d_rotm.transpose();
+	Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
 
-	MT_Vector3 d_rot;
-	d_rot = -0.5 * MT_Vector3(d_rotm[2][1] - d_rotm[1][2],
-	                          d_rotm[0][2] - d_rotm[2][0],
-	                          d_rotm[1][0] - d_rotm[0][1]);
+	Vector3d d_rot;
+	d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
+	                        d_rotm(0, 2) - d_rotm(2, 0),
+	                        d_rotm(1, 0) - d_rotm(0, 1));
 
-	m_distance = d_rot.length();
+	m_distance = d_rot.norm();
 
 	jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
 
@@ -146,9 +145,9 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
 		for (i = 0; i < seg->NumberOfDoF(); i++) {
 
 			if (seg->Translational())
-				jacobian.SetDerivatives(m_id, seg->DoFId() + i, MT_Vector3(0, 0, 0), 1e2);
+				jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
 			else {
-				MT_Vector3 axis = seg->Axis(i) * m_weight;
+				Vector3d axis = seg->Axis(i) * m_weight;
 				jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
 			}
 		}
@@ -160,18 +159,18 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
 IK_QCenterOfMassTask::IK_QCenterOfMassTask(
     bool primary,
     const IK_QSegment *segment,
-    const MT_Vector3& goal_center
+    const Vector3d& goal_center
     ) :
 	IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
 {
 	m_total_mass_inv = ComputeTotalMass(m_segment);
-	if (!MT_fuzzyZero(m_total_mass_inv))
+	if (!FuzzyZero(m_total_mass_inv))
 		m_total_mass_inv = 1.0 / m_total_mass_inv;
 }
 
-MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
+double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
 {
-	MT_Scalar mass = /*seg->Mass()*/ 1.0;
+	double mass = /*seg->Mass()*/ 1.0;
 
 	const IK_QSegment *seg;
 	for (seg = segment->Child(); seg; seg = seg->Sibling())
@@ -180,9 +179,9 @@ MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
 	return mass;
 }
 
-MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
+Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
 {
-	MT_Vector3 center = /*seg->Mass()**/ segment->GlobalStart();
+	Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
 
 	const IK_QSegment *seg;
 	for (seg = segment->Child(); seg; seg = seg->Sibling())
@@ -191,19 +190,19 @@ MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
 	return center;
 }
 
-void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment)
+void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment)
 {
 	int i;
-	MT_Vector3 p = center - segment->GlobalStart();
+	Vector3d p = center - segment->GlobalStart();
 
 	for (i = 0; i < segment->NumberOfDoF(); i++) {
-		MT_Vector3 axis = segment->Axis(i) * m_weight;
+		Vector3d axis = segment->Axis(i) * m_weight;
 		axis *= /*segment->Mass()**/ m_total_mass_inv;
 		
 		if (segment->Translational())
 			jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
 		else {
-			MT_Vector3 pa = axis.cross(p);
+			Vector3d pa = axis.cross(p);
 			jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
 		}
 	}
@@ -215,12 +214,12 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& c
 
 void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
 {
-	MT_Vector3 center = ComputeCenter(m_segment) * m_total_mass_inv;
+	Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
 
 	// compute beta
-	MT_Vector3 d_pos = m_goal_center - center;
+	Vector3d d_pos = m_goal_center - center;
 
-	m_distance = d_pos.length();
+	m_distance = d_pos.norm();
 
 #if 0
 	if (m_distance > m_clamp_length)
@@ -233,7 +232,7 @@ void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
 	JacobianSegment(jacobian, center, m_segment);
 }
 
-MT_Scalar IK_QCenterOfMassTask::Distance() const
+double IK_QCenterOfMassTask::Distance() const
 {
 	return m_distance;
 }
diff --git a/intern/iksolver/intern/IK_QTask.h b/intern/iksolver/intern/IK_QTask.h
index baf1c346d62..141e6d41b47 100644
--- a/intern/iksolver/intern/IK_QTask.h
+++ b/intern/iksolver/intern/IK_QTask.h
@@ -30,13 +30,9 @@
  *  \ingroup iksolver
  */
 
+#pragma once
 
-#ifndef __IK_QTASK_H__
-#define __IK_QTASK_H__
-
-#include "MT_Vector3.h"
-#include "MT_Transform.h"
-#include "MT_Matrix4x4.h"
+#include "IK_Math.h"
 #include "IK_QJacobian.h"
 #include "IK_QSegment.h"
 
@@ -66,19 +62,19 @@ public:
 	bool Active() const
 	{ return m_active; }
 
-	MT_Scalar Weight() const
+	double Weight() const
 	{ return m_weight*m_weight; }
 
-	void SetWeight(MT_Scalar weight)
+	void SetWeight(double weight)
 	{ m_weight = sqrt(weight); }
 
 	virtual void ComputeJacobian(IK_QJacobian& jacobian)=0;
 
-	virtual MT_Scalar Distance() const=0;
+	virtual double Distance() const=0;
 
 	virtual bool PositionTask() const { return false; }
 
-	virtual void Scale(MT_Scalar) {}
+	virtual void Scale(double) {}
 
 protected:
 	int m_id;
@@ -86,7 +82,7 @@ protected:
 	bool m_primary;
 	bool m_active;
 	const IK_QSegment *m_segment;
-	MT_Scalar m_weight;
+	double m_weight;
 };
 
 class IK_QPositionTask : public IK_QTask
@@ -95,19 +91,19 @@ public:
 	IK_QPositionTask(
 		bool primary,
 		const IK_QSegment *segment,
-		const MT_Vector3& goal
+		const Vector3d& goal
 	);
 
 	void ComputeJacobian(IK_QJacobian& jacobian);
 
-	MT_Scalar Distance() const;
+	double Distance() const;
 
 	bool PositionTask() const { return true; }
-	void Scale(MT_Scalar scale) { m_goal *= scale; m_clamp_length *= scale; }
+	void Scale(double scale) { m_goal *= scale; m_clamp_length *= scale; }
 
 private:
-	MT_Vector3 m_goal;
-	MT_Scalar m_clamp_length;
+	Vector3d m_goal;
+	double m_clamp_length;
 };
 
 class IK_QOrientationTask : public IK_QTask
@@ -116,15 +112,15 @@ public:
 	IK_QOrientationTask(
 		bool primary,
 		const IK_QSegment *segment,
-		const MT_Matrix3x3& goal
+		const Matrix3d& goal
 	);
 
-	MT_Scalar Distance() const { return m_distance; }
+	double Distance() const { return m_distance; }
 	void ComputeJacobian(IK_QJacobian& jacobian);
 
 private:
-	MT_Matrix3x3 m_goal;
-	MT_Scalar m_distance;
+	Matrix3d m_goal;
+	double m_distance;
 };
 
 
@@ -134,24 +130,22 @@ public:
 	IK_QCenterOfMassTask(
 		bool primary,
 		const IK_QSegment *segment,
-		const MT_Vector3& center
+		const Vector3d& center
 	);
 
 	void ComputeJacobian(IK_QJacobian& jacobian);
 
-	MT_Scalar Distance() const;
+	double Distance() const;
 
-	void Scale(MT_Scalar scale) { m_goal_center *= scale; m_distance *= scale; }
+	void Scale(double scale) { m_goal_center *= scale; m_distance *= scale; }
 
 private:
-	MT_Scalar ComputeTotalMass(const IK_QSegment *segment);
-	MT_Vector3 ComputeCenter(const IK_QSegment *segment);
-	void JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment);
+	double ComputeTotalMass(const IK_QSegment *segment);
+	Vector3d ComputeCenter(const IK_QSegment *segment);
+	void JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment);
 
-	MT_Vector3 m_goal_center;
-	MT_Scalar m_total_mass_inv;
-	MT_Scalar m_distance;
+	Vector3d m_goal_center;
+	double m_total_mass_inv;
+	double m_distance;
 };
 
-#endif
-
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index eb18cde3356..cefb8c7ed7b 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -154,19 +154,19 @@ void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float bas
 {
 	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);
+	Vector3d mstart(start[0], start[1], start[2]);
+	// convert from blender column major
+	Matrix3d mbasis = CreateMatrix(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]);
+	Matrix3d mrest = CreateMatrix(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]);
+	double mlength(length);
 
 	if (qseg->Composite()) {
-		MT_Vector3 cstart(0, 0, 0);
-		MT_Matrix3x3 cbasis;
+		Vector3d cstart(0, 0, 0);
+		Matrix3d cbasis;
 		cbasis.setIdentity();
 		
 		qseg->SetTransform(mstart, mrest, mbasis, 0.0);
@@ -205,7 +205,7 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
 		stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
 
 	IK_QSegment *qseg = (IK_QSegment *)seg;
-	MT_Scalar weight = 1.0f - stiffness;
+	double weight = 1.0f - stiffness;
 
 	if (axis >= IK_TRANS_X) {
 		if (!qseg->Translational()) {
@@ -230,18 +230,18 @@ void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
 	if (qseg->Translational() && qseg->Composite())
 		qseg = qseg->Composite();
 
-	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];
+	const Matrix3d& change = qseg->BasisChange();
+
+	// convert 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)
@@ -251,7 +251,7 @@ void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
 	if (!qseg->Translational() && qseg->Composite())
 		qseg = qseg->Composite();
 	
-	const MT_Vector3& change = qseg->TranslationChange();
+	const Vector3d& change = qseg->TranslationChange();
 
 	translation_change[0] = (float)change[0];
 	translation_change[1] = (float)change[1];
@@ -296,7 +296,7 @@ void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float w
 	if (qtip->Composite())
 		qtip = qtip->Composite();
 
-	MT_Vector3 pos(goal);
+	Vector3d pos(goal[0], goal[1], goal[2]);
 
 	IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
 	ee->SetWeight(weight);
@@ -315,10 +315,10 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[
 	if (qtip->Composite())
 		qtip = qtip->Composite();
 
-	// 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]);
+	// convert from blender column major
+	Matrix3d rot = CreateMatrix(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(true, qtip, rot);
 	orient->SetWeight(weight);
@@ -337,8 +337,8 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float
 	if (qtip->Composite())
 		qtip = qtip->Composite();
 
-	MT_Vector3 qgoal(goal);
-	MT_Vector3 qpolegoal(polegoal);
+	Vector3d qgoal(goal[0], goal[1], goal[2]);
+	Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
 
 	qsolver->solver.SetPoleVectorConstraint(
 	    qtip, qgoal, qpolegoal, poleangle, getangle);
@@ -363,8 +363,8 @@ static void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float
 	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);
+	// convert from blender column major
+	Vector3d center(goal);
 
 	IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
 	com->SetWeight(weight);
@@ -382,7 +382,7 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
 	IK_QSegment *root = qsolver->root;
 	IK_QJacobianSolver& jacobian = qsolver->solver;
 	std::list<IK_QTask *>& tasks = qsolver->tasks;
-	MT_Scalar tol = tolerance;
+	double tol = tolerance;
 
 	if (!jacobian.Setup(root, tasks))
 		return 0;
diff --git a/intern/iksolver/intern/MT_ExpMap.cpp b/intern/iksolver/intern/MT_ExpMap.cpp
deleted file mode 100644
index b2b13acebeb..00000000000
--- a/intern/iksolver/intern/MT_ExpMap.cpp
+++ /dev/null
@@ -1,250 +0,0 @@
-/*
- * ***** BEGIN GPL 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.
- *
- * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
- *
- * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
- * All rights reserved.
- *
- * The Original Code is: all of this file.
- *
- * Original Author: Laurence
- * Contributor(s): Brecht
- *
- * ***** END GPL LICENSE BLOCK *****
- */
-
-/** \file iksolver/intern/MT_ExpMap.cpp
- *  \ingroup iksolver
- */
-
-
-#include "MT_ExpMap.h"
-
-/** 
- * Set the exponential map from a quaternion. The quaternion must be non-zero.
- */
-
-void
-MT_ExpMap::
-setRotation(
-    const MT_Quaternion &q)
-{
-	// ok first normalize the quaternion
-	// then compute theta the axis-angle and the normalized axis v
-	// scale v by theta and that's it hopefully!
-
-	m_q = q.normalized();
-	m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z());
-
-	MT_Scalar cosp = m_q.w();
-	m_sinp = m_v.length();
-	m_v /= m_sinp;
-
-	m_theta = atan2(double(m_sinp), double(cosp));
-	m_v *= m_theta;
-}
- 	
-/** 
- * Convert from an exponential map to a quaternion
- * representation
- */	
-
-const MT_Quaternion&
-MT_ExpMap::
-getRotation() const
-{
-	return m_q;
-}
-	
-/** 
- * Convert the exponential map to a 3x3 matrix
- */
-
-MT_Matrix3x3
-MT_ExpMap::
-getMatrix() const
-{
-	return MT_Matrix3x3(m_q);
-}
-
-/** 
- * Update & reparameterizate the exponential map
- */
-
-void
-MT_ExpMap::
-update(
-    const MT_Vector3& dv)
-{
-	m_v += dv;
-
-	angleUpdated();
-}
-
-/**
- * Compute the partial derivatives of the exponential
- * map  (dR/de - where R is a 3x3 rotation matrix formed 
- * from the map) and return them as a 3x3 matrix
- */
-
-void
-MT_ExpMap::
-partialDerivatives(
-    MT_Matrix3x3& dRdx,
-    MT_Matrix3x3& dRdy,
-    MT_Matrix3x3& dRdz) const
-{
-	MT_Quaternion dQdx[3];
-
-	compute_dQdVi(dQdx);
-
-	compute_dRdVi(dQdx[0], dRdx);
-	compute_dRdVi(dQdx[1], dRdy);
-	compute_dRdVi(dQdx[2], dRdz);
-}
-
-void
-MT_ExpMap::
-compute_dRdVi(
-    const MT_Quaternion &dQdvi,
-    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) * 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
-
-void
-MT_ExpMap::
-compute_dQdVi(
-    MT_Quaternion *dQdX) const
-{
-	/* 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] = 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(0.5 * m_theta));
-	}
-}
-
diff --git a/intern/iksolver/intern/MT_ExpMap.h b/intern/iksolver/intern/MT_ExpMap.h
deleted file mode 100644
index 65bbe4d4ad5..00000000000
--- a/intern/iksolver/intern/MT_ExpMap.h
+++ /dev/null
@@ -1,213 +0,0 @@
-/*
- * ***** BEGIN GPL 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.
- *
- * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
- *
- * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
- * All rights reserved.
- *
- * The Original Code is: all of this file.
- *
- * Original author: Laurence
- * Contributor(s): Brecht
- *
- * ***** END GPL LICENSE BLOCK *****
- */
-
-/** \file iksolver/intern/MT_ExpMap.h
- *  \ingroup iksolver
- */
-
-
-#ifndef MT_ExpMap_H
-#define MT_ExpMap_H
-
-#include <MT_assert.h>
-
-#include "MT_Vector3.h"
-#include "MT_Quaternion.h"
-#include "MT_Matrix4x4.h"
-
-const MT_Scalar MT_EXPMAP_MINANGLE (1e-7);
-
-/**
- * MT_ExpMap an exponential map parameterization of rotations
- * in 3D. This implementation is derived from the paper 
- * "F. Sebastian Grassia. Practical parameterization of 
- * rotations using the exponential map. Journal of Graphics Tools,
- *  3(3):29-48, 1998" Please go to http://www.acm.org/jgt/papers/Grassia98/
- * for a thorough description of the theory and sample code used 
- * to derive this class. 
- *
- * Basic overview of why this class is used.
- * In an IK system we need to paramterize the joint angles in some
- * way. Typically 2 parameterizations are used.
- * - Euler Angles
- * These suffer from singularities in the parameterization known
- * as gimbal lock. They also do not interpolate well. For every
- * set of euler angles there is exactly 1 corresponding 3d rotation.
- * - Quaternions.
- * Great for interpolating. Only unit quaternions are valid rotations
- * means that in a differential ik solver we often stray outside of
- * this manifold into invalid rotations. Means we have to do a lot
- * of nasty normalizations all the time. Does not suffer from 
- * gimbal lock problems. More expensive to compute partial derivatives
- * as there are 4 of them.
- * 
- * So exponential map is similar to a quaternion axis/angle 
- * representation but we store the angle as the length of the
- * axis. So require only 3 parameters. Means that all exponential
- * maps are valid rotations. Suffers from gimbal lock. But it's
- * possible to detect when gimbal lock is near and reparameterize
- * away from it. Also nice for interpolating.
- * Exponential maps are share some of the useful properties of
- * euler and quaternion parameterizations. And are very useful
- * for differential IK solvers.
- */
-
-class MT_ExpMap {
-public:
-
-	/**
-	 * Default constructor
-	 * @warning there is no initialization in the
-	 * default constructor
-	 */ 
-
-    MT_ExpMap() {}
-    MT_ExpMap(const MT_Vector3& v) : m_v(v) { angleUpdated(); }
-
-    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) { angleUpdated(); }
-
-	/** 
-	 * Construct an exponential map from a quaternion
-	 */
-
-	MT_ExpMap(
-		const MT_Quaternion &q
-	) {
-		setRotation(q);
-	}
-
-	/** 
-	 * Accessors
-	 * Decided not to inherit from MT_Vector3 but rather
-	 * this class contains an MT_Vector3. This is because
-	 * it is very dangerous to use MT_Vector3 functions
-	 * on this class and some of them have no direct meaning.
-	 */
-
-	const 
-		MT_Vector3 &
-	vector(
-	) const {
-		return m_v;
-	}
-
-	/** 
-	 * Set the exponential map from a quaternion
-	 */
-
-		void
-	setRotation(
-		const MT_Quaternion &q
-	);
-
-	/** 
-	 * Convert from an exponential map to a quaternion
-	 * representation
-	 */	
-	
-		const MT_Quaternion&
-	getRotation(
-	) const;
-
-	/** 
-	 * Convert the exponential map to a 3x3 matrix
-	 */
-
-		MT_Matrix3x3
-	getMatrix(
-	) const; 
-	
-	/** 
-	 * Update (and reparameterize) the expontial map
-	 * @param dv delta update values.
-	 */
-
-		void
-	update(
-		const MT_Vector3& dv
-	);
-
-	/**
-	 * Compute the partial derivatives of the exponential
-	 * map  (dR/de - where R is a 4x4 matrix formed 
-	 * from the map) and return them as a 4x4 matrix
-	 */
-
-		void
-	partialDerivatives(
-		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 &dQdV,
-		MT_Matrix3x3 & dRdVi
-	) const;
-
-	// compute partial derivatives dQ/dVi
-
-		void
-	compute_dQdVi(
-		MT_Quaternion *dQdX
-	) const ; 
-
-	// reparametrize away from singularity
-
-		void
-	reParametrize(
-	);
-
-	// (re-)compute cached variables
-
-		void
-	angleUpdated(
-	);
-};
-
-#endif
-