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