ca9d3bb413f969f1bd96fb1a179f449b240f0b83
[blender.git] / source / blender / blenlib / intern / math_geom.c
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: some of this file.
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  * */
25
26 /** \file blender/blenlib/intern/math_geom.c
27  *  \ingroup bli
28  */
29
30 #include "MEM_guardedalloc.h"
31
32 #include "BLI_math.h"
33 #include "BLI_memarena.h"
34 #include "BLI_utildefines.h"
35
36 #include "BLI_strict_flags.h"
37
38 /********************************** Polygons *********************************/
39
40 void cent_tri_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3])
41 {
42         cent[0] = (v1[0] + v2[0] + v3[0]) / 3.0f;
43         cent[1] = (v1[1] + v2[1] + v3[1]) / 3.0f;
44         cent[2] = (v1[2] + v2[2] + v3[2]) / 3.0f;
45 }
46
47 void cent_quad_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
48 {
49         cent[0] = 0.25f * (v1[0] + v2[0] + v3[0] + v4[0]);
50         cent[1] = 0.25f * (v1[1] + v2[1] + v3[1] + v4[1]);
51         cent[2] = 0.25f * (v1[2] + v2[2] + v3[2] + v4[2]);
52 }
53
54 float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
55 {
56         float n1[3], n2[3];
57
58         n1[0] = v1[0] - v2[0];
59         n2[0] = v2[0] - v3[0];
60         n1[1] = v1[1] - v2[1];
61         n2[1] = v2[1] - v3[1];
62         n1[2] = v1[2] - v2[2];
63         n2[2] = v2[2] - v3[2];
64         n[0] = n1[1] * n2[2] - n1[2] * n2[1];
65         n[1] = n1[2] * n2[0] - n1[0] * n2[2];
66         n[2] = n1[0] * n2[1] - n1[1] * n2[0];
67
68         return normalize_v3(n);
69 }
70
71 float normal_quad_v3(float n[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
72 {
73         /* real cross! */
74         float n1[3], n2[3];
75
76         n1[0] = v1[0] - v3[0];
77         n1[1] = v1[1] - v3[1];
78         n1[2] = v1[2] - v3[2];
79
80         n2[0] = v2[0] - v4[0];
81         n2[1] = v2[1] - v4[1];
82         n2[2] = v2[2] - v4[2];
83
84         n[0] = n1[1] * n2[2] - n1[2] * n2[1];
85         n[1] = n1[2] * n2[0] - n1[0] * n2[2];
86         n[2] = n1[0] * n2[1] - n1[1] * n2[0];
87
88         return normalize_v3(n);
89 }
90
91 /* only convex Quadrilaterals */
92 float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
93 {
94         float len, vec1[3], vec2[3], n[3];
95
96         sub_v3_v3v3(vec1, v2, v1);
97         sub_v3_v3v3(vec2, v4, v1);
98         cross_v3_v3v3(n, vec1, vec2);
99         len = len_v3(n);
100
101         sub_v3_v3v3(vec1, v4, v3);
102         sub_v3_v3v3(vec2, v2, v3);
103         cross_v3_v3v3(n, vec1, vec2);
104         len += len_v3(n);
105
106         return (len / 2.0f);
107 }
108
109 /* Triangles */
110 float area_tri_v3(const float v1[3], const float v2[3], const float v3[3])
111 {
112         float vec1[3], vec2[3], n[3];
113
114         sub_v3_v3v3(vec1, v3, v2);
115         sub_v3_v3v3(vec2, v1, v2);
116         cross_v3_v3v3(n, vec1, vec2);
117
118         return len_v3(n) / 2.0f;
119 }
120
121 float area_tri_signed_v3(const float v1[3], const float v2[3], const float v3[3], const float normal[3])
122 {
123         float area, vec1[3], vec2[3], n[3];
124
125         sub_v3_v3v3(vec1, v3, v2);
126         sub_v3_v3v3(vec2, v1, v2);
127         cross_v3_v3v3(n, vec1, vec2);
128         area = len_v3(n) / 2.0f;
129
130         /* negate area for flipped triangles */
131         if (dot_v3v3(n, normal) < 0.0f)
132                 area = -area;
133
134         return area;
135 }
136
137 float area_poly_v3(const float verts[][3], unsigned int nr, const float normal[3])
138 {
139         unsigned int a;
140         int px, py;
141         const float max = axis_dominant_v3_max(&px, &py, normal);
142         float area;
143         const float *co_curr, *co_prev;
144
145         /* The Trapezium Area Rule */
146         co_prev = verts[nr - 1];
147         co_curr = verts[0];
148         area = 0.0f;
149         for (a = 0; a < nr; a++) {
150                 area += (co_curr[px] - co_prev[px]) * (co_curr[py] + co_prev[py]);
151                 co_prev = co_curr;
152                 co_curr += 3;
153         }
154
155         return fabsf(0.5f * area / max);
156 }
157
158 float cross_poly_v2(const float verts[][2], unsigned int nr)
159 {
160         unsigned int a;
161         float cross;
162         const float *co_curr, *co_prev;
163
164         /* The Trapezium Area Rule */
165         co_prev = verts[nr - 1];
166         co_curr = verts[0];
167         cross = 0.0f;
168         for (a = 0; a < nr; a++) {
169                 cross += (co_curr[0] - co_prev[0]) * (co_curr[1] + co_prev[1]);
170                 co_prev = co_curr;
171                 co_curr += 2;
172         }
173
174         return cross;
175 }
176
177 float area_poly_v2(const float verts[][2], unsigned int nr)
178 {
179         return fabsf(0.5f * cross_poly_v2(verts, nr));
180 }
181
182 /********************************* Planes **********************************/
183
184 /**
185  * Calculate a plane from a point and a direction,
186  * \note \a point_no isn't required to be normalized.
187  */
188 void plane_from_point_normal_v3(float r_plane[4], const float plane_co[3], const float plane_no[3])
189 {
190         copy_v3_v3(r_plane, plane_no);
191         r_plane[3] = -dot_v3v3(r_plane, plane_co);
192 }
193
194 /**
195  * Get a point and a normal from a plane.
196  */
197 void plane_to_point_normal_v3(const float plane[4], float r_plane_co[3], float r_plane_no[3])
198 {
199         const float length = normalize_v3_v3(r_plane_no, plane);
200         madd_v3_v3v3fl(r_plane_co, r_plane_no, r_plane_no, (-plane[3] / length) - 1.0f);
201 }
202
203
204 /********************************* Volume **********************************/
205
206 /**
207  * The volume from a tetrahedron, points can be in any order
208  */
209 float volume_tetrahedron_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
210 {
211         float m[3][3];
212         sub_v3_v3v3(m[0], v1, v2);
213         sub_v3_v3v3(m[1], v2, v3);
214         sub_v3_v3v3(m[2], v3, v4);
215         return fabsf(determinant_m3_array(m)) / 6.0f;
216 }
217
218
219 /********************************* Distance **********************************/
220
221 /* distance p to line v1-v2
222  * using Hesse formula, NO LINE PIECE! */
223 float dist_squared_to_line_v2(const float p[2], const float l1[2], const float l2[2])
224 {
225         float a[2], deler;
226
227         a[0] = l1[1] - l2[1];
228         a[1] = l2[0] - l1[0];
229
230         deler = len_squared_v2(a);
231
232         if (deler != 0.0f) {
233                 float f = ((p[0] - l1[0]) * a[0] +
234                            (p[1] - l1[1]) * a[1]);
235                 return (f * f) / deler;
236         }
237         else {
238                 return 0.0f;
239         }
240 }
241 float dist_to_line_v2(const float p[2], const float l1[2], const float l2[2])
242 {
243         float a[2], deler;
244
245         a[0] = l1[1] - l2[1];
246         a[1] = l2[0] - l1[0];
247
248         deler = len_squared_v2(a);
249
250         if (deler != 0.0f) {
251                 float f = ((p[0] - l1[0]) * a[0] +
252                            (p[1] - l1[1]) * a[1]);
253                 return fabsf(f) / sqrtf(deler);
254         }
255         else {
256                 return 0.0f;
257         }
258 }
259
260 /* distance p to line-piece v1-v2 */
261 float dist_squared_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
262 {
263         float lambda, rc[2], pt[2], len;
264
265         rc[0] = l2[0] - l1[0];
266         rc[1] = l2[1] - l1[1];
267         len = rc[0] * rc[0] + rc[1] * rc[1];
268         if (len == 0.0f) {
269                 rc[0] = p[0] - l1[0];
270                 rc[1] = p[1] - l1[1];
271                 return (rc[0] * rc[0] + rc[1] * rc[1]);
272         }
273
274         lambda = (rc[0] * (p[0] - l1[0]) + rc[1] * (p[1] - l1[1])) / len;
275         if (lambda <= 0.0f) {
276                 pt[0] = l1[0];
277                 pt[1] = l1[1];
278         }
279         else if (lambda >= 1.0f) {
280                 pt[0] = l2[0];
281                 pt[1] = l2[1];
282         }
283         else {
284                 pt[0] = lambda * rc[0] + l1[0];
285                 pt[1] = lambda * rc[1] + l1[1];
286         }
287
288         rc[0] = pt[0] - p[0];
289         rc[1] = pt[1] - p[1];
290         return (rc[0] * rc[0] + rc[1] * rc[1]);
291 }
292
293 float dist_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
294 {
295         return sqrtf(dist_squared_to_line_segment_v2(p, l1, l2));
296 }
297
298 /* point closest to v1 on line v2-v3 in 2D */
299 void closest_to_line_segment_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2])
300 {
301         float lambda, cp[2];
302
303         lambda = closest_to_line_v2(cp, p, l1, l2);
304
305         if (lambda <= 0.0f)
306                 copy_v2_v2(r_close, l1);
307         else if (lambda >= 1.0f)
308                 copy_v2_v2(r_close, l2);
309         else
310                 copy_v2_v2(r_close, cp);
311 }
312
313 /* point closest to v1 on line v2-v3 in 3D */
314 void closest_to_line_segment_v3(float r_close[3], const float v1[3], const float v2[3], const float v3[3])
315 {
316         float lambda, cp[3];
317
318         lambda = closest_to_line_v3(cp, v1, v2, v3);
319
320         if (lambda <= 0.0f)
321                 copy_v3_v3(r_close, v2);
322         else if (lambda >= 1.0f)
323                 copy_v3_v3(r_close, v3);
324         else
325                 copy_v3_v3(r_close, cp);
326 }
327
328 /**
329  * Find the closest point on a plane.
330  *
331  * \param r_close  Return coordinate
332  * \param plane  The plane to test against.
333  * \param pt  The point to find the nearest of
334  *
335  * \note non-unit-length planes are supported.
336  */
337 void closest_to_plane_v3(float r_close[3], const float plane[4], const float pt[3])
338 {
339         const float len_sq = len_squared_v3(plane);
340         const float side = plane_point_side_v3(plane, pt);
341         madd_v3_v3v3fl(r_close, pt, plane, -side / len_sq);
342 }
343
344 float dist_squared_to_plane_v3(const float pt[3], const float plane[4])
345 {
346         const float len_sq = len_squared_v3(plane);
347         const float side = plane_point_side_v3(plane, pt);
348         const float fac = side / len_sq;
349         return copysignf(len_sq * (fac * fac), side);
350 }
351
352 /**
353  * Return the signed distance from the point to the plane.
354  */
355 float dist_to_plane_v3(const float pt[3], const float plane[4])
356 {
357         const float len_sq = len_squared_v3(plane);
358         const float side = plane_point_side_v3(plane, pt);
359         const float fac = side / len_sq;
360         return sqrtf(len_sq) * fac;
361 }
362
363 /* distance v1 to line-piece l1-l2 in 3D */
364 float dist_squared_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
365 {
366         float closest[3];
367
368         closest_to_line_segment_v3(closest, p, l1, l2);
369
370         return len_squared_v3v3(closest, p);
371 }
372
373 float dist_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
374 {
375         return sqrtf(dist_squared_to_line_segment_v3(p, l1, l2));
376 }
377
378 float dist_squared_to_line_v3(const float v1[3], const float l1[3], const float l2[3])
379 {
380         float closest[3];
381
382         closest_to_line_v3(closest, v1, l1, l2);
383
384         return len_squared_v3v3(closest, v1);
385 }
386 float dist_to_line_v3(const float v1[3], const float l1[3], const float l2[3])
387 {
388         return sqrtf(dist_squared_to_line_v3(v1, l1, l2));
389 }
390
391 /* Adapted from "Real-Time Collision Detection" by Christer Ericson,
392  * published by Morgan Kaufmann Publishers, copyright 2005 Elsevier Inc.
393  * 
394  * Set 'r' to the point in triangle (a, b, c) closest to point 'p' */
395 void closest_on_tri_to_point_v3(float r[3], const float p[3],
396                                 const float a[3], const float b[3], const float c[3])
397 {
398         float ab[3], ac[3], ap[3], d1, d2;
399         float bp[3], d3, d4, vc, cp[3], d5, d6, vb, va;
400         float denom, v, w;
401
402         /* Check if P in vertex region outside A */
403         sub_v3_v3v3(ab, b, a);
404         sub_v3_v3v3(ac, c, a);
405         sub_v3_v3v3(ap, p, a);
406         d1 = dot_v3v3(ab, ap);
407         d2 = dot_v3v3(ac, ap);
408         if (d1 <= 0.0f && d2 <= 0.0f) {
409                 /* barycentric coordinates (1,0,0) */
410                 copy_v3_v3(r, a);
411                 return;
412         }
413
414         /* Check if P in vertex region outside B */
415         sub_v3_v3v3(bp, p, b);
416         d3 = dot_v3v3(ab, bp);
417         d4 = dot_v3v3(ac, bp);
418         if (d3 >= 0.0f && d4 <= d3) {
419                 /* barycentric coordinates (0,1,0) */
420                 copy_v3_v3(r, b);
421                 return;
422         }
423         /* Check if P in edge region of AB, if so return projection of P onto AB */
424         vc = d1 * d4 - d3 * d2;
425         if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
426                 v = d1 / (d1 - d3);
427                 /* barycentric coordinates (1-v,v,0) */
428                 madd_v3_v3v3fl(r, a, ab, v);
429                 return;
430         }
431         /* Check if P in vertex region outside C */
432         sub_v3_v3v3(cp, p, c);
433         d5 = dot_v3v3(ab, cp);
434         d6 = dot_v3v3(ac, cp);
435         if (d6 >= 0.0f && d5 <= d6) {
436                 /* barycentric coordinates (0,0,1) */
437                 copy_v3_v3(r, c);
438                 return;
439         }
440         /* Check if P in edge region of AC, if so return projection of P onto AC */
441         vb = d5 * d2 - d1 * d6;
442         if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
443                 w = d2 / (d2 - d6);
444                 /* barycentric coordinates (1-w,0,w) */
445                 madd_v3_v3v3fl(r, a, ac, w);
446                 return;
447         }
448         /* Check if P in edge region of BC, if so return projection of P onto BC */
449         va = d3 * d6 - d5 * d4;
450         if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
451                 w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
452                 /* barycentric coordinates (0,1-w,w) */
453                 sub_v3_v3v3(r, c, b);
454                 mul_v3_fl(r, w);
455                 add_v3_v3(r, b);
456                 return;
457         }
458
459         /* P inside face region. Compute Q through its barycentric coordinates (u,v,w) */
460         denom = 1.0f / (va + vb + vc);
461         v = vb * denom;
462         w = vc * denom;
463
464         /* = u*a + v*b + w*c, u = va * denom = 1.0f - v - w */
465         /* ac * w */
466         mul_v3_fl(ac, w);
467         /* a + ab * v */
468         madd_v3_v3v3fl(r, a, ab, v);
469         /* a + ab * v + ac * w */
470         add_v3_v3(r, ac);
471 }
472
473 /******************************* Intersection ********************************/
474
475 /* intersect Line-Line, shorts */
476 int isect_line_line_v2_int(const int v1[2], const int v2[2], const int v3[2], const int v4[2])
477 {
478         float div, lambda, mu;
479
480         div = (float)((v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]));
481         if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR;
482
483         lambda = (float)((v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
484
485         mu = (float)((v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
486
487         if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
488                 if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT;
489                 return ISECT_LINE_LINE_CROSS;
490         }
491         return ISECT_LINE_LINE_NONE;
492 }
493
494 /* intersect Line-Line, floats - gives intersection point */
495 int isect_line_line_v2_point(const float v1[2], const float v2[2], const float v3[2], const float v4[2], float vi[2])
496 {
497         float div;
498
499         div = (v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]);
500         if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR;
501
502         vi[0] = ((v3[0] - v4[0]) * (v1[0] * v2[1] - v1[1] * v2[0]) - (v1[0] - v2[0]) * (v3[0] * v4[1] - v3[1] * v4[0])) / div;
503         vi[1] = ((v3[1] - v4[1]) * (v1[0] * v2[1] - v1[1] * v2[0]) - (v1[1] - v2[1]) * (v3[0] * v4[1] - v3[1] * v4[0])) / div;
504
505         return ISECT_LINE_LINE_CROSS;
506 }
507
508
509 /* intersect Line-Line, floats */
510 int isect_line_line_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
511 {
512         float div, lambda, mu;
513
514         div = (v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]);
515         if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR;
516
517         lambda = ((float)(v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
518
519         mu = ((float)(v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
520
521         if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
522                 if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT;
523                 return ISECT_LINE_LINE_CROSS;
524         }
525         return ISECT_LINE_LINE_NONE;
526 }
527
528 /* get intersection point of two 2D segments and return intersection type:
529  *  -1: collinear
530  *   1: intersection
531  */
532 int isect_seg_seg_v2_point(const float v1[2], const float v2[2], const float v3[2], const float v4[2], float vi[2])
533 {
534         float a1, a2, b1, b2, c1, c2, d;
535         float u, v;
536         const float eps = 0.000001f;
537         const float eps_sq = eps * eps;
538
539         a1 = v2[0] - v1[0];
540         b1 = v4[0] - v3[0];
541         c1 = v1[0] - v4[0];
542
543         a2 = v2[1] - v1[1];
544         b2 = v4[1] - v3[1];
545         c2 = v1[1] - v4[1];
546
547         d = a1 * b2 - a2 * b1;
548
549         if (d == 0) {
550                 if (a1 * c2 - a2 * c1 == 0.0f && b1 * c2 - b2 * c1 == 0.0f) { /* equal lines */
551                         float a[2], b[2], c[2];
552                         float u2;
553
554                         if (equals_v2v2(v1, v2)) {
555                                 if (len_squared_v2v2(v3, v4) > eps_sq) {
556                                         /* use non-point segment as basis */
557                                         SWAP(const float *, v1, v3);
558                                         SWAP(const float *, v2, v4);
559                                 }
560                                 else { /* both of segments are points */
561                                         if (equals_v2v2(v1, v3)) { /* points are equal */
562                                                 copy_v2_v2(vi, v1);
563                                                 return 1;
564                                         }
565
566                                         /* two different points */
567                                         return -1;
568                                 }
569                         }
570
571                         sub_v2_v2v2(a, v3, v1);
572                         sub_v2_v2v2(b, v2, v1);
573                         sub_v2_v2v2(c, v2, v1);
574                         u = dot_v2v2(a, b) / dot_v2v2(c, c);
575
576                         sub_v2_v2v2(a, v4, v1);
577                         u2 = dot_v2v2(a, b) / dot_v2v2(c, c);
578
579                         if (u > u2) SWAP(float, u, u2);
580
581                         if (u > 1.0f + eps || u2 < -eps) return -1;  /* non-ovlerlapping segments */
582                         else if (max_ff(0.0f, u) == min_ff(1.0f, u2)) { /* one common point: can return result */
583                                 interp_v2_v2v2(vi, v1, v2, max_ff(0, u));
584                                 return 1;
585                         }
586                 }
587
588                 /* lines are collinear */
589                 return -1;
590         }
591
592         u = (c2 * b1 - b2 * c1) / d;
593         v = (c1 * a2 - a1 * c2) / d;
594
595         if (u >= -eps && u <= 1.0f + eps && v >= -eps && v <= 1.0f + eps) { /* intersection */
596                 interp_v2_v2v2(vi, v1, v2, u);
597                 return 1;
598         }
599
600         /* out of segment intersection */
601         return -1;
602 }
603
604 int isect_seg_seg_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
605 {
606 #define CCW(A, B, C) ((C[1] - A[1]) * (B[0] - A[0]) > (B[1]-A[1]) * (C[0]-A[0]))
607
608         return CCW(v1, v3, v4) != CCW(v2, v3, v4) && CCW(v1, v2, v3) != CCW(v1, v2, v4);
609
610 #undef CCW
611 }
612
613 int isect_line_sphere_v3(const float l1[3], const float l2[3],
614                          const float sp[3], const float r,
615                          float r_p1[3], float r_p2[3])
616 {
617         /* l1:         coordinates (point of line)
618          * l2:         coordinates (point of line)
619          * sp, r:      coordinates and radius (sphere)
620          * r_p1, r_p2: return intersection coordinates
621          */
622
623
624         /* adapted for use in blender by Campbell Barton - 2011
625          *
626          * atelier iebele abel - 2001
627          * atelier@iebele.nl
628          * http://www.iebele.nl
629          *
630          * sphere_line_intersection function adapted from:
631          * http://astronomy.swin.edu.au/pbourke/geometry/sphereline
632          * Paul Bourke pbourke@swin.edu.au
633          */
634
635         const float ldir[3] = {
636                 l2[0] - l1[0],
637                 l2[1] - l1[1],
638                 l2[2] - l1[2]
639         };
640
641         const float a = dot_v3v3(ldir, ldir);
642
643         const float b = 2.0f *
644                         (ldir[0] * (l1[0] - sp[0]) +
645                          ldir[1] * (l1[1] - sp[1]) +
646                          ldir[2] * (l1[2] - sp[2]));
647
648         const float c =
649             dot_v3v3(sp, sp) +
650             dot_v3v3(l1, l1) -
651             (2.0f * dot_v3v3(sp, l1)) -
652             (r * r);
653
654         const float i = b * b - 4.0f * a * c;
655
656         float mu;
657
658         if (i < 0.0f) {
659                 /* no intersections */
660                 return 0;
661         }
662         else if (i == 0.0f) {
663                 /* one intersection */
664                 mu = -b / (2.0f * a);
665                 madd_v3_v3v3fl(r_p1, l1, ldir, mu);
666                 return 1;
667         }
668         else if (i > 0.0f) {
669                 const float i_sqrt = sqrtf(i);  /* avoid calc twice */
670
671                 /* first intersection */
672                 mu = (-b + i_sqrt) / (2.0f * a);
673                 madd_v3_v3v3fl(r_p1, l1, ldir, mu);
674
675                 /* second intersection */
676                 mu = (-b - i_sqrt) / (2.0f * a);
677                 madd_v3_v3v3fl(r_p2, l1, ldir, mu);
678                 return 2;
679         }
680         else {
681                 /* math domain error - nan */
682                 return -1;
683         }
684 }
685
686 /* keep in sync with isect_line_sphere_v3 */
687 int isect_line_sphere_v2(const float l1[2], const float l2[2],
688                          const float sp[2], const float r,
689                          float r_p1[2], float r_p2[2])
690 {
691         const float ldir[2] = {l2[0] - l1[0],
692                                l2[1] - l1[1]};
693
694         const float a = dot_v2v2(ldir, ldir);
695
696         const float b = 2.0f *
697                         (ldir[0] * (l1[0] - sp[0]) +
698                          ldir[1] * (l1[1] - sp[1]));
699
700         const float c =
701             dot_v2v2(sp, sp) +
702             dot_v2v2(l1, l1) -
703             (2.0f * dot_v2v2(sp, l1)) -
704             (r * r);
705
706         const float i = b * b - 4.0f * a * c;
707
708         float mu;
709
710         if (i < 0.0f) {
711                 /* no intersections */
712                 return 0;
713         }
714         else if (i == 0.0f) {
715                 /* one intersection */
716                 mu = -b / (2.0f * a);
717                 madd_v2_v2v2fl(r_p1, l1, ldir, mu);
718                 return 1;
719         }
720         else if (i > 0.0f) {
721                 const float i_sqrt = sqrtf(i);  /* avoid calc twice */
722
723                 /* first intersection */
724                 mu = (-b + i_sqrt) / (2.0f * a);
725                 madd_v2_v2v2fl(r_p1, l1, ldir, mu);
726
727                 /* second intersection */
728                 mu = (-b - i_sqrt) / (2.0f * a);
729                 madd_v2_v2v2fl(r_p2, l1, ldir, mu);
730                 return 2;
731         }
732         else {
733                 /* math domain error - nan */
734                 return -1;
735         }
736 }
737
738 /* point in polygon (keep float and int versions in sync) */
739 #if 0
740 bool isect_point_poly_v2(const float pt[2], const float verts[][2], const unsigned int nr,
741                          const bool use_holes)
742 {
743         /* we do the angle rule, define that all added angles should be about zero or (2 * PI) */
744         float angletot = 0.0;
745         float fp1[2], fp2[2];
746         unsigned int i;
747         const float *p1, *p2;
748
749         p1 = verts[nr - 1];
750
751         /* first vector */
752         fp1[0] = (float)(p1[0] - pt[0]);
753         fp1[1] = (float)(p1[1] - pt[1]);
754
755         for (i = 0; i < nr; i++) {
756                 p2 = verts[i];
757
758                 /* second vector */
759                 fp2[0] = (float)(p2[0] - pt[0]);
760                 fp2[1] = (float)(p2[1] - pt[1]);
761
762                 /* dot and angle and cross */
763                 angletot += angle_signed_v2v2(fp1, fp2);
764
765                 /* circulate */
766                 copy_v2_v2(fp1, fp2);
767                 p1 = p2;
768         }
769
770         angletot = fabsf(angletot);
771         if (use_holes) {
772                 const float nested = floorf((angletot / (float)(M_PI * 2.0)) + 0.00001f);
773                 angletot -= nested * (float)(M_PI * 2.0);
774                 return (angletot > 4.0f) != ((int)nested % 2);
775         }
776         else {
777                 return (angletot > 4.0f);
778         }
779 }
780 bool isect_point_poly_v2_int(const int pt[2], const int verts[][2], const unsigned int nr,
781                              const bool use_holes)
782 {
783         /* we do the angle rule, define that all added angles should be about zero or (2 * PI) */
784         float angletot = 0.0;
785         float fp1[2], fp2[2];
786         unsigned int i;
787         const int *p1, *p2;
788
789         p1 = verts[nr - 1];
790
791         /* first vector */
792         fp1[0] = (float)(p1[0] - pt[0]);
793         fp1[1] = (float)(p1[1] - pt[1]);
794
795         for (i = 0; i < nr; i++) {
796                 p2 = verts[i];
797
798                 /* second vector */
799                 fp2[0] = (float)(p2[0] - pt[0]);
800                 fp2[1] = (float)(p2[1] - pt[1]);
801
802                 /* dot and angle and cross */
803                 angletot += angle_signed_v2v2(fp1, fp2);
804
805                 /* circulate */
806                 copy_v2_v2(fp1, fp2);
807                 p1 = p2;
808         }
809
810         angletot = fabsf(angletot);
811         if (use_holes) {
812                 const float nested = floorf((angletot / (float)(M_PI * 2.0)) + 0.00001f);
813                 angletot -= nested * (float)(M_PI * 2.0);
814                 return (angletot > 4.0f) != ((int)nested % 2);
815         }
816         else {
817                 return (angletot > 4.0f);
818         }
819 }
820
821 #else
822
823 bool isect_point_poly_v2(const float pt[2], const float verts[][2], const unsigned int nr,
824                          const bool UNUSED(use_holes))
825 {
826         unsigned int i, j;
827         bool isect = false;
828         for (i = 0, j = nr - 1; i < nr; j = i++) {
829                 if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
830                     (pt[0] < (verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) + verts[i][0]))
831                 {
832                         isect = !isect;
833                 }
834         }
835         return isect;
836 }
837 bool isect_point_poly_v2_int(const int pt[2], const int verts[][2], const unsigned int nr,
838                              const bool UNUSED(use_holes))
839 {
840         unsigned int i, j;
841         bool isect = false;
842         for (i = 0, j = nr - 1; i < nr; j = i++) {
843                 if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
844                     (pt[0] < (verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) + verts[i][0]))
845                 {
846                         isect = !isect;
847                 }
848         }
849         return isect;
850 }
851
852 #endif
853
854 /* point in tri */
855
856 /* only single direction */
857 int isect_point_tri_v2_cw(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
858 {
859         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
860                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
861                         if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
862                                 return 1;
863                         }
864                 }
865         }
866
867         return 0;
868 }
869
870 int isect_point_tri_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
871 {
872         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
873                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
874                         if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
875                                 return 1;
876                         }
877                 }
878         }
879         else {
880                 if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
881                         if (!(line_point_side_v2(v3, v1, pt) >= 0.0f)) {
882                                 return -1;
883                         }
884                 }
885         }
886
887         return 0;
888 }
889
890 /* point in quad - only convex quads */
891 int isect_point_quad_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2], const float v4[2])
892 {
893         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
894                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
895                         if (line_point_side_v2(v3, v4, pt) >= 0.0f) {
896                                 if (line_point_side_v2(v4, v1, pt) >= 0.0f) {
897                                         return 1;
898                                 }
899                         }
900                 }
901         }
902         else {
903                 if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
904                         if (!(line_point_side_v2(v3, v4, pt) >= 0.0f)) {
905                                 if (!(line_point_side_v2(v4, v1, pt) >= 0.0f)) {
906                                         return -1;
907                                 }
908                         }
909                 }
910         }
911
912         return 0;
913 }
914
915 /* moved from effect.c
916  * test if the line starting at p1 ending at p2 intersects the triangle v0..v2
917  * return non zero if it does
918  */
919 bool isect_line_tri_v3(const float p1[3], const float p2[3],
920                        const float v0[3], const float v1[3], const float v2[3],
921                        float *r_lambda, float r_uv[2])
922 {
923
924         float p[3], s[3], d[3], e1[3], e2[3], q[3];
925         float a, f, u, v;
926
927         sub_v3_v3v3(e1, v1, v0);
928         sub_v3_v3v3(e2, v2, v0);
929         sub_v3_v3v3(d, p2, p1);
930
931         cross_v3_v3v3(p, d, e2);
932         a = dot_v3v3(e1, p);
933         if ((a > -0.000001f) && (a < 0.000001f)) return 0;
934         f = 1.0f / a;
935
936         sub_v3_v3v3(s, p1, v0);
937
938         u = f * dot_v3v3(s, p);
939         if ((u < 0.0f) || (u > 1.0f)) return 0;
940
941         cross_v3_v3v3(q, s, e1);
942
943         v = f * dot_v3v3(d, q);
944         if ((v < 0.0f) || ((u + v) > 1.0f)) return 0;
945
946         *r_lambda = f * dot_v3v3(e2, q);
947         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return 0;
948
949         if (r_uv) {
950                 r_uv[0] = u;
951                 r_uv[1] = v;
952         }
953
954         return 1;
955 }
956
957 /* like isect_line_tri_v3, but allows epsilon tolerance around triangle */
958 bool isect_line_tri_epsilon_v3(const float p1[3], const float p2[3],
959                        const float v0[3], const float v1[3], const float v2[3],
960                        float *r_lambda, float r_uv[2], const float epsilon)
961 {
962
963         float p[3], s[3], d[3], e1[3], e2[3], q[3];
964         float a, f, u, v;
965
966         sub_v3_v3v3(e1, v1, v0);
967         sub_v3_v3v3(e2, v2, v0);
968         sub_v3_v3v3(d, p2, p1);
969
970         cross_v3_v3v3(p, d, e2);
971         a = dot_v3v3(e1, p);
972         if ((a > -0.000001f) && (a < 0.000001f)) return 0;
973         f = 1.0f / a;
974
975         sub_v3_v3v3(s, p1, v0);
976
977         u = f * dot_v3v3(s, p);
978         if ((u < -epsilon) || (u > 1.0f + epsilon)) return 0;
979
980         cross_v3_v3v3(q, s, e1);
981
982         v = f * dot_v3v3(d, q);
983         if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) return 0;
984
985         *r_lambda = f * dot_v3v3(e2, q);
986         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return 0;
987
988         if (r_uv) {
989                 r_uv[0] = u;
990                 r_uv[1] = v;
991         }
992
993         return 1;
994 }
995
996 /* moved from effect.c
997  * test if the ray starting at p1 going in d direction intersects the triangle v0..v2
998  * return non zero if it does
999  */
1000 bool isect_ray_tri_v3(const float p1[3], const float d[3],
1001                       const float v0[3], const float v1[3], const float v2[3],
1002                       float *r_lambda, float r_uv[2])
1003 {
1004         float p[3], s[3], e1[3], e2[3], q[3];
1005         float a, f, u, v;
1006
1007         sub_v3_v3v3(e1, v1, v0);
1008         sub_v3_v3v3(e2, v2, v0);
1009
1010         cross_v3_v3v3(p, d, e2);
1011         a = dot_v3v3(e1, p);
1012         /* note: these values were 0.000001 in 2.4x but for projection snapping on
1013          * a human head (1BU == 1m), subsurf level 2, this gave many errors - campbell */
1014         if ((a > -0.00000001f) && (a < 0.00000001f)) return 0;
1015         f = 1.0f / a;
1016
1017         sub_v3_v3v3(s, p1, v0);
1018
1019         u = f * dot_v3v3(s, p);
1020         if ((u < 0.0f) || (u > 1.0f)) return 0;
1021
1022         cross_v3_v3v3(q, s, e1);
1023
1024         v = f * dot_v3v3(d, q);
1025         if ((v < 0.0f) || ((u + v) > 1.0f)) return 0;
1026
1027         *r_lambda = f * dot_v3v3(e2, q);
1028         if ((*r_lambda < 0.0f)) return 0;
1029
1030         if (r_uv) {
1031                 r_uv[0] = u;
1032                 r_uv[1] = v;
1033         }
1034
1035         return 1;
1036 }
1037
1038 /**
1039  * if clip is nonzero, will only return true if lambda is >= 0.0
1040  * (i.e. intersection point is along positive d)
1041  */
1042 bool isect_ray_plane_v3(const float p1[3], const float d[3],
1043                         const float v0[3], const float v1[3], const float v2[3],
1044                         float *r_lambda, const int clip)
1045 {
1046         float p[3], s[3], e1[3], e2[3], q[3];
1047         float a, f;
1048         /* float  u, v; */ /*UNUSED*/
1049
1050         sub_v3_v3v3(e1, v1, v0);
1051         sub_v3_v3v3(e2, v2, v0);
1052
1053         cross_v3_v3v3(p, d, e2);
1054         a = dot_v3v3(e1, p);
1055         /* note: these values were 0.000001 in 2.4x but for projection snapping on
1056          * a human head (1BU == 1m), subsurf level 2, this gave many errors - campbell */
1057         if ((a > -0.00000001f) && (a < 0.00000001f)) return 0;
1058         f = 1.0f / a;
1059
1060         sub_v3_v3v3(s, p1, v0);
1061
1062         /* u = f * dot_v3v3(s, p); */ /*UNUSED*/
1063
1064         cross_v3_v3v3(q, s, e1);
1065
1066         /* v = f * dot_v3v3(d, q); */ /*UNUSED*/
1067
1068         *r_lambda = f * dot_v3v3(e2, q);
1069         if (clip && (*r_lambda < 0.0f)) return 0;
1070
1071         return 1;
1072 }
1073
1074 bool isect_ray_tri_epsilon_v3(const float p1[3], const float d[3],
1075                               const float v0[3], const float v1[3], const float v2[3],
1076                               float *r_lambda, float uv[2], const float epsilon)
1077 {
1078         float p[3], s[3], e1[3], e2[3], q[3];
1079         float a, f, u, v;
1080
1081         sub_v3_v3v3(e1, v1, v0);
1082         sub_v3_v3v3(e2, v2, v0);
1083
1084         cross_v3_v3v3(p, d, e2);
1085         a = dot_v3v3(e1, p);
1086         if (a == 0.0f) return 0;
1087         f = 1.0f / a;
1088
1089         sub_v3_v3v3(s, p1, v0);
1090
1091         u = f * dot_v3v3(s, p);
1092         if ((u < -epsilon) || (u > 1.0f + epsilon)) return 0;
1093
1094         cross_v3_v3v3(q, s, e1);
1095
1096         v = f * dot_v3v3(d, q);
1097         if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) return 0;
1098
1099         *r_lambda = f * dot_v3v3(e2, q);
1100         if ((*r_lambda < 0.0f)) return 0;
1101
1102         if (uv) {
1103                 uv[0] = u;
1104                 uv[1] = v;
1105         }
1106
1107         return 1;
1108 }
1109
1110 bool isect_ray_tri_threshold_v3(const float p1[3], const float d[3],
1111                                 const float v0[3], const float v1[3], const float v2[3],
1112                                 float *r_lambda, float r_uv[2], const float threshold)
1113 {
1114         float p[3], s[3], e1[3], e2[3], q[3];
1115         float a, f, u, v;
1116         float du = 0, dv = 0;
1117
1118         sub_v3_v3v3(e1, v1, v0);
1119         sub_v3_v3v3(e2, v2, v0);
1120
1121         cross_v3_v3v3(p, d, e2);
1122         a = dot_v3v3(e1, p);
1123         if ((a > -0.000001f) && (a < 0.000001f)) return 0;
1124         f = 1.0f / a;
1125
1126         sub_v3_v3v3(s, p1, v0);
1127
1128         cross_v3_v3v3(q, s, e1);
1129         *r_lambda = f * dot_v3v3(e2, q);
1130         if ((*r_lambda < 0.0f)) return 0;
1131
1132         u = f * dot_v3v3(s, p);
1133         v = f * dot_v3v3(d, q);
1134
1135         if (u < 0) du = u;
1136         if (u > 1) du = u - 1;
1137         if (v < 0) dv = v;
1138         if (v > 1) dv = v - 1;
1139         if (u > 0 && v > 0 && u + v > 1) {
1140                 float t = u + v - 1;
1141                 du = u - t / 2;
1142                 dv = v - t / 2;
1143         }
1144
1145         mul_v3_fl(e1, du);
1146         mul_v3_fl(e2, dv);
1147
1148         if (dot_v3v3(e1, e1) + dot_v3v3(e2, e2) > threshold * threshold) {
1149                 return 0;
1150         }
1151
1152         if (r_uv) {
1153                 r_uv[0] = u;
1154                 r_uv[1] = v;
1155         }
1156
1157         return 1;
1158 }
1159
1160 /**
1161  * Check if a point is behind all planes.
1162  */
1163 bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3])
1164 {
1165         int i;
1166
1167         for (i = 0; i < totplane; i++) {
1168                 if (plane_point_side_v3(planes[i], p) > 0.0f) {
1169                         return false;
1170                 }
1171         }
1172
1173         return true;
1174 }
1175
1176 /**
1177  * Intersect line/plane.
1178  *
1179  * \param out The intersection point.
1180  * \param l1 The first point of the line.
1181  * \param l2 The second point of the line.
1182  * \param plane_co A point on the plane to intersect with.
1183  * \param plane_no The direction of the plane (does not need to be normalized).
1184  *
1185  * \note #line_plane_factor_v3() shares logic.
1186  */
1187 bool isect_line_plane_v3(float out[3],
1188                          const float l1[3], const float l2[3],
1189                          const float plane_co[3], const float plane_no[3])
1190 {
1191         float u[3], h[3];
1192         float dot;
1193
1194         sub_v3_v3v3(u, l2, l1);
1195         sub_v3_v3v3(h, l1, plane_co);
1196         dot = dot_v3v3(plane_no, u);
1197
1198         if (fabsf(dot) > FLT_EPSILON) {
1199                 float lambda = -dot_v3v3(plane_no, h) / dot;
1200                 madd_v3_v3v3fl(out, l1, u, lambda);
1201                 return true;
1202         }
1203         else {
1204                 /* The segment is parallel to plane */
1205                 return false;
1206         }
1207 }
1208
1209 /**
1210  * Intersect two planes, return a point on the intersection and a vector
1211  * that runs on the direction of the intersection.
1212  * Return error code is the same as 'isect_line_line_v3'.
1213  *
1214  * \param r_isect_co The resulting intersection point.
1215  * \param r_isect_no The resulting vector of the intersection.
1216  * \param plane_a_co The point on the first plane.
1217  * \param plane_a_no The normal of the first plane.
1218  * \param plane_b_co The point on the second plane.
1219  * \param plane_b_no The normal of the second plane.
1220  *
1221  * \note return normal isn't unit length
1222  */
1223 bool isect_plane_plane_v3(float r_isect_co[3], float r_isect_no[3],
1224                           const float plane_a_co[3], const float plane_a_no[3],
1225                           const float plane_b_co[3], const float plane_b_no[3])
1226 {
1227         float plane_a_co_other[3];
1228         cross_v3_v3v3(r_isect_no, plane_a_no, plane_b_no); /* direction is simply the cross product */
1229         cross_v3_v3v3(plane_a_co_other, plane_a_no, r_isect_no);
1230         add_v3_v3(plane_a_co_other, plane_a_co);
1231         return isect_line_plane_v3(r_isect_co, plane_a_co, plane_a_co_other, plane_b_co, plane_b_no);
1232 }
1233
1234
1235 /* Adapted from the paper by Kasper Fauerby */
1236
1237 /* "Improved Collision detection and Response" */
1238 static bool getLowestRoot(const float a, const float b, const float c, const float maxR, float *root)
1239 {
1240         /* Check if a solution exists */
1241         float determinant = b * b - 4.0f * a * c;
1242
1243         /* If determinant is negative it means no solutions. */
1244         if (determinant >= 0.0f) {
1245                 /* calculate the two roots: (if determinant == 0 then
1246                  * x1==x2 but lets disregard that slight optimization) */
1247                 float sqrtD = (float)sqrt(determinant);
1248                 float r1 = (-b - sqrtD) / (2.0f * a);
1249                 float r2 = (-b + sqrtD) / (2.0f * a);
1250
1251                 /* Sort so x1 <= x2 */
1252                 if (r1 > r2)
1253                         SWAP(float, r1, r2);
1254
1255                 /* Get lowest root: */
1256                 if (r1 > 0.0f && r1 < maxR) {
1257                         *root = r1;
1258                         return 1;
1259                 }
1260
1261                 /* It is possible that we want x2 - this can happen */
1262                 /* if x1 < 0 */
1263                 if (r2 > 0.0f && r2 < maxR) {
1264                         *root = r2;
1265                         return 1;
1266                 }
1267         }
1268         /* No (valid) solutions */
1269         return 0;
1270 }
1271
1272 bool isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const float radius,
1273                                   const float v0[3], const float v1[3], const float v2[3],
1274                                   float *r_lambda, float ipoint[3])
1275 {
1276         float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
1277         float a, b, c, d, e, x, y, z, radius2 = radius * radius;
1278         float elen2, edotv, edotbv, nordotv;
1279         float newLambda;
1280         bool found_by_sweep = false;
1281
1282         sub_v3_v3v3(e1, v1, v0);
1283         sub_v3_v3v3(e2, v2, v0);
1284         sub_v3_v3v3(vel, p2, p1);
1285
1286         /*---test plane of tri---*/
1287         cross_v3_v3v3(nor, e1, e2);
1288         normalize_v3(nor);
1289
1290         /* flip normal */
1291         if (dot_v3v3(nor, vel) > 0.0f) negate_v3(nor);
1292
1293         a = dot_v3v3(p1, nor) - dot_v3v3(v0, nor);
1294         nordotv = dot_v3v3(nor, vel);
1295
1296         if (fabsf(nordotv) < 0.000001f) {
1297                 if (fabsf(a) >= radius) {
1298                         return 0;
1299                 }
1300         }
1301         else {
1302                 float t0 = (-a + radius) / nordotv;
1303                 float t1 = (-a - radius) / nordotv;
1304
1305                 if (t0 > t1)
1306                         SWAP(float, t0, t1);
1307
1308                 if (t0 > 1.0f || t1 < 0.0f) return 0;
1309
1310                 /* clamp to [0, 1] */
1311                 CLAMP(t0, 0.0f, 1.0f);
1312                 CLAMP(t1, 0.0f, 1.0f);
1313
1314                 /*---test inside of tri---*/
1315                 /* plane intersection point */
1316
1317                 point[0] = p1[0] + vel[0] * t0 - nor[0] * radius;
1318                 point[1] = p1[1] + vel[1] * t0 - nor[1] * radius;
1319                 point[2] = p1[2] + vel[2] * t0 - nor[2] * radius;
1320
1321
1322                 /* is the point in the tri? */
1323                 a = dot_v3v3(e1, e1);
1324                 b = dot_v3v3(e1, e2);
1325                 c = dot_v3v3(e2, e2);
1326
1327                 sub_v3_v3v3(temp, point, v0);
1328                 d = dot_v3v3(temp, e1);
1329                 e = dot_v3v3(temp, e2);
1330
1331                 x = d * c - e * b;
1332                 y = e * a - d * b;
1333                 z = x + y - (a * c - b * b);
1334
1335
1336                 if (z <= 0.0f && (x >= 0.0f && y >= 0.0f)) {
1337                         //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000) {
1338                         *r_lambda = t0;
1339                         copy_v3_v3(ipoint, point);
1340                         return 1;
1341                 }
1342         }
1343
1344
1345         *r_lambda = 1.0f;
1346
1347         /*---test points---*/
1348         a = dot_v3v3(vel, vel);
1349
1350         /*v0*/
1351         sub_v3_v3v3(temp, p1, v0);
1352         b = 2.0f * dot_v3v3(vel, temp);
1353         c = dot_v3v3(temp, temp) - radius2;
1354
1355         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1356                 copy_v3_v3(ipoint, v0);
1357                 found_by_sweep = 1;
1358         }
1359
1360         /*v1*/
1361         sub_v3_v3v3(temp, p1, v1);
1362         b = 2.0f * dot_v3v3(vel, temp);
1363         c = dot_v3v3(temp, temp) - radius2;
1364
1365         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1366                 copy_v3_v3(ipoint, v1);
1367                 found_by_sweep = 1;
1368         }
1369
1370         /*v2*/
1371         sub_v3_v3v3(temp, p1, v2);
1372         b = 2.0f * dot_v3v3(vel, temp);
1373         c = dot_v3v3(temp, temp) - radius2;
1374
1375         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1376                 copy_v3_v3(ipoint, v2);
1377                 found_by_sweep = 1;
1378         }
1379
1380         /*---test edges---*/
1381         sub_v3_v3v3(e3, v2, v1);  /* wasnt yet calculated */
1382
1383
1384         /*e1*/
1385         sub_v3_v3v3(bv, v0, p1);
1386
1387         elen2 = dot_v3v3(e1, e1);
1388         edotv = dot_v3v3(e1, vel);
1389         edotbv = dot_v3v3(e1, bv);
1390
1391         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
1392         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
1393         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
1394
1395         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
1396                 e = (edotv * newLambda - edotbv) / elen2;
1397
1398                 if (e >= 0.0f && e <= 1.0f) {
1399                         *r_lambda = newLambda;
1400                         copy_v3_v3(ipoint, e1);
1401                         mul_v3_fl(ipoint, e);
1402                         add_v3_v3(ipoint, v0);
1403                         found_by_sweep = 1;
1404                 }
1405         }
1406
1407         /*e2*/
1408         /*bv is same*/
1409         elen2 = dot_v3v3(e2, e2);
1410         edotv = dot_v3v3(e2, vel);
1411         edotbv = dot_v3v3(e2, bv);
1412
1413         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
1414         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
1415         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
1416
1417         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
1418                 e = (edotv * newLambda - edotbv) / elen2;
1419
1420                 if (e >= 0.0f && e <= 1.0f) {
1421                         *r_lambda = newLambda;
1422                         copy_v3_v3(ipoint, e2);
1423                         mul_v3_fl(ipoint, e);
1424                         add_v3_v3(ipoint, v0);
1425                         found_by_sweep = 1;
1426                 }
1427         }
1428
1429         /*e3*/
1430         /* sub_v3_v3v3(bv, v0, p1); */ /* UNUSED */
1431         /* elen2 = dot_v3v3(e1, e1); */ /* UNUSED */
1432         /* edotv = dot_v3v3(e1, vel); */ /* UNUSED */
1433         /* edotbv = dot_v3v3(e1, bv); */ /* UNUSED */
1434
1435         sub_v3_v3v3(bv, v1, p1);
1436         elen2 = dot_v3v3(e3, e3);
1437         edotv = dot_v3v3(e3, vel);
1438         edotbv = dot_v3v3(e3, bv);
1439
1440         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
1441         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
1442         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
1443
1444         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
1445                 e = (edotv * newLambda - edotbv) / elen2;
1446
1447                 if (e >= 0.0f && e <= 1.0f) {
1448                         *r_lambda = newLambda;
1449                         copy_v3_v3(ipoint, e3);
1450                         mul_v3_fl(ipoint, e);
1451                         add_v3_v3(ipoint, v1);
1452                         found_by_sweep = 1;
1453                 }
1454         }
1455
1456
1457         return found_by_sweep;
1458 }
1459
1460 bool isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3],
1461                              const float v0[3], const float v1[3], const float v2[3], float *r_lambda)
1462 {
1463         float p[3], e1[3], e2[3];
1464         float u, v, f;
1465         int a0 = axis, a1 = (axis + 1) % 3, a2 = (axis + 2) % 3;
1466
1467 #if 0
1468         return isect_line_tri_v3(p1, p2, v0, v1, v2, lambda);
1469
1470         /* first a simple bounding box test */
1471         if (min_fff(v0[a1], v1[a1], v2[a1]) > p1[a1]) return 0;
1472         if (min_fff(v0[a2], v1[a2], v2[a2]) > p1[a2]) return 0;
1473         if (max_fff(v0[a1], v1[a1], v2[a1]) < p1[a1]) return 0;
1474         if (max_fff(v0[a2], v1[a2], v2[a2]) < p1[a2]) return 0;
1475
1476         /* then a full intersection test */
1477 #endif
1478
1479         sub_v3_v3v3(e1, v1, v0);
1480         sub_v3_v3v3(e2, v2, v0);
1481         sub_v3_v3v3(p, v0, p1);
1482
1483         f = (e2[a1] * e1[a2] - e2[a2] * e1[a1]);
1484         if ((f > -0.000001f) && (f < 0.000001f)) return 0;
1485
1486         v = (p[a2] * e1[a1] - p[a1] * e1[a2]) / f;
1487         if ((v < 0.0f) || (v > 1.0f)) return 0;
1488
1489         f = e1[a1];
1490         if ((f > -0.000001f) && (f < 0.000001f)) {
1491                 f = e1[a2];
1492                 if ((f > -0.000001f) && (f < 0.000001f)) return 0;
1493                 u = (-p[a2] - v * e2[a2]) / f;
1494         }
1495         else
1496                 u = (-p[a1] - v * e2[a1]) / f;
1497
1498         if ((u < 0.0f) || ((u + v) > 1.0f)) return 0;
1499
1500         *r_lambda = (p[a0] + u * e1[a0] + v * e2[a0]) / (p2[a0] - p1[a0]);
1501
1502         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return 0;
1503
1504         return 1;
1505 }
1506
1507 /**
1508  * \return The number of point of interests
1509  * 0 - lines are colinear
1510  * 1 - lines are coplanar, i1 is set to intersection
1511  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively
1512  */
1513 int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3], float i1[3], float i2[3])
1514 {
1515         float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3];
1516         float d, div;
1517
1518         sub_v3_v3v3(c, v3, v1);
1519         sub_v3_v3v3(a, v2, v1);
1520         sub_v3_v3v3(b, v4, v3);
1521
1522         normalize_v3_v3(dir1, a);
1523         normalize_v3_v3(dir2, b);
1524         d = dot_v3v3(dir1, dir2);
1525         if (d == 1.0f || d == -1.0f) {
1526                 /* colinear */
1527                 return 0;
1528         }
1529
1530         cross_v3_v3v3(ab, a, b);
1531         d = dot_v3v3(c, ab);
1532         div = dot_v3v3(ab, ab);
1533
1534         /* test zero length line */
1535         if (UNLIKELY(div == 0.0f)) {
1536                 return 0;
1537         }
1538         /* test if the two lines are coplanar */
1539         else if (d > -0.000001f && d < 0.000001f) {
1540                 cross_v3_v3v3(cb, c, b);
1541
1542                 mul_v3_fl(a, dot_v3v3(cb, ab) / div);
1543                 add_v3_v3v3(i1, v1, a);
1544                 copy_v3_v3(i2, i1);
1545
1546                 return 1; /* one intersection only */
1547         }
1548         /* if not */
1549         else {
1550                 float n[3], t[3];
1551                 float v3t[3], v4t[3];
1552                 sub_v3_v3v3(t, v1, v3);
1553
1554                 /* offset between both plane where the lines lies */
1555                 cross_v3_v3v3(n, a, b);
1556                 project_v3_v3v3(t, t, n);
1557
1558                 /* for the first line, offset the second line until it is coplanar */
1559                 add_v3_v3v3(v3t, v3, t);
1560                 add_v3_v3v3(v4t, v4, t);
1561
1562                 sub_v3_v3v3(c, v3t, v1);
1563                 sub_v3_v3v3(a, v2, v1);
1564                 sub_v3_v3v3(b, v4t, v3t);
1565
1566                 cross_v3_v3v3(ab, a, b);
1567                 cross_v3_v3v3(cb, c, b);
1568
1569                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
1570                 add_v3_v3v3(i1, v1, a);
1571
1572                 /* for the second line, just substract the offset from the first intersection point */
1573                 sub_v3_v3v3(i2, i1, t);
1574
1575                 return 2; /* two nearest points */
1576         }
1577 }
1578
1579 /* Intersection point strictly between the two lines
1580  * 0 when no intersection is found
1581  * */
1582 bool isect_line_line_strict_v3(const float v1[3], const float v2[3],
1583                                const float v3[3], const float v4[3],
1584                                float vi[3], float *r_lambda)
1585 {
1586         float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3];
1587         float d, div;
1588
1589         sub_v3_v3v3(c, v3, v1);
1590         sub_v3_v3v3(a, v2, v1);
1591         sub_v3_v3v3(b, v4, v3);
1592
1593         normalize_v3_v3(dir1, a);
1594         normalize_v3_v3(dir2, b);
1595         d = dot_v3v3(dir1, dir2);
1596         if (d == 1.0f || d == -1.0f || d == 0) {
1597                 /* colinear or one vector is zero-length*/
1598                 return 0;
1599         }
1600
1601         cross_v3_v3v3(ab, a, b);
1602         d = dot_v3v3(c, ab);
1603         div = dot_v3v3(ab, ab);
1604
1605         /* test zero length line */
1606         if (UNLIKELY(div == 0.0f)) {
1607                 return 0;
1608         }
1609         /* test if the two lines are coplanar */
1610         else if (d > -0.000001f && d < 0.000001f) {
1611                 float f1, f2;
1612                 cross_v3_v3v3(cb, c, b);
1613                 cross_v3_v3v3(ca, c, a);
1614
1615                 f1 = dot_v3v3(cb, ab) / div;
1616                 f2 = dot_v3v3(ca, ab) / div;
1617
1618                 if (f1 >= 0 && f1 <= 1 &&
1619                     f2 >= 0 && f2 <= 1)
1620                 {
1621                         mul_v3_fl(a, f1);
1622                         add_v3_v3v3(vi, v1, a);
1623
1624                         if (r_lambda) *r_lambda = f1;
1625
1626                         return 1; /* intersection found */
1627                 }
1628                 else {
1629                         return 0;
1630                 }
1631         }
1632         else {
1633                 return 0;
1634         }
1635 }
1636
1637 bool isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min2[3], const float max2[3])
1638 {
1639         return (min1[0] < max2[0] && min1[1] < max2[1] && min1[2] < max2[2] &&
1640                 min2[0] < max1[0] && min2[1] < max1[1] && min2[2] < max1[2]);
1641 }
1642
1643 void isect_ray_aabb_initialize(IsectRayAABBData *data, const float ray_start[3], const float ray_direction[3])
1644 {
1645         copy_v3_v3(data->ray_start, ray_start);
1646
1647         data->ray_inv_dir[0] = 1.0f / ray_direction[0];
1648         data->ray_inv_dir[1] = 1.0f / ray_direction[1];
1649         data->ray_inv_dir[2] = 1.0f / ray_direction[2];
1650
1651         data->sign[0] = data->ray_inv_dir[0] < 0;
1652         data->sign[1] = data->ray_inv_dir[1] < 0;
1653         data->sign[2] = data->ray_inv_dir[2] < 0;
1654 }
1655
1656 /* Adapted from http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
1657 bool isect_ray_aabb(const IsectRayAABBData *data, const float bb_min[3],
1658                     const float bb_max[3], float *tmin_out)
1659 {
1660         float bbox[2][3];
1661         float tmin, tmax, tymin, tymax, tzmin, tzmax;
1662
1663         copy_v3_v3(bbox[0], bb_min);
1664         copy_v3_v3(bbox[1], bb_max);
1665
1666         tmin = (bbox[data->sign[0]][0] - data->ray_start[0]) * data->ray_inv_dir[0];
1667         tmax = (bbox[1 - data->sign[0]][0] - data->ray_start[0]) * data->ray_inv_dir[0];
1668
1669         tymin = (bbox[data->sign[1]][1] - data->ray_start[1]) * data->ray_inv_dir[1];
1670         tymax = (bbox[1 - data->sign[1]][1] - data->ray_start[1]) * data->ray_inv_dir[1];
1671
1672         if ((tmin > tymax) || (tymin > tmax))
1673                 return false;
1674
1675         if (tymin > tmin)
1676                 tmin = tymin;
1677
1678         if (tymax < tmax)
1679                 tmax = tymax;
1680
1681         tzmin = (bbox[data->sign[2]][2] - data->ray_start[2]) * data->ray_inv_dir[2];
1682         tzmax = (bbox[1 - data->sign[2]][2] - data->ray_start[2]) * data->ray_inv_dir[2];
1683
1684         if ((tmin > tzmax) || (tzmin > tmax))
1685                 return false;
1686
1687         if (tzmin > tmin)
1688                 tmin = tzmin;
1689
1690         /* XXX jwilkins: tmax does not need to be updated since we don't use it
1691          * keeping this here for future reference */
1692         //if (tzmax < tmax) tmax = tzmax;
1693
1694         if (tmin_out)
1695                 (*tmin_out) = tmin;
1696
1697         return true;
1698 }
1699
1700 /* find closest point to p on line through (l1, l2) and return lambda,
1701  * where (0 <= lambda <= 1) when cp is in the line segment (l1, l2)
1702  */
1703 float closest_to_line_v3(float cp[3], const float p[3], const float l1[3], const float l2[3])
1704 {
1705         float h[3], u[3], lambda;
1706         sub_v3_v3v3(u, l2, l1);
1707         sub_v3_v3v3(h, p, l1);
1708         lambda = dot_v3v3(u, h) / dot_v3v3(u, u);
1709         cp[0] = l1[0] + u[0] * lambda;
1710         cp[1] = l1[1] + u[1] * lambda;
1711         cp[2] = l1[2] + u[2] * lambda;
1712         return lambda;
1713 }
1714
1715 float closest_to_line_v2(float cp[2], const float p[2], const float l1[2], const float l2[2])
1716 {
1717         float h[2], u[2], lambda;
1718         sub_v2_v2v2(u, l2, l1);
1719         sub_v2_v2v2(h, p, l1);
1720         lambda = dot_v2v2(u, h) / dot_v2v2(u, u);
1721         cp[0] = l1[0] + u[0] * lambda;
1722         cp[1] = l1[1] + u[1] * lambda;
1723         return lambda;
1724 }
1725
1726 /* little sister we only need to know lambda */
1727 float line_point_factor_v3(const float p[3], const float l1[3], const float l2[3])
1728 {
1729         float h[3], u[3];
1730         float dot;
1731         sub_v3_v3v3(u, l2, l1);
1732         sub_v3_v3v3(h, p, l1);
1733 #if 0
1734         return (dot_v3v3(u, h) / dot_v3v3(u, u));
1735 #else
1736         /* better check for zero */
1737         dot = dot_v3v3(u, u);
1738         return (dot != 0.0f) ? (dot_v3v3(u, h) / dot) : 0.0f;
1739 #endif
1740 }
1741
1742 float line_point_factor_v2(const float p[2], const float l1[2], const float l2[2])
1743 {
1744         float h[2], u[2];
1745         float dot;
1746         sub_v2_v2v2(u, l2, l1);
1747         sub_v2_v2v2(h, p, l1);
1748 #if 0
1749         return (dot_v2v2(u, h) / dot_v2v2(u, u));
1750 #else
1751         /* better check for zero */
1752         dot = dot_v2v2(u, u);
1753         return (dot != 0.0f) ? (dot_v2v2(u, h) / dot) : 0.0f;
1754 #endif
1755 }
1756
1757 /**
1758  * \note #isect_line_plane_v3() shares logic
1759  */
1760 float line_plane_factor_v3(const float plane_co[3], const float plane_no[3],
1761                            const float l1[3], const float l2[3])
1762 {
1763         float u[3], h[3];
1764         float dot;
1765         sub_v3_v3v3(u, l2, l1);
1766         sub_v3_v3v3(h, l1, plane_co);
1767         dot = dot_v3v3(plane_no, u);
1768         return (dot != 0.0f) ? -dot_v3v3(plane_no, h) / dot : 0.0f;
1769 }
1770
1771 /* ensure the distance between these points is no greater then 'dist'
1772  * if it is, scale then both into the center */
1773 void limit_dist_v3(float v1[3], float v2[3], const float dist)
1774 {
1775         const float dist_old = len_v3v3(v1, v2);
1776
1777         if (dist_old > dist) {
1778                 float v1_old[3];
1779                 float v2_old[3];
1780                 float fac = (dist / dist_old) * 0.5f;
1781
1782                 copy_v3_v3(v1_old, v1);
1783                 copy_v3_v3(v2_old, v2);
1784
1785                 interp_v3_v3v3(v1, v1_old, v2_old, 0.5f - fac);
1786                 interp_v3_v3v3(v2, v1_old, v2_old, 0.5f + fac);
1787         }
1788 }
1789
1790 /*
1791  *     x1,y2
1792  *     |  \
1793  *     |   \     .(a,b)
1794  *     |    \
1795  *     x1,y1-- x2,y1
1796  */
1797 int isect_point_tri_v2_int(const int x1, const int y1, const int x2, const int y2, const int a, const int b)
1798 {
1799         float v1[2], v2[2], v3[2], p[2];
1800
1801         v1[0] = (float)x1;
1802         v1[1] = (float)y1;
1803
1804         v2[0] = (float)x1;
1805         v2[1] = (float)y2;
1806
1807         v3[0] = (float)x2;
1808         v3[1] = (float)y1;
1809
1810         p[0] = (float)a;
1811         p[1] = (float)b;
1812
1813         return isect_point_tri_v2(p, v1, v2, v3);
1814 }
1815
1816 static bool point_in_slice(const float p[3], const float v1[3], const float l1[3], const float l2[3])
1817 {
1818         /*
1819          * what is a slice ?
1820          * some maths:
1821          * a line including (l1, l2) and a point not on the line
1822          * define a subset of R3 delimited by planes parallel to the line and orthogonal
1823          * to the (point --> line) distance vector, one plane on the line one on the point,
1824          * the room inside usually is rather small compared to R3 though still infinite
1825          * useful for restricting (speeding up) searches
1826          * e.g. all points of triangular prism are within the intersection of 3 'slices'
1827          * another trivial case : cube
1828          * but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
1829          */
1830         float h, rp[3], cp[3], q[3];
1831
1832         closest_to_line_v3(cp, v1, l1, l2);
1833         sub_v3_v3v3(q, cp, v1);
1834
1835         sub_v3_v3v3(rp, p, v1);
1836         h = dot_v3v3(q, rp) / dot_v3v3(q, q);
1837         if (h < 0.0f || h > 1.0f) return 0;
1838         return 1;
1839 }
1840
1841 #if 0
1842
1843 /* adult sister defining the slice planes by the origin and the normal
1844  * NOTE |normal| may not be 1 but defining the thickness of the slice */
1845 static int point_in_slice_as(float p[3], float origin[3], float normal[3])
1846 {
1847         float h, rp[3];
1848         sub_v3_v3v3(rp, p, origin);
1849         h = dot_v3v3(normal, rp) / dot_v3v3(normal, normal);
1850         if (h < 0.0f || h > 1.0f) return 0;
1851         return 1;
1852 }
1853
1854 /*mama (knowing the squared length of the normal) */
1855 static int point_in_slice_m(float p[3], float origin[3], float normal[3], float lns)
1856 {
1857         float h, rp[3];
1858         sub_v3_v3v3(rp, p, origin);
1859         h = dot_v3v3(normal, rp) / lns;
1860         if (h < 0.0f || h > 1.0f) return 0;
1861         return 1;
1862 }
1863 #endif
1864
1865 bool isect_point_tri_prism_v3(const float p[3], const float v1[3], const float v2[3], const float v3[3])
1866 {
1867         if (!point_in_slice(p, v1, v2, v3)) return 0;
1868         if (!point_in_slice(p, v2, v3, v1)) return 0;
1869         if (!point_in_slice(p, v3, v1, v2)) return 0;
1870         return 1;
1871 }
1872
1873 bool clip_segment_v3_plane(float p1[3], float p2[3], const float plane[4])
1874 {
1875         float dp[3], div, t, pc[3];
1876
1877         sub_v3_v3v3(dp, p2, p1);
1878         div = dot_v3v3(dp, plane);
1879
1880         if (div == 0.0f) /* parallel */
1881                 return 1;
1882
1883         t = -plane_point_side_v3(plane, p1) / div;
1884
1885         if (div > 0.0f) {
1886                 /* behind plane, completely clipped */
1887                 if (t >= 1.0f) {
1888                         zero_v3(p1);
1889                         zero_v3(p2);
1890                         return 0;
1891                 }
1892
1893                 /* intersect plane */
1894                 if (t > 0.0f) {
1895                         madd_v3_v3v3fl(pc, p1, dp, t);
1896                         copy_v3_v3(p1, pc);
1897                         return 1;
1898                 }
1899
1900                 return 1;
1901         }
1902         else {
1903                 /* behind plane, completely clipped */
1904                 if (t <= 0.0f) {
1905                         zero_v3(p1);
1906                         zero_v3(p2);
1907                         return 0;
1908                 }
1909
1910                 /* intersect plane */
1911                 if (t < 1.0f) {
1912                         madd_v3_v3v3fl(pc, p1, dp, t);
1913                         copy_v3_v3(p2, pc);
1914                         return 1;
1915                 }
1916
1917                 return 1;
1918         }
1919 }
1920
1921 bool clip_segment_v3_plane_n(float r_p1[3], float r_p2[3], float plane_array[][4], const int plane_tot)
1922 {
1923         /* intersect from both directions */
1924         float p1[3], p2[3], dp[3], dp_orig[3];
1925         int i;
1926         copy_v3_v3(p1, r_p1);
1927         copy_v3_v3(p2, r_p2);
1928
1929         sub_v3_v3v3(dp, p2, p1);
1930         copy_v3_v3(dp_orig, dp);
1931
1932         for (i = 0; i < plane_tot; i++) {
1933                 const float *plane = plane_array[i];
1934                 const float div = dot_v3v3(dp, plane);
1935
1936                 if (div != 0.0f) {
1937                         const float t = -plane_point_side_v3(plane, p1) / div;
1938                         if (div > 0.0f) {
1939                                 /* clip a */
1940                                 if (t >= 1.0f) {
1941                                         return false;
1942                                 }
1943
1944                                 /* intersect plane */
1945                                 if (t > 0.0f) {
1946                                         madd_v3_v3v3fl(p1, p1, dp, t);
1947                                         /* recalc direction and test for flipping */
1948                                         sub_v3_v3v3(dp, p2, p1);
1949                                         if (dot_v3v3(dp, dp_orig) < 0.0f) {
1950                                                 return false;
1951                                         }
1952                                 }
1953                         }
1954                         else if (div < 0.0f) {
1955                                 /* clip b */
1956                                 if (t <= 0.0f) {
1957                                         return false;
1958                                 }
1959
1960                                 /* intersect plane */
1961                                 if (t < 1.0f) {
1962                                         madd_v3_v3v3fl(p2, p1, dp, t);
1963                                         /* recalc direction and test for flipping */
1964                                         sub_v3_v3v3(dp, p2, p1);
1965                                         if (dot_v3v3(dp, dp_orig) < 0.0f) {
1966                                                 return false;
1967                                         }
1968                                 }
1969                         }
1970                 }
1971         }
1972
1973         copy_v3_v3(r_p1, p1);
1974         copy_v3_v3(r_p2, p2);
1975         return true;
1976 }
1977
1978 void plot_line_v2v2i(const int p1[2], const int p2[2], bool (*callback)(int, int, void *), void *userData)
1979 {
1980         int x1 = p1[0];
1981         int y1 = p1[1];
1982         int x2 = p2[0];
1983         int y2 = p2[1];
1984
1985         signed char ix;
1986         signed char iy;
1987
1988         /* if x1 == x2 or y1 == y2, then it does not matter what we set here */
1989         int delta_x = (x2 > x1 ? (ix = 1, x2 - x1) : (ix = -1, x1 - x2)) << 1;
1990         int delta_y = (y2 > y1 ? (iy = 1, y2 - y1) : (iy = -1, y1 - y2)) << 1;
1991
1992         if (callback(x1, y1, userData) == 0) {
1993                 return;
1994         }
1995
1996         if (delta_x >= delta_y) {
1997                 /* error may go below zero */
1998                 int error = delta_y - (delta_x >> 1);
1999
2000                 while (x1 != x2) {
2001                         if (error >= 0) {
2002                                 if (error || (ix > 0)) {
2003                                         y1 += iy;
2004                                         error -= delta_x;
2005                                 }
2006                                 /* else do nothing */
2007                         }
2008                         /* else do nothing */
2009
2010                         x1 += ix;
2011                         error += delta_y;
2012
2013                         if (callback(x1, y1, userData) == 0) {
2014                                 return;
2015                         }
2016                 }
2017         }
2018         else {
2019                 /* error may go below zero */
2020                 int error = delta_x - (delta_y >> 1);
2021
2022                 while (y1 != y2) {
2023                         if (error >= 0) {
2024                                 if (error || (iy > 0)) {
2025                                         x1 += ix;
2026                                         error -= delta_y;
2027                                 }
2028                                 /* else do nothing */
2029                         }
2030                         /* else do nothing */
2031
2032                         y1 += iy;
2033                         error += delta_x;
2034
2035                         if (callback(x1, y1, userData) == 0) {
2036                                 return;
2037                         }
2038                 }
2039         }
2040 }
2041
2042 void fill_poly_v2i_n(
2043         const int xmin, const int ymin, const int xmax, const int ymax,
2044         const int verts[][2], const int nr,
2045         void (*callback)(int, int, void *), void *userData)
2046 {
2047         /* originally by Darel Rex Finley, 2007 */
2048
2049         int  nodes, pixel_y, i, j, swap;
2050         int *node_x = MEM_mallocN(sizeof(*node_x) * (size_t)(nr + 1), __func__);
2051
2052         /* Loop through the rows of the image. */
2053         for (pixel_y = ymin; pixel_y < ymax; pixel_y++) {
2054
2055                 /* Build a list of nodes. */
2056                 nodes = 0; j = nr - 1;
2057                 for (i = 0; i < nr; i++) {
2058                         if ((verts[i][1] < pixel_y && verts[j][1] >= pixel_y) ||
2059                             (verts[j][1] < pixel_y && verts[i][1] >= pixel_y))
2060                         {
2061                                 node_x[nodes++] = (int)(verts[i][0] +
2062                                                         ((double)(pixel_y - verts[i][1]) / (verts[j][1] - verts[i][1])) *
2063                                                         (verts[j][0] - verts[i][0]));
2064                         }
2065                         j = i;
2066                 }
2067
2068                 /* Sort the nodes, via a simple "Bubble" sort. */
2069                 i = 0;
2070                 while (i < nodes - 1) {
2071                         if (node_x[i] > node_x[i + 1]) {
2072                                 SWAP_TVAL(swap, node_x[i], node_x[i + 1]);
2073                                 if (i) i--;
2074                         }
2075                         else {
2076                                 i++;
2077                         }
2078                 }
2079
2080                 /* Fill the pixels between node pairs. */
2081                 for (i = 0; i < nodes; i += 2) {
2082                         if (node_x[i] >= xmax) break;
2083                         if (node_x[i + 1] >  xmin) {
2084                                 if (node_x[i    ] < xmin) node_x[i    ] = xmin;
2085                                 if (node_x[i + 1] > xmax) node_x[i + 1] = xmax;
2086                                 for (j = node_x[i]; j < node_x[i + 1]; j++) {
2087                                         callback(j - xmin, pixel_y - ymin, userData);
2088                                 }
2089                         }
2090                 }
2091         }
2092         MEM_freeN(node_x);
2093 }
2094
2095 /****************************** Axis Utils ********************************/
2096
2097 /**
2098  * \brief Normal to x,y matrix
2099  *
2100  * Creates a 3x3 matrix from a normal.
2101  * This matrix can be applied to vectors so their 'z' axis runs along \a normal.
2102  * In practice it means you can use x,y as 2d coords. \see
2103  *
2104  * \param r_mat The matrix to return.
2105  * \param normal A unit length vector.
2106  */
2107 bool axis_dominant_v3_to_m3(float r_mat[3][3], const float normal[3])
2108 {
2109         float up[3] = {0.0f, 0.0f, 1.0f};
2110         float axis[3];
2111         float angle;
2112
2113         /* double check they are normalized */
2114         BLI_ASSERT_UNIT_V3(normal);
2115
2116         cross_v3_v3v3(axis, normal, up);
2117         angle = saacos(dot_v3v3(normal, up));
2118
2119         if (angle >= FLT_EPSILON) {
2120                 if (len_squared_v3(axis) < FLT_EPSILON) {
2121                         axis[0] = 0.0f;
2122                         axis[1] = 1.0f;
2123                         axis[2] = 0.0f;
2124                 }
2125
2126                 axis_angle_to_mat3(r_mat, axis, angle);
2127                 return true;
2128         }
2129         else {
2130                 unit_m3(r_mat);
2131                 return false;
2132         }
2133 }
2134
2135 /****************************** Interpolation ********************************/
2136
2137 static float tri_signed_area(const float v1[3], const float v2[3], const float v3[3], const int i, const int j)
2138 {
2139         return 0.5f * ((v1[i] - v2[i]) * (v2[j] - v3[j]) + (v1[j] - v2[j]) * (v3[i] - v2[i]));
2140 }
2141
2142 /* return 1 when degenerate */
2143 static int barycentric_weights(const float v1[3], const float v2[3], const float v3[3], const float co[3], const float n[3], float w[3])
2144 {
2145         float wtot;
2146         int i, j;
2147
2148         axis_dominant_v3(&i, &j, n);
2149
2150         w[0] = tri_signed_area(v2, v3, co, i, j);
2151         w[1] = tri_signed_area(v3, v1, co, i, j);
2152         w[2] = tri_signed_area(v1, v2, co, i, j);
2153
2154         wtot = w[0] + w[1] + w[2];
2155
2156         if (fabsf(wtot) > FLT_EPSILON) {
2157                 mul_v3_fl(w, 1.0f / wtot);
2158                 return 0;
2159         }
2160         else {
2161                 /* zero area triangle */
2162                 copy_v3_fl(w, 1.0f / 3.0f);
2163                 return 1;
2164         }
2165 }
2166
2167 void interp_weights_face_v3(float w[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3], const float co[3])
2168 {
2169         float w2[3];
2170
2171         w[0] = w[1] = w[2] = w[3] = 0.0f;
2172
2173         /* first check for exact match */
2174         if (equals_v3v3(co, v1))
2175                 w[0] = 1.0f;
2176         else if (equals_v3v3(co, v2))
2177                 w[1] = 1.0f;
2178         else if (equals_v3v3(co, v3))
2179                 w[2] = 1.0f;
2180         else if (v4 && equals_v3v3(co, v4))
2181                 w[3] = 1.0f;
2182         else {
2183                 /* otherwise compute barycentric interpolation weights */
2184                 float n1[3], n2[3], n[3];
2185                 int degenerate;
2186
2187                 sub_v3_v3v3(n1, v1, v3);
2188                 if (v4) {
2189                         sub_v3_v3v3(n2, v2, v4);
2190                 }
2191                 else {
2192                         sub_v3_v3v3(n2, v2, v3);
2193                 }
2194                 cross_v3_v3v3(n, n1, n2);
2195
2196                 /* OpenGL seems to split this way, so we do too */
2197                 if (v4) {
2198                         degenerate = barycentric_weights(v1, v2, v4, co, n, w);
2199                         SWAP(float, w[2], w[3]);
2200
2201                         if (degenerate || (w[0] < 0.0f)) {
2202                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2203                                  * so we interpolate using the other triangle */
2204                                 degenerate = barycentric_weights(v2, v3, v4, co, n, w2);
2205
2206                                 if (!degenerate) {
2207                                         w[0] = 0.0f;
2208                                         w[1] = w2[0];
2209                                         w[2] = w2[1];
2210                                         w[3] = w2[2];
2211                                 }
2212                         }
2213                 }
2214                 else
2215                         barycentric_weights(v1, v2, v3, co, n, w);
2216         }
2217 }
2218
2219 /* return 1 of point is inside triangle, 2 if it's on the edge, 0 if point is outside of triangle */
2220 int barycentric_inside_triangle_v2(const float w[3])
2221 {
2222         if (IN_RANGE(w[0], 0.0f, 1.0f) &&
2223             IN_RANGE(w[1], 0.0f, 1.0f) &&
2224             IN_RANGE(w[2], 0.0f, 1.0f))
2225         {
2226                 return 1;
2227         }
2228         else if (IN_RANGE_INCL(w[0], 0.0f, 1.0f) &&
2229                  IN_RANGE_INCL(w[1], 0.0f, 1.0f) &&
2230                  IN_RANGE_INCL(w[2], 0.0f, 1.0f))
2231         {
2232                 return 2;
2233         }
2234
2235         return 0;
2236 }
2237
2238 /* returns 0 for degenerated triangles */
2239 bool barycentric_coords_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
2240 {
2241         float x = co[0], y = co[1];
2242         float x1 = v1[0], y1 = v1[1];
2243         float x2 = v2[0], y2 = v2[1];
2244         float x3 = v3[0], y3 = v3[1];
2245         float det = (y2 - y3) * (x1 - x3) + (x3 - x2) * (y1 - y3);
2246
2247         if (fabsf(det) > FLT_EPSILON) {
2248                 w[0] = ((y2 - y3) * (x - x3) + (x3 - x2) * (y - y3)) / det;
2249                 w[1] = ((y3 - y1) * (x - x3) + (x1 - x3) * (y - y3)) / det;
2250                 w[2] = 1.0f - w[0] - w[1];
2251
2252                 return true;
2253         }
2254
2255         return false;
2256 }
2257
2258 /* used by projection painting
2259  * note: using area_tri_signed_v2 means locations outside the triangle are correctly weighted */
2260 void barycentric_weights_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
2261 {
2262         float wtot;
2263
2264         w[0] = area_tri_signed_v2(v2, v3, co);
2265         w[1] = area_tri_signed_v2(v3, v1, co);
2266         w[2] = area_tri_signed_v2(v1, v2, co);
2267         wtot = w[0] + w[1] + w[2];
2268
2269         if (wtot != 0.0f) {
2270                 mul_v3_fl(w, 1.0f / wtot);
2271         }
2272         else { /* dummy values for zero area face */
2273                 copy_v3_fl(w, 1.0f / 3.0f);
2274         }
2275 }
2276
2277 /* same as #barycentric_weights_v2 but works with a quad,
2278  * note: untested for values outside the quad's bounds
2279  * this is #interp_weights_poly_v2 expanded for quads only */
2280 void barycentric_weights_v2_quad(const float v1[2], const float v2[2], const float v3[2], const float v4[2],
2281                                  const float co[2], float w[4])
2282 {
2283         /* note: fabsf() here is not needed for convex quads (and not used in interp_weights_poly_v2).
2284          *       but in the case of concave/bow-tie quads for the mask rasterizer it gives unreliable results
2285          *       without adding absf(). If this becomes an issue for more general usage we could have
2286          *       this optional or use a different function - Campbell */
2287 #define MEAN_VALUE_HALF_TAN_V2(_area, i1, i2) \
2288                 ((_area = cross_v2v2(dirs[i1], dirs[i2])) != 0.0f ? \
2289                  fabsf(((lens[i1] * lens[i2]) - dot_v2v2(dirs[i1], dirs[i2])) / _area) : 0.0f)
2290
2291         const float dirs[4][2] = {
2292             {v1[0] - co[0], v1[1] - co[1]},
2293             {v2[0] - co[0], v2[1] - co[1]},
2294             {v3[0] - co[0], v3[1] - co[1]},
2295             {v4[0] - co[0], v4[1] - co[1]},
2296         };
2297
2298         const float lens[4] = {
2299             len_v2(dirs[0]),
2300             len_v2(dirs[1]),
2301             len_v2(dirs[2]),
2302             len_v2(dirs[3]),
2303         };
2304
2305         /* avoid divide by zero */
2306         if      (UNLIKELY(lens[0] < FLT_EPSILON)) { w[0] = 1.0f; w[1] = w[2] = w[3] = 0.0f; }
2307         else if (UNLIKELY(lens[1] < FLT_EPSILON)) { w[1] = 1.0f; w[0] = w[2] = w[3] = 0.0f; }
2308         else if (UNLIKELY(lens[2] < FLT_EPSILON)) { w[2] = 1.0f; w[0] = w[1] = w[3] = 0.0f; }
2309         else if (UNLIKELY(lens[3] < FLT_EPSILON)) { w[3] = 1.0f; w[0] = w[1] = w[2] = 0.0f;
2310         }
2311         else {
2312                 float wtot, area;
2313
2314                 /* variable 'area' is just for storage,
2315                  * the order its initialized doesn't matter */
2316 #ifdef __clang__
2317 #  pragma clang diagnostic push
2318 #  pragma clang diagnostic ignored "-Wunsequenced"
2319 #endif
2320
2321                 /* inline mean_value_half_tan four times here */
2322                 float t[4] = {
2323                         MEAN_VALUE_HALF_TAN_V2(area, 0, 1),
2324                         MEAN_VALUE_HALF_TAN_V2(area, 1, 2),
2325                         MEAN_VALUE_HALF_TAN_V2(area, 2, 3),
2326                         MEAN_VALUE_HALF_TAN_V2(area, 3, 0),
2327                 };
2328
2329 #ifdef __clang__
2330 #  pragma clang diagnostic pop
2331 #endif
2332
2333 #undef MEAN_VALUE_HALF_TAN_V2
2334
2335                 w[0] = (t[3] + t[0]) / lens[0];
2336                 w[1] = (t[0] + t[1]) / lens[1];
2337                 w[2] = (t[1] + t[2]) / lens[2];
2338                 w[3] = (t[2] + t[3]) / lens[3];
2339
2340                 wtot = w[0] + w[1] + w[2] + w[3];
2341
2342                 if (wtot != 0.0f) {
2343                         mul_v4_fl(w, 1.0f / wtot);
2344                 }
2345                 else { /* dummy values for zero area face */
2346                         copy_v4_fl(w, 1.0f / 4.0f);
2347                 }
2348         }
2349 }
2350
2351 /* given 2 triangles in 3D space, and a point in relation to the first triangle.
2352  * calculate the location of a point in relation to the second triangle.
2353  * Useful for finding relative positions with geometry */
2354 void barycentric_transform(float pt_tar[3], float const pt_src[3],
2355                            const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3],
2356                            const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3])
2357 {
2358         /* this works by moving the source triangle so its normal is pointing on the Z
2359          * axis where its barycentric wights can be calculated in 2D and its Z offset can
2360          *  be re-applied. The weights are applied directly to the targets 3D points and the
2361          *  z-depth is used to scale the targets normal as an offset.
2362          * This saves transforming the target into its Z-Up orientation and back (which could also work) */
2363         const float z_up[3] = {0, 0, 1};
2364         float no_tar[3], no_src[3];
2365         float quat_src[4];
2366         float pt_src_xy[3];
2367         float tri_xy_src[3][3];
2368         float w_src[3];
2369         float area_tar, area_src;
2370         float z_ofs_src;
2371
2372         normal_tri_v3(no_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3);
2373         normal_tri_v3(no_src, tri_src_p1, tri_src_p2, tri_src_p3);
2374
2375         rotation_between_vecs_to_quat(quat_src, no_src, z_up);
2376         normalize_qt(quat_src);
2377
2378         copy_v3_v3(pt_src_xy, pt_src);
2379         copy_v3_v3(tri_xy_src[0], tri_src_p1);
2380         copy_v3_v3(tri_xy_src[1], tri_src_p2);
2381         copy_v3_v3(tri_xy_src[2], tri_src_p3);
2382
2383         /* make the source tri xy space */
2384         mul_qt_v3(quat_src, pt_src_xy);
2385         mul_qt_v3(quat_src, tri_xy_src[0]);
2386         mul_qt_v3(quat_src, tri_xy_src[1]);
2387         mul_qt_v3(quat_src, tri_xy_src[2]);
2388
2389         barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src);
2390         interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src);
2391
2392         area_tar = sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3));
2393         area_src = sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2]));
2394
2395         z_ofs_src = pt_src_xy[2] - tri_xy_src[0][2];
2396         madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar);
2397 }
2398
2399 /* given an array with some invalid values this function interpolates valid values
2400  * replacing the invalid ones */
2401 int interp_sparse_array(float *array, const int list_size, const float skipval)
2402 {
2403         int found_invalid = 0;
2404         int found_valid = 0;
2405         int i;
2406
2407         for (i = 0; i < list_size; i++) {
2408                 if (array[i] == skipval)
2409                         found_invalid = 1;
2410                 else
2411                         found_valid = 1;
2412         }
2413
2414         if (found_valid == 0) {
2415                 return -1;
2416         }
2417         else if (found_invalid == 0) {
2418                 return 0;
2419         }
2420         else {
2421                 /* found invalid depths, interpolate */
2422                 float valid_last = skipval;
2423                 int valid_ofs = 0;
2424
2425                 float *array_up = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
2426                 float *array_down = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
2427
2428                 int *ofs_tot_up = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tup");
2429                 int *ofs_tot_down = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tdown");
2430
2431                 for (i = 0; i < list_size; i++) {
2432                         if (array[i] == skipval) {
2433                                 array_up[i] = valid_last;
2434                                 ofs_tot_up[i] = ++valid_ofs;
2435                         }
2436                         else {
2437                                 valid_last = array[i];
2438                                 valid_ofs = 0;
2439                         }
2440                 }
2441
2442                 valid_last = skipval;
2443                 valid_ofs = 0;
2444
2445                 for (i = list_size - 1; i >= 0; i--) {
2446                         if (array[i] == skipval) {
2447                                 array_down[i] = valid_last;
2448                                 ofs_tot_down[i] = ++valid_ofs;
2449                         }
2450                         else {
2451                                 valid_last = array[i];
2452                                 valid_ofs = 0;
2453                         }
2454                 }
2455
2456                 /* now blend */
2457                 for (i = 0; i < list_size; i++) {
2458                         if (array[i] == skipval) {
2459                                 if (array_up[i] != skipval && array_down[i] != skipval) {
2460                                         array[i] = ((array_up[i]   * (float)ofs_tot_down[i]) +
2461                                                     (array_down[i] * (float)ofs_tot_up[i])) / (float)(ofs_tot_down[i] + ofs_tot_up[i]);
2462                                 }
2463                                 else if (array_up[i] != skipval) {
2464                                         array[i] = array_up[i];
2465                                 }
2466                                 else if (array_down[i] != skipval) {
2467                                         array[i] = array_down[i];
2468                                 }
2469                         }
2470                 }
2471
2472                 MEM_freeN(array_up);
2473                 MEM_freeN(array_down);
2474
2475                 MEM_freeN(ofs_tot_up);
2476                 MEM_freeN(ofs_tot_down);
2477         }
2478
2479         return 1;
2480 }
2481
2482 /* Mean value weights - smooth interpolation weights for polygons with
2483  * more than 3 vertices */
2484 static float mean_value_half_tan_v3(const float v1[3], const float v2[3], const float v3[3])
2485 {
2486         float d2[3], d3[3], cross[3], area;
2487
2488         sub_v3_v3v3(d2, v2, v1);
2489         sub_v3_v3v3(d3, v3, v1);
2490         cross_v3_v3v3(cross, d2, d3);
2491
2492         area = len_v3(cross);
2493         if (LIKELY(area != 0.0f)) {
2494                 const float dot = dot_v3v3(d2, d3);
2495                 const float len = len_v3(d2) * len_v3(d3);
2496                 return (len - dot) / area;
2497         }
2498         else {
2499                 return 0.0f;
2500         }
2501 }
2502 static float mean_value_half_tan_v2(const float v1[2], const float v2[2], const float v3[2])
2503 {
2504         float d2[2], d3[2], area;
2505
2506         sub_v2_v2v2(d2, v2, v1);
2507         sub_v2_v2v2(d3, v3, v1);
2508
2509         /* different from the 3d version but still correct */
2510         area = cross_v2v2(d2, d3);
2511         if (LIKELY(area != 0.0f)) {
2512                 const float dot = dot_v2v2(d2, d3);
2513                 const float len = len_v2(d2) * len_v2(d3);
2514                 return (len - dot) / area;
2515         }
2516         else {
2517                 return 0.0f;
2518         }
2519 }
2520
2521 void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[3])
2522 {
2523         const float eps = 0.00001f;  /* take care, low values cause [#36105] */
2524         const float eps_sq = eps * eps;
2525         float *v_curr, *v_next;
2526         float ht_prev, ht;  /* half tangents */
2527         float totweight = 0.0f;
2528         int i = 0;
2529         bool vert_interp = false;
2530         bool edge_interp = false;
2531
2532         v_curr = v[0];
2533         v_next = v[1];
2534
2535         ht_prev = mean_value_half_tan_v3(co, v[n - 1], v_curr);
2536
2537         while (i < n) {
2538                 const float len_sq = len_squared_v3v3(co, v_curr);
2539
2540                 /* Mark Mayer et al algorithm that is used here does not operate well if vertex is close
2541                  * to borders of face. In that case, do simple linear interpolation between the two edge vertices */
2542                 if (len_sq < eps_sq) {
2543                         vert_interp = true;
2544                         break;
2545                 }
2546                 else if (dist_squared_to_line_segment_v3(co, v_curr, v_next) < eps_sq) {
2547                         edge_interp = true;
2548                         break;
2549                 }
2550
2551                 ht = mean_value_half_tan_v3(co, v_curr, v_next);
2552                 w[i] = (ht_prev + ht) / sqrtf(len_sq);
2553                 totweight += w[i];
2554
2555                 /* step */
2556                 i++;
2557                 v_curr = v_next;
2558                 v_next = v[(i + 1) % n];
2559
2560                 ht_prev = ht;
2561         }
2562
2563         if (vert_interp) {
2564                 const int i_curr = i;
2565                 for (i = 0; i < n; i++)
2566                         w[i] = 0.0;
2567                 w[i_curr] = 1.0f;
2568         }
2569         else if (edge_interp) {
2570                 const int i_curr = i;
2571                 float len_curr = len_v3v3(co, v_curr);
2572                 float len_next = len_v3v3(co, v_next);
2573                 float edge_len = len_curr + len_next;
2574                 for (i = 0; i < n; i++)
2575                         w[i] = 0.0;
2576
2577                 w[i_curr] = len_next / edge_len;
2578                 w[(i_curr + 1) % n] = len_curr / edge_len;
2579         }
2580         else {
2581                 if (totweight != 0.0f) {
2582                         for (i = 0; i < n; i++) {
2583                                 w[i] /= totweight;
2584                         }
2585                 }
2586         }
2587 }
2588
2589
2590 void interp_weights_poly_v2(float *w, float v[][2], const int n, const float co[2])
2591 {
2592         const float eps = 0.00001f;  /* take care, low values cause [#36105] */
2593         const float eps_sq = eps * eps;
2594         float *v_curr, *v_next;
2595         float ht_prev, ht;  /* half tangents */
2596         float totweight = 0.0f;
2597         int i = 0;
2598         bool vert_interp = false;
2599         bool edge_interp = false;
2600
2601         v_curr = v[0];
2602         v_next = v[1];
2603
2604         ht_prev = mean_value_half_tan_v2(co, v[n - 1], v_curr);
2605
2606         while (i < n) {
2607                 const float len_sq = len_squared_v2v2(co, v_curr);
2608
2609                 /* Mark Mayer et al algorithm that is used here does not operate well if vertex is close
2610                  * to borders of face. In that case, do simple linear interpolation between the two edge vertices */
2611                 if (len_sq < eps_sq) {
2612                         vert_interp = true;
2613                         break;
2614                 }
2615                 else if (dist_squared_to_line_segment_v2(co, v_curr, v_next) < eps_sq) {
2616                         edge_interp = true;
2617                         break;
2618                 }
2619
2620                 ht = mean_value_half_tan_v2(co, v_curr, v_next);
2621                 w[i] = (ht_prev + ht) / sqrtf(len_sq);
2622                 totweight += w[i];
2623
2624                 /* step */
2625                 i++;
2626                 v_curr = v_next;
2627                 v_next = v[(i + 1) % n];
2628
2629                 ht_prev = ht;
2630         }
2631
2632         if (vert_interp) {
2633                 const int i_curr = i;
2634                 for (i = 0; i < n; i++)
2635                         w[i] = 0.0;
2636                 w[i_curr] = 1.0f;
2637         }
2638         else if (edge_interp) {
2639                 const int i_curr = i;
2640                 float len_curr = len_v2v2(co, v_curr);
2641                 float len_next = len_v2v2(co, v_next);
2642                 float edge_len = len_curr + len_next;
2643                 for (i = 0; i < n; i++)
2644                         w[i] = 0.0;
2645
2646                 w[i_curr] = len_next / edge_len;
2647                 w[(i_curr + 1) % n] = len_curr / edge_len;
2648         }
2649         else {
2650                 if (totweight != 0.0f) {
2651                         for (i = 0; i < n; i++) {
2652                                 w[i] /= totweight;
2653                         }
2654                 }
2655         }
2656 }
2657
2658 /* (x1, v1)(t1=0)------(x2, v2)(t2=1), 0<t<1 --> (x, v)(t) */
2659 void interp_cubic_v3(float x[3], float v[3], const float x1[3], const float v1[3], const float x2[3], const float v2[3], const float t)
2660 {
2661         float a[3], b[3];
2662         float t2 = t * t;
2663         float t3 = t2 * t;
2664
2665         /* cubic interpolation */
2666         a[0] = v1[0] + v2[0] + 2 * (x1[0] - x2[0]);
2667         a[1] = v1[1] + v2[1] + 2 * (x1[1] - x2[1]);
2668         a[2] = v1[2] + v2[2] + 2 * (x1[2] - x2[2]);
2669
2670         b[0] = -2 * v1[0] - v2[0] - 3 * (x1[0] - x2[0]);
2671         b[1] = -2 * v1[1] - v2[1] - 3 * (x1[1] - x2[1]);
2672         b[2] = -2 * v1[2] - v2[2] - 3 * (x1[2] - x2[2]);
2673
2674         x[0] = a[0] * t3 + b[0] * t2 + v1[0] * t + x1[0];
2675         x[1] = a[1] * t3 + b[1] * t2 + v1[1] * t + x1[1];
2676         x[2] = a[2] * t3 + b[2] * t2 + v1[2] * t + x1[2];
2677
2678         v[0] = 3 * a[0] * t2 + 2 * b[0] * t + v1[0];
2679         v[1] = 3 * a[1] * t2 + 2 * b[1] * t + v1[1];
2680         v[2] = 3 * a[2] * t2 + 2 * b[2] * t + v1[2];
2681 }
2682
2683 /* unfortunately internal calculations have to be done at double precision to achieve correct/stable results. */
2684
2685 #define IS_ZERO(x) ((x > (-DBL_EPSILON) && x < DBL_EPSILON) ? 1 : 0)
2686
2687 /**
2688  * Barycentric reverse
2689  *
2690  * Compute coordinates (u, v) for point \a st with respect to triangle (\a st0, \a st1, \a st2)
2691  */
2692 void resolve_tri_uv_v2(float r_uv[2], const float st[2],
2693                        const float st0[2], const float st1[2], const float st2[2])
2694 {
2695         /* find UV such that
2696          * t = u * t0 + v * t1 + (1 - u - v) * t2
2697          * u * (t0 - t2) + v * (t1 - t2) = t - t2 */
2698         const double a = st0[0] - st2[0], b = st1[0] - st2[0];
2699         const double c = st0[1] - st2[1], d = st1[1] - st2[1];
2700         const double det = a * d - c * b;
2701
2702         /* det should never be zero since the determinant is the signed ST area of the triangle. */
2703         if (IS_ZERO(det) == 0) {
2704                 const double x[2] = {st[0] - st2[0], st[1] - st2[1]};
2705
2706                 r_uv[0] = (float)((d * x[0] - b * x[1]) / det);
2707                 r_uv[1] = (float)(((-c) * x[0] + a * x[1]) / det);
2708         }
2709         else {
2710                 zero_v2(r_uv);
2711         }
2712 }
2713
2714 /**
2715  * Barycentric reverse 3d
2716  *
2717  * Compute coordinates (u, v) for point \a st with respect to triangle (\a st0, \a st1, \a st2)
2718  */
2719 void resolve_tri_uv_v3(float r_uv[2], const float st[3], const float st0[3], const float st1[3], const float st2[3])
2720 {
2721         float v0[3], v1[3], v2[3];
2722         double d00, d01, d11, d20, d21, det;
2723
2724         sub_v3_v3v3(v0, st1, st0);
2725         sub_v3_v3v3(v1, st2, st0);
2726         sub_v3_v3v3(v2, st, st0);
2727
2728         d00 = dot_v3v3(v0, v0);
2729         d01 = dot_v3v3(v0, v1);
2730         d11 = dot_v3v3(v1, v1);
2731         d20 = dot_v3v3(v2, v0);
2732         d21 = dot_v3v3(v2, v1);
2733
2734         det = d00 * d11 - d01 * d01;
2735
2736         /* det should never be zero since the determinant is the signed ST area of the triangle. */
2737         if (IS_ZERO(det) == 0) {
2738                 float w;
2739
2740                 w =       (float)((d00 * d21 - d01 * d20) / det);
2741                 r_uv[1] = (float)((d11 * d20 - d01 * d21) / det);
2742                 r_uv[0] = 1.0f - r_uv[1] - w;
2743         }
2744         else {
2745                 zero_v2(r_uv);
2746         }
2747 }
2748
2749 /* bilinear reverse */
2750 void resolve_quad_uv_v2(float r_uv[2], const float st[2],
2751                         const float st0[2], const float st1[2], const float st2[2], const float st3[2])
2752 {
2753         resolve_quad_uv_v2_deriv(r_uv, NULL, st, st0, st1, st2, st3);
2754 }
2755
2756 /* bilinear reverse with derivatives */
2757 void resolve_quad_uv_v2_deriv(float r_uv[2], float r_deriv[2][2],
2758                               const float st[2], const float st0[2], const float st1[2], const float st2[2], const float st3[2])
2759 {
2760         const double signed_area = (st0[0] * st1[1] - st0[1] * st1[0]) + (st1[0] * st2[1] - st1[1] * st2[0]) +
2761                                    (st2[0] * st3[1] - st2[1] * st3[0]) + (st3[0] * st0[1] - st3[1] * st0[0]);
2762
2763         /* X is 2D cross product (determinant)
2764          * A = (p0 - p) X (p0 - p3)*/
2765         const double a = (st0[0] - st[0]) * (st0[1] - st3[1]) - (st0[1] - st[1]) * (st0[0] - st3[0]);
2766
2767         /* B = ( (p0 - p) X (p1 - p2) + (p1 - p) X (p0 - p3) ) / 2 */
2768         const double b = 0.5 * (double)(((st0[0] - st[0]) * (st1[1] - st2[1]) - (st0[1] - st[1]) * (st1[0] - st2[0])) +
2769                                         ((st1[0] - st[0]) * (st0[1] - st3[1]) - (st1[1] - st[1]) * (st0[0] - st3[0])));
2770
2771         /* C = (p1-p) X (p1-p2) */
2772         const double fC = (st1[0] - st[0]) * (st1[1] - st2[1]) - (st1[1] - st[1]) * (st1[0] - st2[0]);
2773         double denom = a - 2 * b + fC;
2774
2775         /* clear outputs */
2776         zero_v2(r_uv);
2777
2778         if (IS_ZERO(denom) != 0) {
2779                 const double fDen = a - fC;
2780                 if (IS_ZERO(fDen) == 0)
2781                         r_uv[0] = (float)(a / fDen);
2782         }
2783         else {
2784                 const double desc_sq = b * b - a * fC;
2785                 const double desc = sqrt(desc_sq < 0.0 ? 0.0 : desc_sq);
2786                 const double s = signed_area > 0 ? (-1.0) : 1.0;
2787
2788                 r_uv[0] = (float)(((a - b) + s * desc) / denom);
2789         }
2790
2791         /* find UV such that
2792          * fST = (1-u)(1-v) * ST0 + u * (1-v) * ST1 + u * v * ST2 + (1-u) * v * ST3 */
2793         {
2794                 const double denom_s = (1 - r_uv[0]) * (st0[0] - st3[0]) + r_uv[0] * (st1[0] - st2[0]);
2795                 const double denom_t = (1 - r_uv[0]) * (st0[1] - st3[1]) + r_uv[0] * (st1[1] - st2[1]);
2796                 int i = 0;
2797                 denom = denom_s;
2798
2799                 if (fabs(denom_s) < fabs(denom_t)) {
2800                         i = 1;
2801                         denom = denom_t;
2802                 }
2803
2804                 if (IS_ZERO(denom) == 0)
2805                         r_uv[1] = (float)((double)((1.0f - r_uv[0]) * (st0[i] - st[i]) + r_uv[0] * (st1[i] - st[i])) / denom);
2806         }
2807
2808         if (r_deriv) {
2809                 float tmp1[2], tmp2[2], s[2], t[2];
2810                 
2811                 /* clear outputs */
2812                 zero_v2(r_deriv[0]);
2813                 zero_v2(r_deriv[1]);
2814                 
2815                 sub_v2_v2v2(tmp1, st1, st0);
2816                 sub_v2_v2v2(tmp2, st2, st3);
2817                 interp_v2_v2v2(s, tmp1, tmp2, r_uv[1]);
2818                 sub_v2_v2v2(tmp1, st3, st0);
2819                 sub_v2_v2v2(tmp2, st2, st1);
2820                 interp_v2_v2v2(t, tmp1, tmp2, r_uv[0]);
2821
2822                 denom = t[0] * s[1] - t[1] * s[0];
2823
2824                 if (!IS_ZERO(denom)) {
2825                         double inv_denom = 1.0 / denom;
2826                         r_deriv[0][0] = (float)((double)-t[1] * inv_denom);
2827                         r_deriv[0][1] = (float)((double) t[0] * inv_denom);
2828                         r_deriv[1][0] = (float)((double) s[1] * inv_denom);
2829                         r_deriv[1][1] = (float)((double)-s[0] * inv_denom);
2830                 }
2831         }
2832 }
2833
2834 #undef IS_ZERO
2835
2836 /* reverse of the functions above */
2837 void interp_bilinear_quad_v3(float data[4][3], float u, float v, float res[3])
2838 {
2839         float vec[3];
2840
2841         copy_v3_v3(res, data[0]);
2842         mul_v3_fl(res, (1 - u) * (1 - v));
2843         copy_v3_v3(vec, data[1]);
2844         mul_v3_fl(vec, u * (1 - v)); add_v3_v3(res, vec);
2845         copy_v3_v3(vec, data[2]);
2846         mul_v3_fl(vec, u * v); add_v3_v3(res, vec);
2847         copy_v3_v3(vec, data[3]);
2848         mul_v3_fl(vec, (1 - u) * v); add_v3_v3(res, vec);
2849 }
2850
2851 void interp_barycentric_tri_v3(float data[3][3], float u, float v, float res[3])
2852 {
2853         float vec[3];
2854
2855         copy_v3_v3(res, data[0]);
2856         mul_v3_fl(res, u);
2857         copy_v3_v3(vec, data[1]);
2858         mul_v3_fl(vec, v); add_v3_v3(res, vec);
2859         copy_v3_v3(vec, data[2]);
2860         mul_v3_fl(vec, 1.0f - u - v); add_v3_v3(res, vec);
2861 }
2862
2863 /***************************** View & Projection *****************************/
2864
2865 void orthographic_m4(float matrix[4][4], const float left, const float right, const float bottom, const float top,
2866                      const float nearClip, const float farClip)
2867 {
2868         float Xdelta, Ydelta, Zdelta;
2869
2870         Xdelta = right - left;
2871         Ydelta = top - bottom;
2872         Zdelta = farClip - nearClip;
2873         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
2874                 return;
2875         }
2876         unit_m4(matrix);
2877         matrix[0][0] = 2.0f / Xdelta;
2878         matrix[3][0] = -(right + left) / Xdelta;
2879         matrix[1][1] = 2.0f / Ydelta;
2880         matrix[3][1] = -(top + bottom) / Ydelta;
2881         matrix[2][2] = -2.0f / Zdelta; /* note: negate Z        */
2882         matrix[3][2] = -(farClip + nearClip) / Zdelta;
2883 }
2884
2885 void perspective_m4(float mat[4][4], const float left, const float right, const float bottom, const float top,
2886                     const float nearClip, const float farClip)
2887 {
2888         float Xdelta, Ydelta, Zdelta;
2889
2890         Xdelta = right - left;
2891         Ydelta = top - bottom;
2892         Zdelta = farClip - nearClip;
2893
2894         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
2895                 return;
2896         }
2897         mat[0][0] = nearClip * 2.0f / Xdelta;
2898         mat[1][1] = nearClip * 2.0f / Ydelta;
2899         mat[2][0] = (right + left) / Xdelta; /* note: negate Z  */
2900         mat[2][1] = (top + bottom) / Ydelta;
2901         mat[2][2] = -(farClip + nearClip) / Zdelta;
2902         mat[2][3] = -1.0f;
2903         mat[3][2] = (-2.0f * nearClip * farClip) / Zdelta;
2904         mat[0][1] = mat[0][2] = mat[0][3] =
2905                 mat[1][0] = mat[1][2] = mat[1][3] =
2906                 mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
2907
2908 }
2909
2910 /* translate a matrix created by orthographic_m4 or perspective_m4 in XY coords (used to jitter the view) */
2911 void window_translate_m4(float winmat[4][4], float perspmat[4][4], const float x, const float y)
2912 {
2913         if (winmat[2][3] == -1.0f) {
2914                 /* in the case of a win-matrix, this means perspective always */
2915                 float v1[3];
2916                 float v2[3];
2917                 float len1, len2;
2918
2919                 v1[0] = perspmat[0][0];
2920                 v1[1] = perspmat[1][0];
2921                 v1[2] = perspmat[2][0];
2922
2923                 v2[0] = perspmat[0][1];
2924                 v2[1] = perspmat[1][1];
2925                 v2[2] = perspmat[2][1];
2926
2927                 len1 = (1.0f / len_v3(v1));
2928                 len2 = (1.0f / len_v3(v2));
2929
2930                 winmat[2][0] += len1 * winmat[0][0] * x;
2931                 winmat[2][1] += len2 * winmat[1][1] * y;
2932         }
2933         else {
2934                 winmat[3][0] += x;
2935                 winmat[3][1] += y;
2936         }
2937 }
2938
2939 static void i_multmatrix(float icand[4][4], float Vm[4][4])
2940 {
2941         int row, col;
2942         float temp[4][4];
2943
2944         for (row = 0; row < 4; row++)
2945                 for (col = 0; col < 4; col++)
2946                         temp[row][col] = (icand[row][0] * Vm[0][col] +
2947                                           icand[row][1] * Vm[1][col] +
2948                                           icand[row][2] * Vm[2][col] +
2949                                           icand[row][3] * Vm[3][col]);
2950         copy_m4_m4(Vm, temp);
2951 }
2952
2953 void polarview_m4(float Vm[4][4], float dist, float azimuth, float incidence, float twist)
2954 {
2955
2956         unit_m4(Vm);
2957
2958         translate_m4(Vm, 0.0, 0.0, -dist);
2959         rotate_m4(Vm, 'Z', -twist);
2960         rotate_m4(Vm, 'X', -incidence);
2961         rotate_m4(Vm, 'Z', -azimuth);
2962 }
2963
2964 void lookat_m4(float mat[4][4], float vx, float vy, float vz, float px, float py, float pz, float twist)
2965 {
2966         float sine, cosine, hyp, hyp1, dx, dy, dz;
2967         float mat1[4][4] = MAT4_UNITY;
2968
2969         unit_m4(mat);
2970
2971         rotate_m4(mat, 'Z', -twist);
2972
2973         dx = px - vx;
2974         dy = py - vy;
2975         dz = pz - vz;
2976         hyp = dx * dx + dz * dz; /* hyp squared */
2977         hyp1 = (float)sqrt(dy * dy + hyp);
2978         hyp = (float)sqrt(hyp); /* the real hyp */
2979
2980         if (hyp1 != 0.0f) { /* rotate X */
2981                 sine = -dy / hyp1;
2982                 cosine = hyp / hyp1;
2983         }
2984         else {
2985                 sine = 0;
2986                 cosine = 1.0f;
2987         }
2988         mat1[1][1] = cosine;
2989         mat1[1][2] = sine;
2990         mat1[2][1] = -sine;
2991         mat1[2][2] = cosine;
2992
2993         i_multmatrix(mat1, mat);
2994
2995         mat1[1][1] = mat1[2][2] = 1.0f; /* be careful here to reinit    */
2996         mat1[1][2] = mat1[2][1] = 0.0; /* those modified by the last    */
2997
2998         /* paragraph    */
2999         if (hyp != 0.0f) { /* rotate Y  */
3000                 sine = dx / hyp;
3001                 cosine = -dz / hyp;
3002         }
3003         else {
3004                 sine = 0;
3005                 cosine = 1.0f;
3006         }
3007         mat1[0][0] = cosine;
3008         mat1[0][2] = -sine;
3009         mat1[2][0] = sine;
3010         mat1[2][2] = cosine;
3011
3012         i_multmatrix(mat1, mat);
3013         translate_m4(mat, -vx, -vy, -vz); /* translate viewpoint to origin */
3014 }
3015
3016 int box_clip_bounds_m4(float boundbox[2][3], const float bounds[4], float winmat[4][4])
3017 {
3018         float mat[4][4], vec[4];
3019         int a, fl, flag = -1;
3020
3021         copy_m4_m4(mat, winmat);
3022
3023         for (a = 0; a < 8; a++) {
3024                 vec[0] = (a & 1) ? boundbox[0][0] : boundbox[1][0];
3025                 vec[1] = (a & 2) ? boundbox[0][1] : boundbox[1][1];
3026                 vec[2] = (a & 4) ? boundbox[0][2] : boundbox[1][2];
3027                 vec[3] = 1.0;
3028                 mul_m4_v4(mat, vec);
3029
3030                 fl = 0;
3031                 if (bounds) {
3032                         if (vec[0] > bounds[1] * vec[3]) fl |= 1;
3033                         if (vec[0] < bounds[0] * vec[3]) fl |= 2;
3034                         if (vec[1] > bounds[3] * vec[3]) fl |= 4;
3035                         if (vec[1] < bounds[2] * vec[3]) fl |= 8;
3036                 }
3037                 else {
3038                         if (vec[0] < -vec[3]) fl |= 1;
3039                         if (vec[0] > vec[3]) fl |= 2;
3040                         if (vec[1] < -vec[3]) fl |= 4;
3041                         if (vec[1] > vec[3]) fl |= 8;
3042                 }
3043                 if (vec[2] < -vec[3]) fl |= 16;
3044                 if (vec[2] > vec[3]) fl |= 32;
3045
3046                 flag &= fl;
3047                 if (flag == 0) return 0;
3048         }
3049
3050         return flag;
3051 }
3052
3053 void box_minmax_bounds_m4(float min[3], float max[3], float boundbox[2][3], float mat[4][4])
3054 {
3055         float mn[3], mx[3], vec[3];
3056         int a;
3057
3058         copy_v3_v3(mn, min);
3059         copy_v3_v3(mx, max);
3060
3061         for (a = 0; a < 8; a++) {
3062                 vec[0] = (a & 1) ? boundbox[0][0] : boundbox[1][0];
3063                 vec[1] = (a & 2) ? boundbox[0][1] : boundbox[1][1];
3064                 vec[2] = (a & 4) ? boundbox[0][2] : boundbox[1][2];
3065
3066                 mul_m4_v3(mat, vec);
3067                 minmax_v3v3_v3(mn, mx, vec);
3068         }
3069
3070         copy_v3_v3(min, mn);
3071         copy_v3_v3(max, mx);
3072 }
3073
3074 /********************************** Mapping **********************************/
3075
3076 void map_to_tube(float *r_u, float *r_v, const float x, const float y, const float z)
3077 {
3078         float len;
3079
3080         *r_v = (z + 1.0f) / 2.0f;
3081
3082         len = sqrtf(x * x + y * y);
3083         if (len > 0.0f) {
3084                 *r_u = (float)((1.0 - (atan2(x / len, y / len) / M_PI)) / 2.0);
3085         }
3086         else {
3087                 *r_v = *r_u = 0.0f; /* to avoid un-initialized variables */
3088         }
3089 }
3090
3091 void map_to_sphere(float *r_u, float *r_v, const float x, const float y, const float z)
3092 {
3093         float len;
3094
3095         len = sqrtf(x * x + y * y + z * z);
3096         if (len > 0.0f) {
3097                 if (x == 0.0f && y == 0.0f) *r_u = 0.0f;  /* othwise domain error */
3098                 else *r_u = (1.0f - atan2f(x, y) / (float)M_PI) / 2.0f;
3099
3100                 *r_v = 1.0f - (float)saacos(z / len) / (float)M_PI;
3101         }
3102         else {
3103                 *r_v = *r_u = 0.0f; /* to avoid un-initialized variables */
3104         }
3105 }
3106
3107 /********************************* Normals **********************************/
3108
3109 void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3],
3110                                float n4[3], const float f_no[3], const float co1[3], const float co2[3],
3111                                const float co3[3], const float co4[3])
3112 {
3113         float vdiffs[4][3];
3114         const int nverts = (n4 != NULL && co4 != NULL) ? 4 : 3;
3115
3116         /* compute normalized edge vectors */
3117         sub_v3_v3v3(vdiffs[0], co2, co1);
3118         sub_v3_v3v3(vdiffs[1], co3, co2);
3119
3120         if (nverts == 3) {
3121                 sub_v3_v3v3(vdiffs[2], co1, co3);
3122         }
3123         else {
3124                 sub_v3_v3v3(vdiffs[2], co4, co3);
3125                 sub_v3_v3v3(vdiffs[3], co1, co4);
3126                 normalize_v3(vdiffs[3]);
3127         }
3128
3129         normalize_v3(vdiffs[0]);
3130         normalize_v3(vdiffs[1]);
3131         normalize_v3(vdiffs[2]);
3132
3133         /* accumulate angle weighted face normal */
3134         {
3135                 float *vn[] = {n1, n2, n3, n4};
3136                 const float *prev_edge = vdiffs[nverts - 1];
3137                 int i;
3138
3139                 for (i = 0; i < nverts; i++) {
3140                         const float *cur_edge = vdiffs[i];
3141                         const float fac = saacos(-dot_v3v3(cur_edge, prev_edge));
3142
3143                         /* accumulate */
3144                         madd_v3_v3fl(vn[i], f_no, fac);
3145                         prev_edge = cur_edge;
3146                 }
3147         }
3148 }
3149
3150 /* Add weighted face normal component into normals of the face vertices.
3151  * Caller must pass pre-allocated vdiffs of nverts length. */
3152 void accumulate_vertex_normals_poly(float **vertnos, const float polyno[3],
3153                                     const float **vertcos, float vdiffs[][3], const int nverts)
3154 {
3155         int i;
3156
3157         /* calculate normalized edge directions for each edge in the poly */
3158         for (i = 0; i < nverts; i++) {
3159                 sub_v3_v3v3(vdiffs[i], vertcos[(i + 1) % nverts], vertcos[i]);
3160                 normalize_v3(vdiffs[i]);
3161         }
3162
3163         /* accumulate angle weighted face normal */
3164         {
3165                 const float *prev_edge = vdiffs[nverts - 1];
3166
3167                 for (i = 0; i < nverts; i++) {
3168                         const float *cur_edge = vdiffs[i];
3169
3170                         /* calculate angle between the two poly edges incident on
3171                          * this vertex */
3172                         const float fac = saacos(-dot_v3v3(cur_edge, prev_edge));
3173
3174                         /* accumulate */
3175                         madd_v3_v3fl(vertnos[i], polyno, fac);
3176                         prev_edge = cur_edge;
3177                 }
3178         }
3179 }
3180
3181 /********************************* Tangents **********************************/
3182
3183 void tangent_from_uv(float uv1[2], float uv2[2], float uv3[3], float co1[3], float co2[3], float co3[3], float n[3], float tang[3])
3184 {
3185         float s1 = uv2[0] - uv1[0];
3186         float s2 = uv3[0] - uv1[0];
3187         float t1 = uv2[1] - uv1[1];
3188         float t2 = uv3[1] - uv1[1];
3189         float det = (s1 * t2 - s2 * t1);
3190
3191         if (det != 0.0f) { /* otherwise 'tang' becomes nan */
3192                 float tangv[3], ct[3], e1[3], e2[3];
3193
3194                 det = 1.0f / det;
3195
3196                 /* normals in render are inversed... */
3197                 sub_v3_v3v3(e1, co1, co2);
3198                 sub_v3_v3v3(e2, co1, co3);
3199                 tang[0] = (t2 * e1[0] - t1 * e2[0]) * det;
3200                 tang[1] = (t2 * e1[1] - t1 * e2[1]) * det;
3201                 tang[2] = (t2 * e1[2] - t1 * e2[2]) * det;
3202                 tangv[0] = (s1 * e2[0] - s2 * e1[0]) * det;
3203                 tangv[1] = (s1 * e2[1] - s2 * e1[1]) * det;
3204                 tangv[2] = (s1 * e2[2] - s2 * e1[2]) * det;
3205                 cross_v3_v3v3(ct, tang, tangv);
3206
3207                 /* check flip */
3208                 if (dot_v3v3(ct, n) < 0.0f) {
3209                         negate_v3(tang);
3210                 }
3211         }
3212         else {
3213                 tang[0] = tang[1] = tang[2] = 0.0;
3214         }
3215 }
3216
3217 /****************************** Vector Clouds ********************************/
3218
3219 /* vector clouds */
3220 /* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight, float (*rpos)[3], float *rweight,
3221  *                                float lloc[3], float rloc[3], float lrot[3][3], float lscale[3][3])
3222  *
3223  * input
3224  * (
3225  * int list_size
3226  * 4 lists as pointer to array[list_size]
3227  * 1. current pos array of 'new' positions
3228  * 2. current weight array of 'new'weights (may be NULL pointer if you have no weights )
3229  * 3. reference rpos array of 'old' positions
3230  * 4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights )
3231  * )
3232  * output
3233  * (
3234  * float lloc[3] center of mass pos
3235  * float rloc[3] center of mass rpos
3236  * float lrot[3][3] rotation matrix
3237  * float lscale[3][3] scale matrix
3238  * pointers may be NULL if not needed
3239  * )
3240  */
3241
3242 void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight, float (*rpos)[3], float *rweight,
3243                                float lloc[3], float rloc[3], float lrot[3][3], float lscale[3][3])
3244 {
3245         float accu_com[3] = {0.0f, 0.0f, 0.0f}, accu_rcom[3] = {0.0f, 0.0f, 0.0f};
3246         float accu_weight = 0.0f, accu_rweight = 0.0f, eps = 0.000001f;
3247
3248         int a;
3249         /* first set up a nice default response */
3250         if (lloc) zero_v3(lloc);
3251         if (rloc) zero_v3(rloc);
3252         if (lrot) unit_m3(lrot);
3253         if (lscale) unit_m3(lscale);
3254         /* do com for both clouds */
3255         if (pos && rpos && (list_size > 0)) { /* paranoya check */
3256                 /* do com for both clouds */
3257                 for (a = 0; a < list_size; a++) {
3258                         if (weight) {
3259                                 float v[3];
3260                                 copy_v3_v3(v, pos[a]);
3261                                 mul_v3_fl(v, weight[a]);
3262                                 add_v3_v3(accu_com, v);
3263                                 accu_weight += weight[a];
3264                         }
3265                         else {
3266                                 add_v3_v3(accu_com, pos[a]);
3267                         }
3268
3269                         if (rweight) {
3270                                 float v[3];
3271                                 copy_v3_v3(v, rpos[a]);
3272                                 mul_v3_fl(v, rweight[a]);
3273                                 add_v3_v3(accu_rcom, v);
3274                                 accu_rweight += rweight[a];
3275                         }
3276                         else {
3277                                 add_v3_v3(accu_rcom, rpos[a]);
3278                         }
3279                 }
3280                 if (!weight || !rweight) {
3281                         accu_weight = accu_rweight = (float)list_size;
3282                 }
3283
3284                 mul_v3_fl(accu_com, 1.0f / accu_weight);
3285                 mul_v3_fl(accu_rcom, 1.0f / accu_rweight);
3286                 if (lloc) copy_v3_v3(lloc, accu_com);
3287                 if (rloc) copy_v3_v3(rloc, accu_rcom);
3288                 if (lrot || lscale) { /* caller does not want rot nor scale, strange but legal */
3289                         /*so now do some reverse engineering and see if we can split rotation from scale ->Polardecompose*/
3290                         /* build 'projection' matrix */
3291                         float m[3][3], mr[3][3], q[3][3], qi[3][3];
3292                         float va[3], vb[3], stunt[3];
3293                         float odet, ndet;
3294                         int i = 0, imax = 15;
3295                         zero_m3(m);
3296                         zero_m3(mr);
3297
3298                         /* build 'projection' matrix */
3299                         for (a = 0; a < list_size; a++) {
3300                                 sub_v3_v3v3(va, rpos[a], accu_rcom);
3301                                 /* mul_v3_fl(va, bp->mass);  mass needs renormalzation here ?? */
3302                                 sub_v3_v3v3(vb, pos[a], accu_com);
3303                                 /* mul_v3_fl(va, rp->mass); */
3304                                 m[0][0] += va[0] * vb[0];
3305                                 m[0][1] += va[0] * vb[1];
3306                                 m[0][2] += va[0] * vb[2];
3307
3308                                 m[1][0] += va[1] * vb[0];
3309                                 m[1][1] += va[1] * vb[1];
3310                                 m[1][2] += va[1] * vb[2];
3311
3312                                 m[2][0] += va[2] * vb[0];
3313                                 m[2][1] += va[2] * vb[1];
3314                                 m[2][2] += va[2] * vb[2];
3315
3316                                 /* building the reference matrix on the fly
3317                                  * needed to scale properly later */
3318
3319                                 mr[0][0] += va[0] * va[0];
3320                                 mr[0][1] += va[0] * va[1];
3321                                 mr[0][2] += va[0] * va[2];
3322
3323                                 mr[1][0] += va[1] * va[0];
3324                                 mr[1][1] += va[1] * va[1];
3325                                 mr[1][2] += va[1] * va[2];
3326
3327                                 mr[2][0] += va[2] * va[0];
3328                                 mr[2][1] += va[2] * va[1];
3329                                 mr[2][2] += va[2] * va[2];
3330                 &nb