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