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