1 /*
2 * This program is free software; you can redistribute it and/or
3 * modify it under the terms of the GNU General Public License
4 * as published by the Free Software Foundation; either version 2
5 * of the License, or (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software Foundation,
14 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15 *
16 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
17 * All rights reserved.
18 * Original author: Laurence
19 */
20
21 #pragma once
22
23 #include <Eigen/Core>
24 #include <Eigen/Geometry>
25
26 #include <cmath>
27
28 using Eigen::Affine3d;
29 using Eigen::Matrix3d;
30 using Eigen::MatrixXd;
31 using Eigen::Vector3d;
32 using Eigen::VectorXd;
33
34 static const double IK_EPSILON = 1e-20;
35
FuzzyZero(double x)36 static inline bool FuzzyZero(double x)
37 {
38 return fabs(x) < IK_EPSILON;
39 }
40
Clamp(const double x,const double min,const double max)41 static inline double Clamp(const double x, const double min, const double max)
42 {
43 return (x < min) ? min : (x > max) ? max : x;
44 }
45
CreateMatrix(double xx,double xy,double xz,double yx,double yy,double yz,double zx,double zy,double zz)46 static inline Eigen::Matrix3d CreateMatrix(double xx,
47 double xy,
48 double xz,
49 double yx,
50 double yy,
51 double yz,
52 double zx,
53 double zy,
54 double zz)
55 {
56 Eigen::Matrix3d M;
57 M(0, 0) = xx;
58 M(0, 1) = xy;
59 M(0, 2) = xz;
60 M(1, 0) = yx;
61 M(1, 1) = yy;
62 M(1, 2) = yz;
63 M(2, 0) = zx;
64 M(2, 1) = zy;
65 M(2, 2) = zz;
66 return M;
67 }
68
RotationMatrix(double sine,double cosine,int axis)69 static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
70 {
71 if (axis == 0)
72 return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine);
73 else if (axis == 1)
74 return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine);
75 else
76 return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
77 }
78
RotationMatrix(double angle,int axis)79 static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
80 {
81 return RotationMatrix(sin(angle), cos(angle), axis);
82 }
83
EulerAngleFromMatrix(const Eigen::Matrix3d & R,int axis)84 static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis)
85 {
86 double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
87
88 if (t > 16.0 * IK_EPSILON) {
89 if (axis == 0)
90 return -atan2(R(1, 2), R(2, 2));
91 else if (axis == 1)
92 return atan2(-R(0, 2), t);
93 else
94 return -atan2(R(0, 1), R(0, 0));
95 }
96 else {
97 if (axis == 0)
98 return -atan2(-R(2, 1), R(1, 1));
99 else if (axis == 1)
100 return atan2(-R(0, 2), t);
101 else
102 return 0.0f;
103 }
104 }
105
safe_acos(double f)106 static inline double safe_acos(double f)
107 {
108 // acos that does not return NaN with rounding errors
109 if (f <= -1.0)
110 return M_PI;
111 else if (f >= 1.0)
112 return 0.0;
113 else
114 return acos(f);
115 }
116
normalize(const Eigen::Vector3d & v)117 static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v)
118 {
119 // a sane normalize function that doesn't give (1, 0, 0) in case
120 // of a zero length vector
121 double len = v.norm();
122 return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
123 }
124
angle(const Eigen::Vector3d & v1,const Eigen::Vector3d & v2)125 static inline double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2)
126 {
127 return safe_acos(v1.dot(v2));
128 }
129
ComputeTwist(const Eigen::Matrix3d & R)130 static inline double ComputeTwist(const Eigen::Matrix3d &R)
131 {
132 // qy and qw are the y and w components of the quaternion from R
133 double qy = R(0, 2) - R(2, 0);
134 double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
135
136 double tau = 2.0 * atan2(qy, qw);
137
138 return tau;
139 }
140
ComputeTwistMatrix(double tau)141 static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
142 {
143 return RotationMatrix(tau, 1);
144 }
145
RemoveTwist(Eigen::Matrix3d & R)146 static inline void RemoveTwist(Eigen::Matrix3d &R)
147 {
148 // compute twist parameter
149 double tau = ComputeTwist(R);
150
151 // compute twist matrix
152 Eigen::Matrix3d T = ComputeTwistMatrix(tau);
153
154 // remove twist
155 R = R * T.transpose();
156 }
157
SphericalRangeParameters(const Eigen::Matrix3d & R)158 static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
159 {
160 // compute twist parameter
161 double tau = ComputeTwist(R);
162
163 // compute swing parameters
164 double num = 2.0 * (1.0 + R(1, 1));
165
166 // singularity at pi
167 if (fabs(num) < IK_EPSILON)
168 // TODO: this does now rotation of size pi over z axis, but could
169 // be any axis, how to deal with this I'm not sure, maybe don't
170 // enforce limits at all then.
171 return Eigen::Vector3d(0.0, tau, 1.0);
172
173 num = 1.0 / sqrt(num);
174 double ax = -R(2, 1) * num;
175 double az = R(0, 1) * num;
176
177 return Eigen::Vector3d(ax, tau, az);
178 }
179
ComputeSwingMatrix(double ax,double az)180 static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
181 {
182 // length of (ax, 0, az) = sin(theta/2)
183 double sine2 = ax * ax + az * az;
184 double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
185
186 // compute swing matrix
187 Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
188
189 return S;
190 }
191
MatrixToAxisAngle(const Eigen::Matrix3d & R)192 static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
193 {
194 Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
195
196 double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
197 double l = delta.norm();
198
199 if (!FuzzyZero(l))
200 delta *= c / l;
201
202 return delta;
203 }
204
EllipseClamp(double & ax,double & az,double * amin,double * amax)205 static inline bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
206 {
207 double xlim, zlim, x, z;
208
209 if (ax < 0.0) {
210 x = -ax;
211 xlim = -amin[0];
212 }
213 else {
214 x = ax;
215 xlim = amax[0];
216 }
217
218 if (az < 0.0) {
219 z = -az;
220 zlim = -amin[1];
221 }
222 else {
223 z = az;
224 zlim = amax[1];
225 }
226
227 if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
228 if (x <= xlim && z <= zlim)
229 return false;
230
231 if (x > xlim)
232 x = xlim;
233 if (z > zlim)
234 z = zlim;
235 }
236 else {
237 double invx = 1.0 / (xlim * xlim);
238 double invz = 1.0 / (zlim * zlim);
239
240 if ((x * x * invx + z * z * invz) <= 1.0)
241 return false;
242
243 if (FuzzyZero(x)) {
244 x = 0.0;
245 z = zlim;
246 }
247 else {
248 double rico = z / x;
249 double old_x = x;
250 x = sqrt(1.0 / (invx + invz * rico * rico));
251 if (old_x < 0.0)
252 x = -x;
253 z = rico * x;
254 }
255 }
256
257 ax = (ax < 0.0) ? -x : x;
258 az = (az < 0.0) ? -z : z;
259
260 return true;
261 }
262