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