IK solver: replace Moto math library with Eigen.
authorBrecht Van Lommel <brechtvanlommel@gmail.com>
Thu, 10 Dec 2015 18:12:26 +0000 (19:12 +0100)
committerBrecht Van Lommel <brechtvanlommel@gmail.com>
Thu, 10 Dec 2015 23:59:00 +0000 (00:59 +0100)
14 files changed:
intern/iksolver/CMakeLists.txt
intern/iksolver/SConscript
intern/iksolver/intern/IK_Math.h [new file with mode: 0644]
intern/iksolver/intern/IK_QJacobian.cpp
intern/iksolver/intern/IK_QJacobian.h
intern/iksolver/intern/IK_QJacobianSolver.cpp
intern/iksolver/intern/IK_QJacobianSolver.h
intern/iksolver/intern/IK_QSegment.cpp
intern/iksolver/intern/IK_QSegment.h
intern/iksolver/intern/IK_QTask.cpp
intern/iksolver/intern/IK_QTask.h
intern/iksolver/intern/IK_Solver.cpp
intern/iksolver/intern/MT_ExpMap.cpp [deleted file]
intern/iksolver/intern/MT_ExpMap.h [deleted file]

index 9476e0379e9d22cce0fccef1997bcdfac4ad57fc..4e5699f1abdaa256945b88518e171efb498d45e9 100644 (file)
@@ -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
index ba973ad5fd509b6c20c851c885c280a8ded80c96..12d9d14712b4cd38ff633df672c2e06a2a138a36 100644 (file)
@@ -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 (file)
index 0000000..ba0eb04
--- /dev/null
@@ -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;
+}
+
index bb7b7c5c0b85c88e916853e131435a5e6f4b4f9a..cd77d5d662dbea4f94c13a87ff6a6c0c4e351015 100644 (file)
@@ -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);
index b4b5a0402e6c96a42b703d5e8c9ac73cc8fcd074..2a5bac4937371c79e176eb538af49b04d29fca3b 100644 (file)
  *  \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
-
index 75f51f566c922c614cc40c49b29de35c96099417..b78270eb87f2b4c18accb05999ee59d2aead0f5a 100644 (file)
@@ -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);
 
index 646f952b9ffc0c277c995110ec7956ca0e6bf6be..545ef91c710e3bf9385a7072656468b993b973c9 100644 (file)
  *  \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
-
index e511d8233a2566928a47501feac300492112ec7d..23b094db2797a47e438f554e60f27b8aa1245554 100644 (file)
 
 
 #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;
 
index b40bf3739ffe874b248abd3512c1a3ad0ec66276..74f157aa76380be843e02bbd77c37d7c4aeae0ae 100644 (file)
  *  \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
-
index 0ba716ff59da0417af6c797f6fe256e952fd6e84..d7c7386578954c0609d57c8bb1e13bb81c9705a2 100644 (file)
@@ -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;
 }
index baf1c346d6233462560c786653626ed9eb9d6171..141e6d41b474701e7c5a84df2bed9e55d95ce3ac 100644 (file)
  *  \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
-
index eb18cde335685ad6aea8e4fd5a32a89c7f1978e0..cefb8c7ed7b9f273ed6a805f8927f88d6fdf77bb 100644 (file)
@@ -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 (file)
index b2b13ac..0000000
+++ /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 (file)
index 65bbe4d..0000000
+++ /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
-