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