[Bf-blender-cvs] [aaa627d] master: IK solver: replace Moto math library with Eigen.

Brecht Van Lommel noreply at git.blender.org
Fri Dec 11 01:02:11 CET 2015


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




More information about the Bf-blender-cvs mailing list