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