diff options
Diffstat (limited to 'intern/iksolver/intern/IK_Math.h')
| -rw-r--r-- | intern/iksolver/intern/IK_Math.h | 259 |
1 files changed, 259 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_Math.h b/intern/iksolver/intern/IK_Math.h new file mode 100644 index 00000000000..ba0eb047c8a --- /dev/null +++ b/intern/iksolver/intern/IK_Math.h @@ -0,0 +1,259 @@ +/* + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original author: Laurence + * Contributor(s): Brecht + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#pragma once + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include <cmath> + +using Eigen::Matrix3d; +using Eigen::Vector3d; +using Eigen::Affine3d; + +static const double IK_EPSILON = 1e-20; + +static inline bool FuzzyZero(double x) +{ + return fabs(x) < IK_EPSILON; +} + +static inline double Clamp(const double x, const double min, const double max) +{ + return (x < min) ? min : (x > max) ? max : x; +} + +static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz, + double yx, double yy, double yz, + double zx, double zy, double zz) +{ + Eigen::Matrix3d M; + M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz; + M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz; + M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz; + return M; +} + +static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis) +{ + if (axis == 0) + return CreateMatrix(1.0, 0.0, 0.0, + 0.0, cosine, -sine, + 0.0, sine, cosine); + else if (axis == 1) + return CreateMatrix(cosine, 0.0, sine, + 0.0, 1.0, 0.0, + -sine, 0.0, cosine); + else + return CreateMatrix(cosine, -sine, 0.0, + sine, cosine, 0.0, + 0.0, 0.0, 1.0); +} + +static inline Eigen::Matrix3d RotationMatrix(double angle, int axis) +{ + return RotationMatrix(sin(angle), cos(angle), axis); +} + + +static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis) +{ + double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); + + if (t > 16.0 * IK_EPSILON) { + if (axis == 0) return -atan2(R(1, 2), R(2, 2)); + else if (axis == 1) return atan2(-R(0, 2), t); + else return -atan2(R(0, 1), R(0, 0)); + } + else { + if (axis == 0) return -atan2(-R(2, 1), R(1, 1)); + else if (axis == 1) return atan2(-R(0, 2), t); + else return 0.0f; + } +} + +static inline double safe_acos(double f) +{ + // acos that does not return NaN with rounding errors + if (f <= -1.0) + return M_PI; + else if (f >= 1.0) + return 0.0; + else + return acos(f); +} + +static inline Eigen::Vector3d normalize(const Eigen::Vector3d& v) +{ + // a sane normalize function that doesn't give (1, 0, 0) in case + // of a zero length vector + double len = v.norm(); + return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len); +} + +static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) +{ + return safe_acos(v1.dot(v2)); +} + +static inline double ComputeTwist(const Eigen::Matrix3d& R) +{ + // qy and qw are the y and w components of the quaternion from R + double qy = R(0, 2) - R(2, 0); + double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; + + double tau = 2.0 * atan2(qy, qw); + + return tau; +} + +static inline Eigen::Matrix3d ComputeTwistMatrix(double tau) +{ + return RotationMatrix(tau, 1); +} + +static inline void RemoveTwist(Eigen::Matrix3d& R) +{ + // compute twist parameter + double tau = ComputeTwist(R); + + // compute twist matrix + Eigen::Matrix3d T = ComputeTwistMatrix(tau); + + // remove twist + R = R * T.transpose(); +} + +static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d& R) +{ + // compute twist parameter + double tau = ComputeTwist(R); + + // compute swing parameters + double num = 2.0 * (1.0 + R(1, 1)); + + // singularity at pi + if (fabs(num) < IK_EPSILON) + // TODO: this does now rotation of size pi over z axis, but could + // be any axis, how to deal with this i'm not sure, maybe don't + // enforce limits at all then + return Eigen::Vector3d(0.0, tau, 1.0); + + num = 1.0 / sqrt(num); + double ax = -R(2, 1) * num; + double az = R(0, 1) * num; + + return Eigen::Vector3d(ax, tau, az); +} + +static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az) +{ + // length of (ax, 0, az) = sin(theta/2) + double sine2 = ax * ax + az * az; + double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2); + + // compute swing matrix + Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az)); + + return S; +} + +static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R) +{ + Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), + R(0, 2) - R(2, 0), + R(1, 0) - R(0, 1)); + + double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2); + double l = delta.norm(); + + if (!FuzzyZero(l)) + delta *= c / l; + + return delta; +} + +static inline bool EllipseClamp(double& ax, double& az, double *amin, double *amax) +{ + double xlim, zlim, x, z; + + if (ax < 0.0) { + x = -ax; + xlim = -amin[0]; + } + else { + x = ax; + xlim = amax[0]; + } + + if (az < 0.0) { + z = -az; + zlim = -amin[1]; + } + else { + z = az; + zlim = amax[1]; + } + + if (FuzzyZero(xlim) || FuzzyZero(zlim)) { + if (x <= xlim && z <= zlim) + return false; + + if (x > xlim) + x = xlim; + if (z > zlim) + z = zlim; + } + else { + double invx = 1.0 / (xlim * xlim); + double invz = 1.0 / (zlim * zlim); + + if ((x * x * invx + z * z * invz) <= 1.0) + return false; + + if (FuzzyZero(x)) { + x = 0.0; + z = zlim; + } + else { + double rico = z / x; + double old_x = x; + x = sqrt(1.0 / (invx + invz * rico * rico)); + if (old_x < 0.0) + x = -x; + z = rico * x; + } + } + + ax = (ax < 0.0) ? -x : x; + az = (az < 0.0) ? -z : z; + + return true; +} + |