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