Commit: aaa627d5f5560f365da3dee14e9b8a3355f21013
Author: Brecht Van Lommel
Date: Thu Dec 10 19:12:26 2015 +0100
Branches: master
https://developer.blender.org/rBaaa627d5f5560f365da3dee14e9b8a3355f21013
IK solver: replace Moto math library with Eigen.
===================================================================
M intern/iksolver/CMakeLists.txt
M intern/iksolver/SConscript
A intern/iksolver/intern/IK_Math.h
M intern/iksolver/intern/IK_QJacobian.cpp
M intern/iksolver/intern/IK_QJacobian.h
M intern/iksolver/intern/IK_QJacobianSolver.cpp
M intern/iksolver/intern/IK_QJacobianSolver.h
M intern/iksolver/intern/IK_QSegment.cpp
M intern/iksolver/intern/IK_QSegment.h
M intern/iksolver/intern/IK_QTask.cpp
M intern/iksolver/intern/IK_QTask.h
M intern/iksolver/intern/IK_Solver.cpp
D intern/iksolver/intern/MT_ExpMap.cpp
D intern/iksolver/intern/MT_ExpMap.h
===================================================================
diff --git a/intern/iksolver/CMakeLists.txt b/intern/iksolver/CMakeLists.txt
index 9476e03..4e5699f 100644
--- a/intern/iksolver/CMakeLists.txt
+++ b/intern/iksolver/CMakeLists.txt
@@ -29,7 +29,7 @@ set(INC
)
set(INC_SYS
- ../moto/include
+ ${EIGEN3_INCLUDE_DIRS}
)
set(SRC
@@ -38,14 +38,12 @@ set(SRC
intern/IK_QSegment.cpp
intern/IK_QTask.cpp
intern/IK_Solver.cpp
- intern/MT_ExpMap.cpp
extern/IK_solver.h
intern/IK_QJacobian.h
intern/IK_QJacobianSolver.h
intern/IK_QSegment.h
intern/IK_QTask.h
- intern/MT_ExpMap.h
intern/TNT/cholesky.h
intern/TNT/cmat.h
intern/TNT/fcscmat.h
diff --git a/intern/iksolver/SConscript b/intern/iksolver/SConscript
index ba973ad..12d9d14 100644
--- a/intern/iksolver/SConscript
+++ b/intern/iksolver/SConscript
@@ -29,7 +29,7 @@ Import ('env')
sources = env.Glob('intern/*.cpp')
-incs = 'intern ../moto/include ../memutil'
+incs = 'intern #/extern/Eigen3'
env.BlenderLib ('bf_intern_iksolver', sources, Split(incs), [],
libtype=['intern','player'], priority=[100,90] )
diff --git a/intern/iksolver/intern/IK_Math.h b/intern/iksolver/intern/IK_Math.h
new file mode 100644
index 0000000..ba0eb04
--- /dev/null
+++ b/intern/iksolver/intern/IK_Math.h
@@ -0,0 +1,259 @@
+/*
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Original author: Laurence
+ * Contributor(s): Brecht
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <cmath>
+
+using Eigen::Matrix3d;
+using Eigen::Vector3d;
+using Eigen::Affine3d;
+
+static const double IK_EPSILON = 1e-20;
+
+static inline bool FuzzyZero(double x)
+{
+ return fabs(x) < IK_EPSILON;
+}
+
+static inline double Clamp(const double x, const double min, const double max)
+{
+ return (x < min) ? min : (x > max) ? max : x;
+}
+
+static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz,
+ double yx, double yy, double yz,
+ double zx, double zy, double zz)
+{
+ Eigen::Matrix3d M;
+ M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz;
+ M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz;
+ M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz;
+ return M;
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int
axis)
+{
+ if (axis == 0)
+ return CreateMatrix(1.0, 0.0, 0.0,
+ 0.0, cosine, -sine,
+ 0.0, sine, cosine);
+ else if (axis == 1)
+ return CreateMatrix(cosine, 0.0, sine,
+ 0.0, 1.0, 0.0,
+ -sine, 0.0, cosine);
+ else
+ return CreateMatrix(cosine, -sine, 0.0,
+ sine, cosine, 0.0,
+ 0.0, 0.0, 1.0);
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
+{
+ return RotationMatrix(sin(angle), cos(angle), axis);
+}
+
+
+static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis)
+{
+ double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
+
+ if (t > 16.0 * IK_EPSILON) {
+ if (axis == 0) return -atan2(R(1, 2), R(2, 2));
+ else if (axis == 1) return atan2(-R(0, 2), t);
+ else return -atan2(R(0, 1), R(0, 0));
+ }
+ else {
+ if (axis == 0) return -atan2(-R(2, 1), R(1, 1));
+ else if (axis == 1) return atan2(-R(0, 2), t);
+ else return 0.0f;
+ }
+}
+
+static inline double safe_acos(double f)
+{
+ // acos that does not return NaN with rounding errors
+ if (f <= -1.0)
+ return M_PI;
+ else if (f >= 1.0)
+ return 0.0;
+ else
+ return acos(f);
+}
+
+static inline Eigen::Vector3d normalize(const Eigen::Vector3d& v)
+{
+ // a sane normalize function that doesn't give (1, 0, 0) in case
+ // of a zero length vector
+ double len = v.norm();
+ return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v /
len);
+}
+
+static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d&
v2)
+{
+ return safe_acos(v1.dot(v2));
+}
+
+static inline double ComputeTwist(const Eigen::Matrix3d& R)
+{
+ // qy and qw are the y and w components of the quaternion from R
+ double qy = R(0, 2) - R(2, 0);
+ double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
+
+ double tau = 2.0 * atan2(qy, qw);
+
+ return tau;
+}
+
+static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
+{
+ return RotationMatrix(tau, 1);
+}
+
+static inline void RemoveTwist(Eigen::Matrix3d& R)
+{
+ // compute twist parameter
+ double tau = ComputeTwist(R);
+
+ // compute twist matrix
+ Eigen::Matrix3d T = ComputeTwistMatrix(tau);
+
+ // remove twist
+ R = R * T.transpose();
+}
+
+static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d&
R)
+{
+ // compute twist parameter
+ double tau = ComputeTwist(R);
+
+ // compute swing parameters
+ double num = 2.0 * (1.0 + R(1, 1));
+
+ // singularity at pi
+ if (fabs(num) < IK_EPSILON)
+ // TODO: this does now rotation of size pi over z axis, but
could
+ // be any axis, how to deal with this i'm not sure, maybe don't
+ // enforce limits at all then
+ return Eigen::Vector3d(0.0, tau, 1.0);
+
+ num = 1.0 / sqrt(num);
+ double ax = -R(2, 1) * num;
+ double az = R(0, 1) * num;
+
+ return Eigen::Vector3d(ax, tau, az);
+}
+
+static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
+{
+ // length of (ax, 0, az) = sin(theta/2)
+ double sine2 = ax * ax + az * az;
+ double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
+
+ // compute swing matrix
+ Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
+
+ return S;
+}
+
+static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R)
+{
+ Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2),
+ R(0, 2) - R(2, 0),
+ R(1, 0) - R(0, 1));
+
+ double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
+ double l = delta.norm();
+
+ if (!FuzzyZero(l))
+ delta *= c / l;
+
+ return delta;
+}
+
+static inline bool EllipseClamp(double& ax, double& az, double *amin, double
*amax)
+{
+ double xlim, zlim, x, z;
+
+ if (ax < 0.0) {
+ x = -ax;
+ xlim = -amin[0];
+ }
+ else {
+ x = ax;
+ xlim = amax[0];
+ }
+
+ if (az < 0.0) {
+ z = -az;
+ zlim = -amin[1];
+ }
+ else {
+ z = az;
+ zlim = amax[1];
+ }
+
+ if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
+ if (x <= xlim && z <= zlim)
+ return false;
+
+ if (x > xlim)
+ x = xlim;
+ if (z > zlim)
+ z = zlim;
+ }
+ else {
+ double invx = 1.0 / (xlim * xlim);
+ double invz = 1.0 / (zlim * zlim);
+
+ if ((x * x * invx + z * z * invz) <= 1.0)
+ return false;
+
+ if (FuzzyZero(x)) {
+ x = 0.0;
+ z = zlim;
+ }
+ else {
+ double rico = z / x;
+ double old_x = x;
+ x = sqrt(1.0 / (invx + invz * rico * rico));
+ if (old_x < 0.0)
+ x = -x;
+ z = rico * x;
+ }
+ }
+
+ ax = (ax < 0.0) ? -x : x;
+ az = (az < 0.0) ? -z : z;
+
+ return true;
+}
+
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp
b/intern/iksolver/intern/IK_QJacobian.cpp
index bb7b7c5..cd77d5d 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -104,14 +104,14 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size)
}
}
-void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v)
+void IK_QJacobian::SetBetas(int id, int, const Vector3d& v)
{
m_beta[id + 0] = v.x();
m_beta[id + 1] = v.y();
m_beta[id + 2] = v.z();
}
-void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v,
MT_Scalar norm_weight)
+void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v,
double norm_weight)
{
m_jacobian[id + 0][dof_id] = v.x() * m_weight_sqrt[dof_id];
m_jacobian[id + 1][dof_id] = v.y() * m_weight_sqrt[dof_id];
@@ -143,7 +143,7 @@ void IK_QJacobian::Invert()
bool IK_QJacobian::ComputeNullProjection()
{
- MT_Scalar epsilon = 1e-10;
+ double epsilon = 1e-10;
// compute null space projection based on V
int i, j, rank = 0;
@@ -230,8 +230,8 @@ void IK_QJacobian::InvertSDLS()
// DLS. The SDLS damps individual singular values, instead of using a
single
// damping term.
- MT_Scalar max_angle_change = MT_PI / 4.0;
- MT_Scalar epsilon = 1e-10;
+ double max_angle_change = M_PI / 4.0;
+ double epsilon = 1e-10;
int i, j;
m_d_theta = 0;
@@ -240,7 +240,7 @@ void IK_QJacobian::InvertSDLS()
for (i = 0; i < m_dof; i++) {
m_norm[i] = 0.0;
for (j = 0; j < m_task_size; j += 3) {
- MT_Scalar n = 0.0;
+ double n = 0.0;
n += m_jacobian[j][i] * m_jacobian[j][i];
n += m_jacobian[j + 1][i] * m_jacobian[j + 1][i];
n += m_jacobian[j + 2][i] * m_jacobian[j + 2][i];
@@ -252,9 +252,9 @@ void IK_QJacobian::InvertSDLS()
if (m_svd_w[i] <= epsilon)
continue;
- MT_Scalar wInv = 1.0 / m_svd_w[i];
- MT_Scalar alpha = 0.0;
- MT_Scalar N = 0.0;
+ double wInv = 1.0 / m_svd_w[i];
+ double alpha = 0.0;
+ double N = 0.0;
// compute alpha and N
for (j = 0; j < m_svd_u.num_rows(); j += 3) {
@@ -264,7 +264,7 @@ void IK_QJacobian::InvertSDLS()
// note: for 1 end effector, N will always be 1, since
U is
// orthogonal, .. so could be optimized
- MT_Scalar tmp;
+ double tmp;
tmp = m_svd_u[j][i] * m_svd_u[j][i];
tmp += m_svd_u[j + 1][i] * m_svd_u[j + 1][i];
tmp += m_svd_u[j + 2][i] * m_svd_u[j + 2][i];
@@ -273,19 +273,19 @@ void IK_QJacobian::InvertSDLS()
alpha *= wInv;
// compute M, dTheta and max_dtheta
-
@@ Diff output truncated at 10240 characters. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
http://lists.blender.org/mailman/listinfo/bf-blender-cvs
