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