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