Silence some annoying warnings when doing full build with strict flags
[blender.git] / extern / carve / lib / math.cpp
1 // Begin License:
2 // Copyright (C) 2006-2014 Tobias Sargeant (tobias.sargeant@gmail.com).
3 // All rights reserved.
4 //
5 // This file is part of the Carve CSG Library (http://carve-csg.com/)
6 //
7 // This file may be used under the terms of either the GNU General
8 // Public License version 2 or 3 (at your option) as published by the
9 // Free Software Foundation and appearing in the files LICENSE.GPL2
10 // and LICENSE.GPL3 included in the packaging of this file.
11 //
12 // This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
13 // INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
14 // A PARTICULAR PURPOSE.
15 // End:
16
17
18 #if defined(HAVE_CONFIG_H)
19 #  include <carve_config.h>
20 #endif
21
22 #include <carve/math.hpp>
23 #include <carve/matrix.hpp>
24
25 #include <iostream>
26 #include <limits>
27
28 #include <stdio.h>
29
30 #define M_2PI_3 2.0943951023931953
31 #define M_SQRT_3_4 0.8660254037844386
32 #define EPS std::numeric_limits<double>::epsilon()
33
34 namespace carve {
35   namespace math {
36
37     struct Root {
38       double root;
39       int multiplicity;
40
41       Root(double r) : root(r), multiplicity(1) {}
42       Root(double r, int m) : root(r), multiplicity(m) {}
43     };
44
45     namespace {
46 #if 0
47       void cplx_sqrt(double re, double im,
48                      double &re_1, double &im_1,
49                      double &re_2, double &im_2) {
50          if (re == 0.0 && im == 0.0) {
51           re_1 = re_2 = re;
52           im_1 = im_2 = im;
53         } else {
54           double d = sqrt(re * re + im * im);
55           re_1 = sqrt((d + re) / 2.0);
56           re_2 = re_1;
57           im_1 = fabs(sqrt((d - re) / 2.0));
58           im_2 = -im_1;
59         }
60       }
61 #endif
62
63 #if 0
64     void cplx_cbrt(double re, double im,
65                    double &re_1, double &im_1,
66                    double &re_2, double &im_2,
67                    double &re_3, double &im_3) {
68       if (re == 0.0 && im == 0.0) {
69         re_1 = re_2 = re_3 = re;
70         im_1 = im_2 = im_3 = im;
71       } else {
72         double r = cbrt(sqrt(re * re + im * im));
73         double t = atan2(im, re) / 3.0;
74         re_1 = r * cos(t);
75         im_1 = r * sin(t);
76         re_2 = r * cos(t + M_TWOPI / 3.0);
77         im_2 = r * sin(t + M_TWOPI / 3.0);
78         re_3 = r * cos(t + M_TWOPI * 2.0 / 3.0);
79         im_3 = r * sin(t + M_TWOPI * 2.0 / 3.0);
80       }
81     }
82 #endif
83
84       void add_root(std::vector<Root> &roots, double root) {
85         for (size_t i = 0; i < roots.size(); ++i) {
86           if (roots[i].root == root) {
87             roots[i].multiplicity++;
88             return;
89           }
90         }
91         roots.push_back(Root(root));
92       }
93
94       void linear_roots(double c1, double c0, std::vector<Root> &roots) {
95         roots.push_back(Root(c0 / c1));
96       }
97
98       void quadratic_roots(double c2, double c1, double c0, std::vector<Root> &roots) {
99         if (fabs(c2) < EPS) {
100           linear_roots(c1, c0, roots);
101           return;
102         }
103
104         double p = 0.5 * c1 / c2;
105         double dis = p * p - c0 / c2;
106
107         if (dis > 0.0) {
108           dis = sqrt(dis);
109           if (-p - dis != -p + dis) {
110             roots.push_back(Root(-p - dis));
111             roots.push_back(Root(-p + dis));
112           } else {
113             roots.push_back(Root(-p, 2));
114           }
115         }
116       }
117
118       void cubic_roots(double c3, double c2, double c1, double c0, std::vector<Root> &roots) {
119         int n_sol = 0;
120         double _r[3];
121
122         if (fabs(c3) < EPS) {
123           quadratic_roots(c2, c1, c0, roots);
124           return;
125         }
126
127         if (fabs(c0) < EPS) {
128           quadratic_roots(c3, c2, c1, roots);
129           add_root(roots, 0.0);
130           return;
131         }
132
133         double xN = -c2 / (3.0 * c3);
134         double yN = c0 + xN * (c1 + xN * (c2 + c3 * xN));
135
136         double delta_sq = (c2 * c2 - 3.0 * c3 * c1) / (9.0 * c3 * c3);
137         double h_sq = 4.0 / 9.0 * (c2 * c2 - 3.0 * c3 * c1) * (delta_sq * delta_sq);
138         double dis = yN * yN - h_sq;
139
140         if (dis > EPS) {
141           // One real root, two complex roots.
142
143           double dis_sqrt = sqrt(dis);
144           double r_p = yN - dis_sqrt;
145           double r_q = yN + dis_sqrt;
146           double p = cbrt(fabs(r_p)/(2.0 * c3));
147           double q = cbrt(fabs(r_q)/(2.0 * c3));
148
149           if (r_p > 0.0) p = -p;
150           if (r_q > 0.0) q = -q;
151
152           _r[0] = xN + p + q;
153           n_sol = 1;
154
155           double re = xN - p * .5 - q * .5;
156           double im = p * M_SQRT_3_4 - q * M_SQRT_3_4;
157
158           // root 2: xN + p * exp(M_2PI_3.i) + q * exp(-M_2PI_3.i);
159           // root 3: complex conjugate of root 2
160
161           if (im < EPS) {
162             _r[1] = _r[2] = re;
163             n_sol += 2;
164           }
165         } else if (dis < -EPS) {
166           // Three distinct real roots.
167           double theta = acos(-yN / sqrt(h_sq)) / 3.0;
168           double delta = sqrt(c2 * c2 - 3.0 * c3 * c1) / (3.0 * c3);
169
170           _r[0] = xN + (2.0 * delta) * cos(theta);
171           _r[1] = xN + (2.0 * delta) * cos(M_2PI_3 - theta);
172           _r[2] = xN + (2.0 * delta) * cos(M_2PI_3 + theta);
173           n_sol = 3;
174         } else {
175           // Three real roots (two or three equal).
176           double r = yN / (2.0 * c3);
177           double delta = cbrt(r);
178
179           _r[0] = xN + delta;
180           _r[1] = xN + delta;
181           _r[2] = xN - 2.0 * delta;
182           n_sol = 3;
183         }
184
185         for (int i=0; i < n_sol; i++) {
186           add_root(roots, _r[i]);
187         }
188       }
189     }
190
191     static void U(const Matrix3 &m,
192                   double l,
193                   double u[6],
194                   double &u_max,
195                   int &u_argmax) {
196       u[0] = (m._22 - l) * (m._33 - l) - m._23 * m._23;
197       u[1] = m._13 * m._23 - m._12 * (m._33 - l);
198       u[2] = m._12 * m._23 - m._13 * (m._22 - l);
199       u[3] = (m._11 - l) * (m._33 - l) - m._13 * m._13;
200       u[4] = m._12 * m._13 - m._23 * (m._11 - l);
201       u[5] = (m._11 - l) * (m._22 - l) - m._12 * m._12;
202
203       u_max = -1.0;
204       u_argmax = -1;
205
206       for (int i = 0; i < 6; ++i) {
207         if (u_max < fabs(u[i])) { u_max = fabs(u[i]); u_argmax = i; }
208       }
209     }
210
211     static void eig1(const Matrix3 &m, double l, carve::geom::vector<3> &e) {
212       double u[6];
213       double u_max;
214       int u_argmax;
215
216       U(m, l, u, u_max, u_argmax);
217
218       switch(u_argmax) {
219       case 0:
220         e.x = u[0]; e.y = u[1]; e.z = u[2]; break;
221       case 1: case 3:
222         e.x = u[1]; e.y = u[3]; e.z = u[4]; break;
223       case  2: case 4: case 5:
224         e.x = u[2]; e.y = u[4]; e.z = u[5]; break;
225       }
226       e.normalize();
227     }
228
229     static void eig2(const Matrix3 &m, double l, carve::geom::vector<3> &e1, carve::geom::vector<3> &e2) {
230       double u[6];
231       double u_max;
232       int u_argmax;
233
234       U(m, l, u, u_max, u_argmax);
235
236       switch(u_argmax) {
237       case 0: case 1:
238         e1.x = -m._12; e1.y = m._11; e1.z = 0.0;
239         e2.x = -m._13 * m._11; e2.y = -m._13 * m._12; e2.z = m._11 * m._11 + m._12 * m._12;
240         break;
241       case 2:
242         e1.x = m._12; e1.y = 0.0; e1.z = -m._11;
243         e2.x = -m._12 * m._11; e2.y = m._11 * m._11 + m._13 * m._13; e2.z = -m._12 * m._13;
244         break;
245       case 3: case 4:
246         e1.x = 0.0; e1.y = -m._23; e1.z = -m._22;
247         e2.x = m._22 * m._22 + m._23 * m._23; e2.y = -m._12 * m._22; e2.z = -m._12 * m._23;
248         break;
249       case 5:
250         e1.x = 0.0; e1.y = -m._33; e1.z = m._23;
251         e2.x = m._23 * m._23 + m._33 * m._33; e2.y = -m._13 * m._23; e2.z = -m._13 * m._33;
252       }
253       e1.normalize();
254       e2.normalize();
255     }
256
257 #if 0
258     static void eig3(const Matrix3 &m,
259                      double l,
260                      carve::geom::vector<3> &e1,
261                      carve::geom::vector<3> &e2,
262                      carve::geom::vector<3> &e3) {
263       e1.x = 1.0; e1.y = 0.0; e1.z = 0.0;
264       e2.x = 0.0; e2.y = 1.0; e2.z = 0.0;
265       e3.x = 0.0; e3.y = 0.0; e3.z = 1.0;
266     }
267 #endif
268
269     void eigSolveSymmetric(const Matrix3 &m,
270                            double &l1, carve::geom::vector<3> &e1,
271                            double &l2, carve::geom::vector<3> &e2,
272                            double &l3, carve::geom::vector<3> &e3) {
273       double c0 =
274         m._11 * m._22 * m._33 +
275         2.0 * m._12 * m._13 * m._23 -
276         m._11 * m._23 * m._23 -
277         m._22 * m._13 * m._13 -
278         m._33 * m._12 * m._12;
279       double c1 =
280         m._11 * m._22 -
281         m._12 * m._12 +
282         m._11 * m._33 -
283         m._13 * m._13 +
284         m._22 * m._33 -
285         m._23 * m._23;
286       double c2 =
287         m._11 +
288         m._22 +
289         m._33;
290
291       double a = (3.0 * c1 - c2 * c2) / 3.0;
292       double b = (-2.0 * c2 * c2 * c2 + 9.0 * c1 * c2 - 27.0 * c0) / 27.0;
293
294       double Q = b * b / 4.0 + a * a * a / 27.0;
295
296       if (fabs(Q) < 1e-16) {
297         l1 = m._11;   e1.x = 1.0; e1.y = 0.0; e1.z = 0.0;
298         l2 = m._22;   e2.x = 0.0; e2.y = 1.0; e2.z = 0.0;
299         l3 = m._33;   e3.x = 0.0; e3.y = 0.0; e3.z = 1.0;
300       } else if (Q > 0) {
301         l1 = l2 = c2 / 3.0 + cbrt(b / 2.0);
302         l3 = c2 / 3.0 - 2.0 * cbrt(b / 2.0);
303
304         eig2(m, l1, e1, e2);
305         eig1(m, l3, e3);
306       } else if (Q < 0) {
307         double t = atan2(sqrt(-Q), -b / 2.0);
308         double cos_t3 = cos(t / 3.0);
309         double sin_t3 = sin(t / 3.0);
310         double r = cbrt(sqrt(b * b / 4.0 - Q));
311
312         l1 = c2 / 3.0 + 2 * r * cos_t3;
313         l2 = c2 / 3.0 - r * (cos_t3 + M_SQRT_3 * sin_t3);
314         l3 = c2 / 3.0 - r * (cos_t3 - M_SQRT_3 * sin_t3);
315
316         eig1(m, l1, e1);
317         eig1(m, l2, e2);
318         eig1(m, l3, e3);
319       }
320     }
321
322     void eigSolve(const Matrix3 &m, double &l1, double &l2, double &l3) {
323       double c3, c2, c1, c0;
324       std::vector<Root> roots;
325
326       c3 = -1.0;
327       c2 = m._11 + m._22 + m._33;
328       c1 = 
329         -(m._22 * m._33 + m._11 * m._22 + m._11 * m._33)
330         +(m._23 * m._32 + m._13 * m._31 + m._12 * m._21);
331       c0 =
332         +(m._11 * m._22 - m._12 * m._21) * m._33
333         -(m._11 * m._23 - m._13 * m._21) * m._32
334         +(m._12 * m._23 - m._13 * m._22) * m._31;
335
336       cubic_roots(c3, c2, c1, c0, roots);
337
338       for (size_t i = 0; i < roots.size(); i++) {
339         Matrix3 M(m);
340         M._11 -= roots[i].root;
341         M._22 -= roots[i].root;
342         M._33 -= roots[i].root;
343         // solve M.v = 0
344       }
345
346       std::cerr << "n_roots=" << roots.size() << std::endl;
347       for (size_t i = 0; i < roots.size(); i++) {
348         fprintf(stderr, "  %.24f(%d)", roots[i].root, roots[i].multiplicity);
349       }
350       std::cerr << std::endl;
351     }
352
353   }
354 }
355