262 lines
6.2 KiB
C
262 lines
6.2 KiB
C
/*
|
|
* 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.
|
|
* Original author: Laurence
|
|
*/
|
|
|
|
#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;
|
|
}
|