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