griefith/test2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

IK solver: replace Moto math library with Eigen.

Browse Source
This commit is contained in:
Brecht Van Lommel
2015-12-10 19:12:26 +01:00
parent 810f825a39
commit aaa627d5f5
14 changed files with 641 additions and 1076 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
intern/iksolver/CMakeLists.txt
View 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

2
intern/iksolver/SConscript
View 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] )

259
intern/iksolver/intern/IK_Math.h Normal file
View File

@@ -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;
}

68
intern/iksolver/intern/IK_QJacobian.cpp
View 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);

27
intern/iksolver/intern/IK_QJacobian.h
View File

@@ -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

102
intern/iksolver/intern/IK_QJacobianSolver.cpp
View 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);

28
intern/iksolver/intern/IK_QJacobianSolver.h
View File

@@ -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

398
intern/iksolver/intern/IK_QSegment.cpp
View File

@@ -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;

171
intern/iksolver/intern/IK_QSegment.h
View File

@@ -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

71
intern/iksolver/intern/IK_QTask.cpp
View 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;
}

56
intern/iksolver/intern/IK_QTask.h
View File

@@ -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

68
intern/iksolver/intern/IK_Solver.cpp
View 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();
const Matrix3d& 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];
// 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;

250
intern/iksolver/intern/MT_ExpMap.cpp
View File

@@ -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));
}
}

213
intern/iksolver/intern/MT_ExpMap.h
View File

@@ -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