forked from bartvdbraak/blender
Brecht Van Lommel
6e4802d712
Performance is about the same or slightly better for typical IK chains. In extreme cases with many bones and multiple targets, of which some are unreachable, I've seen 2x speedups.
262 lines
6.1 KiB
C
262 lines
6.1 KiB
C
/*
|
|
* ***** 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::Affine3d;
|
|
using Eigen::Matrix3d;
|
|
using Eigen::MatrixXd;
|
|
using Eigen::Vector3d;
|
|
using Eigen::VectorXd;
|
|
|
|
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;
|
|
}
|
|
|