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