Cleanup: quiet warnings
[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 #include "MEM_guardedalloc.h"
31
32 #include "BLI_math.h"
33 #include "BLI_math_bits.h"
34 #include "BLI_utildefines.h"
35
36 #include "BLI_strict_flags.h"
37
38 /********************************** Polygons *********************************/
39
40 void cross_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
41 {
42         float n1[3], n2[3];
43
44         n1[0] = v1[0] - v2[0];
45         n2[0] = v2[0] - v3[0];
46         n1[1] = v1[1] - v2[1];
47         n2[1] = v2[1] - v3[1];
48         n1[2] = v1[2] - v2[2];
49         n2[2] = v2[2] - v3[2];
50         n[0] = n1[1] * n2[2] - n1[2] * n2[1];
51         n[1] = n1[2] * n2[0] - n1[0] * n2[2];
52         n[2] = n1[0] * n2[1] - n1[1] * n2[0];
53 }
54
55 float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
56 {
57         float n1[3], n2[3];
58
59         n1[0] = v1[0] - v2[0];
60         n2[0] = v2[0] - v3[0];
61         n1[1] = v1[1] - v2[1];
62         n2[1] = v2[1] - v3[1];
63         n1[2] = v1[2] - v2[2];
64         n2[2] = v2[2] - v3[2];
65         n[0] = n1[1] * n2[2] - n1[2] * n2[1];
66         n[1] = n1[2] * n2[0] - n1[0] * n2[2];
67         n[2] = n1[0] * n2[1] - n1[1] * n2[0];
68
69         return normalize_v3(n);
70 }
71
72 float normal_quad_v3(float n[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
73 {
74         /* real cross! */
75         float n1[3], n2[3];
76
77         n1[0] = v1[0] - v3[0];
78         n1[1] = v1[1] - v3[1];
79         n1[2] = v1[2] - v3[2];
80
81         n2[0] = v2[0] - v4[0];
82         n2[1] = v2[1] - v4[1];
83         n2[2] = v2[2] - v4[2];
84
85         n[0] = n1[1] * n2[2] - n1[2] * n2[1];
86         n[1] = n1[2] * n2[0] - n1[0] * n2[2];
87         n[2] = n1[0] * n2[1] - n1[1] * n2[0];
88
89         return normalize_v3(n);
90 }
91
92 /**
93  * Computes the normal of a planar
94  * polygon See Graphics Gems for
95  * computing newell normal.
96  */
97 float normal_poly_v3(float n[3], const float verts[][3], unsigned int nr)
98 {
99         cross_poly_v3(n, verts, nr);
100         return normalize_v3(n);
101 }
102
103 float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
104 {
105         const float verts[4][3] = {{UNPACK3(v1)}, {UNPACK3(v2)}, {UNPACK3(v3)}, {UNPACK3(v4)}};
106         return area_poly_v3(verts, 4);
107 }
108
109 float area_squared_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
110 {
111         const float verts[4][3] = {{UNPACK3(v1)}, {UNPACK3(v2)}, {UNPACK3(v3)}, {UNPACK3(v4)}};
112         return area_squared_poly_v3(verts, 4);
113 }
114
115 /* Triangles */
116 float area_tri_v3(const float v1[3], const float v2[3], const float v3[3])
117 {
118         float n[3];
119         cross_tri_v3(n, v1, v2, v3);
120         return len_v3(n) * 0.5f;
121 }
122
123 float area_squared_tri_v3(const float v1[3], const float v2[3], const float v3[3])
124 {
125         float n[3];
126         cross_tri_v3(n, v1, v2, v3);
127         mul_v3_fl(n, 0.5f);
128         return len_squared_v3(n);
129 }
130
131 float area_tri_signed_v3(const float v1[3], const float v2[3], const float v3[3], const float normal[3])
132 {
133         float area, n[3];
134
135         cross_tri_v3(n, v1, v2, v3);
136         area = len_v3(n) * 0.5f;
137
138         /* negate area for flipped triangles */
139         if (dot_v3v3(n, normal) < 0.0f)
140                 area = -area;
141
142         return area;
143 }
144
145 float area_poly_v3(const float verts[][3], unsigned int nr)
146 {
147         float n[3];
148         cross_poly_v3(n, verts, nr);
149         return len_v3(n) * 0.5f;
150 }
151
152 float area_squared_poly_v3(const float verts[][3], unsigned int nr)
153 {
154         float n[3];
155
156         cross_poly_v3(n, verts, nr);
157         mul_v3_fl(n, 0.5f);
158         return len_squared_v3(n);
159 }
160
161 /**
162  * Scalar cross product of a 2d polygon.
163  *
164  * - equivalent to ``area * 2``
165  * - useful for checking polygon winding (a positive value is clockwise).
166  */
167 float cross_poly_v2(const float verts[][2], unsigned int nr)
168 {
169         unsigned int a;
170         float cross;
171         const float *co_curr, *co_prev;
172
173         /* The Trapezium Area Rule */
174         co_prev = verts[nr - 1];
175         co_curr = verts[0];
176         cross = 0.0f;
177         for (a = 0; a < nr; a++) {
178                 cross += (co_curr[0] - co_prev[0]) * (co_curr[1] + co_prev[1]);
179                 co_prev = co_curr;
180                 co_curr += 2;
181         }
182
183         return cross;
184 }
185
186 void cross_poly_v3(float n[3], const float verts[][3], unsigned int nr)
187 {
188         const float *v_prev = verts[nr - 1];
189         const float *v_curr = verts[0];
190         unsigned int i;
191
192         zero_v3(n);
193
194         /* Newell's Method */
195         for (i = 0; i < nr; v_prev = v_curr, v_curr = verts[++i]) {
196                 add_newell_cross_v3_v3v3(n, v_prev, v_curr);
197         }
198 }
199
200 float area_poly_v2(const float verts[][2], unsigned int nr)
201 {
202         return fabsf(0.5f * cross_poly_v2(verts, nr));
203 }
204
205 float area_poly_signed_v2(const float verts[][2], unsigned int nr)
206 {
207         return (0.5f * cross_poly_v2(verts, nr));
208 }
209
210 float area_squared_poly_v2(const float verts[][2], unsigned int nr)
211 {
212         float area = area_poly_signed_v2(verts, nr);
213         return area * area;
214 }
215
216 float cotangent_tri_weight_v3(const float v1[3], const float v2[3], const float v3[3])
217 {
218         float a[3], b[3], c[3], c_len;
219
220         sub_v3_v3v3(a, v2, v1);
221         sub_v3_v3v3(b, v3, v1);
222         cross_v3_v3v3(c, a, b);
223
224         c_len = len_v3(c);
225
226         if (c_len > FLT_EPSILON) {
227                 return dot_v3v3(a, b) / c_len;
228         }
229         else {
230                 return 0.0f;
231         }
232 }
233
234 /********************************* Planes **********************************/
235
236 /**
237  * Calculate a plane from a point and a direction,
238  * \note \a point_no isn't required to be normalized.
239  */
240 void plane_from_point_normal_v3(float r_plane[4], const float plane_co[3], const float plane_no[3])
241 {
242         copy_v3_v3(r_plane, plane_no);
243         r_plane[3] = -dot_v3v3(r_plane, plane_co);
244 }
245
246 /**
247  * Get a point and a direction from a plane.
248  */
249 void plane_to_point_vector_v3(const float plane[4], float r_plane_co[3], float r_plane_no[3])
250 {
251         mul_v3_v3fl(r_plane_co, plane, (-plane[3] / len_squared_v3(plane)));
252         copy_v3_v3(r_plane_no, plane);
253 }
254
255 /**
256  * version of #plane_to_point_vector_v3 that gets a unit length vector.
257  */
258 void plane_to_point_vector_v3_normalized(const float plane[4], float r_plane_co[3], float r_plane_no[3])
259 {
260         const float length = normalize_v3_v3(r_plane_no, plane);
261         mul_v3_v3fl(r_plane_co, r_plane_no, (-plane[3] / length));
262 }
263
264 /********************************* Volume **********************************/
265
266 /**
267  * The volume from a tetrahedron, points can be in any order
268  */
269 float volume_tetrahedron_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
270 {
271         float m[3][3];
272         sub_v3_v3v3(m[0], v1, v2);
273         sub_v3_v3v3(m[1], v2, v3);
274         sub_v3_v3v3(m[2], v3, v4);
275         return fabsf(determinant_m3_array(m)) / 6.0f;
276 }
277
278 /**
279  * The volume from a tetrahedron, normal pointing inside gives negative volume
280  */
281 float volume_tetrahedron_signed_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
282 {
283         float m[3][3];
284         sub_v3_v3v3(m[0], v1, v2);
285         sub_v3_v3v3(m[1], v2, v3);
286         sub_v3_v3v3(m[2], v3, v4);
287         return determinant_m3_array(m) / 6.0f;
288 }
289
290
291 /********************************* Distance **********************************/
292
293 /* distance p to line v1-v2
294  * using Hesse formula, NO LINE PIECE! */
295 float dist_squared_to_line_v2(const float p[2], const float l1[2], const float l2[2])
296 {
297         float closest[2];
298
299         closest_to_line_v2(closest, p, l1, l2);
300
301         return len_squared_v2v2(closest, p);
302 }
303 float dist_to_line_v2(const float p[2], const float l1[2], const float l2[2])
304 {
305         return sqrtf(dist_squared_to_line_v2(p, l1, l2));
306 }
307
308 /* distance p to line-piece v1-v2 */
309 float dist_squared_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
310 {
311         float closest[2];
312
313         closest_to_line_segment_v2(closest, p, l1, l2);
314
315         return len_squared_v2v2(closest, p);
316 }
317
318 float dist_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
319 {
320         return sqrtf(dist_squared_to_line_segment_v2(p, l1, l2));
321 }
322
323 /* point closest to v1 on line v2-v3 in 2D */
324 void closest_to_line_segment_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2])
325 {
326         float lambda, cp[2];
327
328         lambda = closest_to_line_v2(cp, p, l1, l2);
329
330         /* flip checks for !finite case (when segment is a point) */
331         if (!(lambda > 0.0f)) {
332                 copy_v2_v2(r_close, l1);
333         }
334         else if (!(lambda < 1.0f)) {
335                 copy_v2_v2(r_close, l2);
336         }
337         else {
338                 copy_v2_v2(r_close, cp);
339         }
340 }
341
342 /* point closest to v1 on line v2-v3 in 3D */
343 void closest_to_line_segment_v3(float r_close[3], const float p[3], const float l1[3], const float l2[3])
344 {
345         float lambda, cp[3];
346
347         lambda = closest_to_line_v3(cp, p, l1, l2);
348
349         /* flip checks for !finite case (when segment is a point) */
350         if (!(lambda > 0.0f)) {
351                 copy_v3_v3(r_close, l1);
352         }
353         else if (!(lambda < 1.0f)) {
354                 copy_v3_v3(r_close, l2);
355         }
356         else {
357                 copy_v3_v3(r_close, cp);
358         }
359 }
360
361 /**
362  * Find the closest point on a plane.
363  *
364  * \param r_close  Return coordinate
365  * \param plane  The plane to test against.
366  * \param pt  The point to find the nearest of
367  *
368  * \note non-unit-length planes are supported.
369  */
370 void closest_to_plane_v3(float r_close[3], const float plane[4], const float pt[3])
371 {
372         const float len_sq = len_squared_v3(plane);
373         const float side = plane_point_side_v3(plane, pt);
374         madd_v3_v3v3fl(r_close, pt, plane, -side / len_sq);
375 }
376
377 void closest_to_plane_normalized_v3(float r_close[3], const float plane[4], const float pt[3])
378 {
379         const float side = plane_point_side_v3(plane, pt);
380         BLI_ASSERT_UNIT_V3(plane);
381         madd_v3_v3v3fl(r_close, pt, plane, -side);
382 }
383
384 void closest_to_plane3_v3(float r_close[3], const float plane[3], const float pt[3])
385 {
386         const float len_sq = len_squared_v3(plane);
387         const float side = dot_v3v3(plane, pt);
388         madd_v3_v3v3fl(r_close, pt, plane, -side / len_sq);
389 }
390
391 void closest_to_plane3_normalized_v3(float r_close[3], const float plane[3], const float pt[3])
392 {
393         const float side = dot_v3v3(plane, pt);
394         BLI_ASSERT_UNIT_V3(plane);
395         madd_v3_v3v3fl(r_close, pt, plane, -side);
396 }
397
398 float dist_signed_squared_to_plane_v3(const float pt[3], const float plane[4])
399 {
400         const float len_sq = len_squared_v3(plane);
401         const float side = plane_point_side_v3(plane, pt);
402         const float fac = side / len_sq;
403         return copysignf(len_sq * (fac * fac), side);
404 }
405 float dist_squared_to_plane_v3(const float pt[3], const float plane[4])
406 {
407         const float len_sq = len_squared_v3(plane);
408         const float side = plane_point_side_v3(plane, pt);
409         const float fac = side / len_sq;
410         /* only difference to code above - no 'copysignf' */
411         return len_sq * (fac * fac);
412 }
413
414 float dist_signed_squared_to_plane3_v3(const float pt[3], const float plane[3])
415 {
416         const float len_sq = len_squared_v3(plane);
417         const float side = dot_v3v3(plane, pt);  /* only difference with 'plane[4]' version */
418         const float fac = side / len_sq;
419         return copysignf(len_sq * (fac * fac), side);
420 }
421 float dist_squared_to_plane3_v3(const float pt[3], const float plane[3])
422 {
423         const float len_sq = len_squared_v3(plane);
424         const float side = dot_v3v3(plane, pt);  /* only difference with 'plane[4]' version */
425         const float fac = side / len_sq;
426         /* only difference to code above - no 'copysignf' */
427         return len_sq * (fac * fac);
428 }
429
430 /**
431  * Return the signed distance from the point to the plane.
432  */
433 float dist_signed_to_plane_v3(const float pt[3], const float plane[4])
434 {
435         const float len_sq = len_squared_v3(plane);
436         const float side = plane_point_side_v3(plane, pt);
437         const float fac = side / len_sq;
438         return sqrtf(len_sq) * fac;
439 }
440 float dist_to_plane_v3(const float pt[3], const float plane[4])
441 {
442         return fabsf(dist_signed_to_plane_v3(pt, plane));
443 }
444
445 float dist_signed_to_plane3_v3(const float pt[3], const float plane[3])
446 {
447         const float len_sq = len_squared_v3(plane);
448         const float side = dot_v3v3(plane, pt);  /* only difference with 'plane[4]' version */
449         const float fac = side / len_sq;
450         return sqrtf(len_sq) * fac;
451 }
452 float dist_to_plane3_v3(const float pt[3], const float plane[3])
453 {
454         return fabsf(dist_signed_to_plane3_v3(pt, plane));
455 }
456
457 /* distance v1 to line-piece l1-l2 in 3D */
458 float dist_squared_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
459 {
460         float closest[3];
461
462         closest_to_line_segment_v3(closest, p, l1, l2);
463
464         return len_squared_v3v3(closest, p);
465 }
466
467 float dist_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
468 {
469         return sqrtf(dist_squared_to_line_segment_v3(p, l1, l2));
470 }
471
472 float dist_squared_to_line_v3(const float p[3], const float l1[3], const float l2[3])
473 {
474         float closest[3];
475
476         closest_to_line_v3(closest, p, l1, l2);
477
478         return len_squared_v3v3(closest, p);
479 }
480 float dist_to_line_v3(const float p[3], const float l1[3], const float l2[3])
481 {
482         return sqrtf(dist_squared_to_line_v3(p, l1, l2));
483 }
484
485 /**
486  * Check if \a p is inside the 2x planes defined by ``(v1, v2, v3)``
487  * where the 3x points define 2x planes.
488  *
489  * \param axis_ref used when v1,v2,v3 form a line and to check if the corner is concave/convex.
490  *
491  * \note the distance from \a v1 & \a v3 to \a v2 doesnt matter
492  * (it just defines the planes).
493  *
494  * \return the lowest squared distance to either of the planes.
495  * where ``(return < 0.0)`` is outside.
496  *
497  * <pre>
498  *            v1
499  *            +
500  *           /
501  * x - out  /  x - inside
502  *         /
503  *        +----+
504  *        v2   v3
505  *           x - also outside
506  * </pre>
507  */
508 float dist_signed_squared_to_corner_v3v3v3(
509         const float p[3],
510         const float v1[3], const float v2[3], const float v3[3],
511         const float axis_ref[3])
512 {
513         float dir_a[3], dir_b[3];
514         float plane_a[3], plane_b[3];
515         float dist_a, dist_b;
516         float axis[3];
517         float s_p_v2[3];
518         bool flip = false;
519
520         sub_v3_v3v3(dir_a, v1, v2);
521         sub_v3_v3v3(dir_b, v3, v2);
522
523         cross_v3_v3v3(axis, dir_a, dir_b);
524
525         if ((len_squared_v3(axis) < FLT_EPSILON)) {
526                 copy_v3_v3(axis, axis_ref);
527         }
528         else if (dot_v3v3(axis, axis_ref) < 0.0f) {
529                 /* concave */
530                 flip = true;
531                 negate_v3(axis);
532         }
533
534         cross_v3_v3v3(plane_a, dir_a, axis);
535         cross_v3_v3v3(plane_b, axis, dir_b);
536
537 #if 0
538         plane_from_point_normal_v3(plane_a, v2, plane_a);
539         plane_from_point_normal_v3(plane_b, v2, plane_b);
540
541         dist_a = dist_signed_squared_to_plane_v3(p, plane_a);
542         dist_b = dist_signed_squared_to_plane_v3(p, plane_b);
543 #else
544         /* calculate without the planes 4th component to avoid float precision issues */
545         sub_v3_v3v3(s_p_v2, p, v2);
546
547         dist_a = dist_signed_squared_to_plane3_v3(s_p_v2, plane_a);
548         dist_b = dist_signed_squared_to_plane3_v3(s_p_v2, plane_b);
549 #endif
550
551
552
553         if (flip) {
554                 return min_ff(dist_a, dist_b);
555         }
556         else {
557                 return max_ff(dist_a, dist_b);
558         }
559 }
560
561 /**
562  * return the distance squared of a point to a ray.
563  */
564 float dist_squared_to_ray_v3(
565         const float ray_origin[3], const float ray_direction[3],
566         const float co[3], float *r_depth)
567 {
568         float dvec[3];
569         sub_v3_v3v3(dvec, co, ray_origin);
570         *r_depth = dot_v3v3(dvec, ray_direction);
571         return len_squared_v3(dvec) - SQUARE(*r_depth);
572 }
573 /**
574  * Find the closest point in a seg to a ray and return the distance squared.
575  * \param r_point: Is the point on segment closest to ray (or to ray_origin if the ray and the segment are parallel).
576  * \param r_depth: the distance of r_point projection on ray to the ray_origin.
577  */
578 float dist_squared_ray_to_seg_v3(
579         const float ray_origin[3], const float ray_direction[3],
580         const float v0[3], const float v1[3],
581         float r_point[3], float *r_depth)
582 {
583         float a[3], t[3], n[3], lambda;
584         sub_v3_v3v3(a, v1, v0);
585         sub_v3_v3v3(t, v0, ray_origin);
586         cross_v3_v3v3(n, a, ray_direction);
587         const float nlen = len_squared_v3(n);
588
589         /* if (nlen == 0.0f) the lines are parallel,
590          * has no nearest point, only distance squared.*/
591         if (nlen == 0.0f) {
592                 /* Calculate the distance to the point v0 then */
593                 copy_v3_v3(r_point, v0);
594                 *r_depth = dot_v3v3(t, ray_direction);
595         }
596         else {
597                 float c[3], cray[3];
598                 sub_v3_v3v3(c, n, t);
599                 cross_v3_v3v3(cray, c, ray_direction);
600                 lambda = dot_v3v3(cray, n) / nlen;
601                 if (lambda <= 0) {
602                         copy_v3_v3(r_point, v0);
603
604                         *r_depth = dot_v3v3(t, ray_direction);
605                 }
606                 else if (lambda >= 1) {
607                         copy_v3_v3(r_point, v1);
608
609                         sub_v3_v3v3(t, v1, ray_origin);
610                         *r_depth = dot_v3v3(t, ray_direction);
611                 }
612                 else {
613                         madd_v3_v3v3fl(r_point, v0, a, lambda);
614
615                         sub_v3_v3v3(t, r_point, ray_origin);
616                         *r_depth = dot_v3v3(t, ray_direction);
617                 }
618         }
619         return len_squared_v3(t) - SQUARE(*r_depth);
620 }
621
622 /* Adapted from "Real-Time Collision Detection" by Christer Ericson,
623  * published by Morgan Kaufmann Publishers, copyright 2005 Elsevier Inc.
624  * 
625  * Set 'r' to the point in triangle (a, b, c) closest to point 'p' */
626 void closest_on_tri_to_point_v3(float r[3], const float p[3],
627                                 const float a[3], const float b[3], const float c[3])
628 {
629         float ab[3], ac[3], ap[3], d1, d2;
630         float bp[3], d3, d4, vc, cp[3], d5, d6, vb, va;
631         float denom, v, w;
632
633         /* Check if P in vertex region outside A */
634         sub_v3_v3v3(ab, b, a);
635         sub_v3_v3v3(ac, c, a);
636         sub_v3_v3v3(ap, p, a);
637         d1 = dot_v3v3(ab, ap);
638         d2 = dot_v3v3(ac, ap);
639         if (d1 <= 0.0f && d2 <= 0.0f) {
640                 /* barycentric coordinates (1,0,0) */
641                 copy_v3_v3(r, a);
642                 return;
643         }
644
645         /* Check if P in vertex region outside B */
646         sub_v3_v3v3(bp, p, b);
647         d3 = dot_v3v3(ab, bp);
648         d4 = dot_v3v3(ac, bp);
649         if (d3 >= 0.0f && d4 <= d3) {
650                 /* barycentric coordinates (0,1,0) */
651                 copy_v3_v3(r, b);
652                 return;
653         }
654         /* Check if P in edge region of AB, if so return projection of P onto AB */
655         vc = d1 * d4 - d3 * d2;
656         if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
657                 v = d1 / (d1 - d3);
658                 /* barycentric coordinates (1-v,v,0) */
659                 madd_v3_v3v3fl(r, a, ab, v);
660                 return;
661         }
662         /* Check if P in vertex region outside C */
663         sub_v3_v3v3(cp, p, c);
664         d5 = dot_v3v3(ab, cp);
665         d6 = dot_v3v3(ac, cp);
666         if (d6 >= 0.0f && d5 <= d6) {
667                 /* barycentric coordinates (0,0,1) */
668                 copy_v3_v3(r, c);
669                 return;
670         }
671         /* Check if P in edge region of AC, if so return projection of P onto AC */
672         vb = d5 * d2 - d1 * d6;
673         if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
674                 w = d2 / (d2 - d6);
675                 /* barycentric coordinates (1-w,0,w) */
676                 madd_v3_v3v3fl(r, a, ac, w);
677                 return;
678         }
679         /* Check if P in edge region of BC, if so return projection of P onto BC */
680         va = d3 * d6 - d5 * d4;
681         if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
682                 w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
683                 /* barycentric coordinates (0,1-w,w) */
684                 sub_v3_v3v3(r, c, b);
685                 mul_v3_fl(r, w);
686                 add_v3_v3(r, b);
687                 return;
688         }
689
690         /* P inside face region. Compute Q through its barycentric coordinates (u,v,w) */
691         denom = 1.0f / (va + vb + vc);
692         v = vb * denom;
693         w = vc * denom;
694
695         /* = u*a + v*b + w*c, u = va * denom = 1.0f - v - w */
696         /* ac * w */
697         mul_v3_fl(ac, w);
698         /* a + ab * v */
699         madd_v3_v3v3fl(r, a, ab, v);
700         /* a + ab * v + ac * w */
701         add_v3_v3(r, ac);
702 }
703
704 /******************************* Intersection ********************************/
705
706 /* intersect Line-Line, shorts */
707 int isect_seg_seg_v2_int(const int v1[2], const int v2[2], const int v3[2], const int v4[2])
708 {
709         float div, lambda, mu;
710
711         div = (float)((v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]));
712         if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR;
713
714         lambda = (float)((v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
715
716         mu = (float)((v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
717
718         if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
719                 if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT;
720                 return ISECT_LINE_LINE_CROSS;
721         }
722         return ISECT_LINE_LINE_NONE;
723 }
724
725 /* intersect Line-Line, floats - gives intersection point */
726 int isect_line_line_v2_point(const float v0[2], const float v1[2], const float v2[2], const float v3[2], float r_vi[2])
727 {
728         float s10[2], s32[2];
729         float div;
730
731         sub_v2_v2v2(s10, v1, v0);
732         sub_v2_v2v2(s32, v3, v2);
733
734         div = cross_v2v2(s10, s32);
735         if (div != 0.0f) {
736                 const float u = cross_v2v2(v1, v0);
737                 const float v = cross_v2v2(v3, v2);
738
739                 r_vi[0] = ((s32[0] * u) - (s10[0] * v)) / div;
740                 r_vi[1] = ((s32[1] * u) - (s10[1] * v)) / div;
741
742                 return ISECT_LINE_LINE_CROSS;
743         }
744         else {
745                 return ISECT_LINE_LINE_COLINEAR;
746         }
747 }
748
749 /* intersect Line-Line, floats */
750 int isect_seg_seg_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
751 {
752         float div, lambda, mu;
753
754         div = (v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]);
755         if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR;
756
757         lambda = ((float)(v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
758
759         mu = ((float)(v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
760
761         if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
762                 if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT;
763                 return ISECT_LINE_LINE_CROSS;
764         }
765         return ISECT_LINE_LINE_NONE;
766 }
767
768 /* get intersection point of two 2D segments and return intersection type:
769  *  -1: collinear
770  *   1: intersection
771  */
772 int isect_seg_seg_v2_point(
773         const float v0[2], const float v1[2],
774         const float v2[2], const float v3[2],
775         float r_vi[2])
776 {
777         float s10[2], s32[2], s30[2], d;
778         const float eps = 1e-6f;
779         const float eps_sq = eps * eps;
780
781         sub_v2_v2v2(s10, v1, v0);
782         sub_v2_v2v2(s32, v3, v2);
783         sub_v2_v2v2(s30, v3, v0);
784
785         d = cross_v2v2(s10, s32);
786
787         if (d != 0) {
788                 float u, v;
789
790                 u = cross_v2v2(s30, s32) / d;
791                 v = cross_v2v2(s10, s30) / d;
792
793                 if ((u >= -eps && u <= 1.0f + eps) &&
794                     (v >= -eps && v <= 1.0f + eps))
795                 {
796                         /* intersection */
797                         float vi_test[2];
798                         float s_vi_v2[2];
799
800                         madd_v2_v2v2fl(vi_test, v0, s10, u);
801
802                         /* When 'd' approaches zero, float precision lets non-overlapping co-linear segments
803                          * detect as an intersection. So re-calculate 'v' to ensure the point overlaps both.
804                          * see T45123 */
805
806                         /* inline since we have most vars already */
807 #if 0
808                         v = line_point_factor_v2(ix_test, v2, v3);
809 #else
810                         sub_v2_v2v2(s_vi_v2, vi_test, v2);
811                         v = (dot_v2v2(s32, s_vi_v2) / dot_v2v2(s32, s32));
812 #endif
813                         if (v >= -eps && v <= 1.0f + eps) {
814                                 copy_v2_v2(r_vi, vi_test);
815                                 return 1;
816                         }
817                 }
818
819                 /* out of segment intersection */
820                 return -1;
821         }
822         else {
823                 if ((cross_v2v2(s10, s30) == 0.0f) &&
824                     (cross_v2v2(s32, s30) == 0.0f))
825                 {
826                         /* equal lines */
827                         float s20[2];
828                         float u_a, u_b;
829
830                         if (equals_v2v2(v0, v1)) {
831                                 if (len_squared_v2v2(v2, v3) > eps_sq) {
832                                         /* use non-point segment as basis */
833                                         SWAP(const float *, v0, v2);
834                                         SWAP(const float *, v1, v3);
835
836                                         sub_v2_v2v2(s10, v1, v0);
837                                         sub_v2_v2v2(s30, v3, v0);
838                                 }
839                                 else { /* both of segments are points */
840                                         if (equals_v2v2(v0, v2)) { /* points are equal */
841                                                 copy_v2_v2(r_vi, v0);
842                                                 return 1;
843                                         }
844
845                                         /* two different points */
846                                         return -1;
847                                 }
848                         }
849
850                         sub_v2_v2v2(s20, v2, v0);
851
852                         u_a = dot_v2v2(s20, s10) / dot_v2v2(s10, s10);
853                         u_b = dot_v2v2(s30, s10) / dot_v2v2(s10, s10);
854
855                         if (u_a > u_b)
856                                 SWAP(float, u_a, u_b);
857
858                         if (u_a > 1.0f + eps || u_b < -eps) {
859                                 /* non-overlapping segments */
860                                 return -1;
861                         }
862                         else if (max_ff(0.0f, u_a) == min_ff(1.0f, u_b)) {
863                                 /* one common point: can return result */
864                                 madd_v2_v2v2fl(r_vi, v0, s10, max_ff(0, u_a));
865                                 return 1;
866                         }
867                 }
868
869                 /* lines are collinear */
870                 return -1;
871         }
872 }
873
874 bool isect_seg_seg_v2_simple(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
875 {
876 #define CCW(A, B, C) \
877         ((C[1] - A[1]) * (B[0] - A[0]) > \
878          (B[1] - A[1]) * (C[0] - A[0]))
879
880         return CCW(v1, v3, v4) != CCW(v2, v3, v4) && CCW(v1, v2, v3) != CCW(v1, v2, v4);
881
882 #undef CCW
883 }
884
885 /**
886  * \param l1, l2: Coordinates (point of line).
887  * \param sp, r:  Coordinate and radius (sphere).
888  * \return r_p1, r_p2: Intersection coordinates.
889  *
890  * \note The order of assignment for intersection points (\a r_p1, \a r_p2) is predictable,
891  * based on the direction defined by ``l2 - l1``,
892  * this direction compared with the normal of each point on the sphere:
893  * \a r_p1 always has a >= 0.0 dot product.
894  * \a r_p2 always has a <= 0.0 dot product.
895  * For example, when \a l1 is inside the sphere and \a l2 is outside,
896  * \a r_p1 will always be between \a l1 and \a l2.
897  */
898 int isect_line_sphere_v3(const float l1[3], const float l2[3],
899                          const float sp[3], const float r,
900                          float r_p1[3], float r_p2[3])
901 {
902         /* adapted for use in blender by Campbell Barton - 2011
903          *
904          * atelier iebele abel - 2001
905          * atelier@iebele.nl
906          * http://www.iebele.nl
907          *
908          * sphere_line_intersection function adapted from:
909          * http://astronomy.swin.edu.au/pbourke/geometry/sphereline
910          * Paul Bourke pbourke@swin.edu.au
911          */
912
913         const float ldir[3] = {
914                 l2[0] - l1[0],
915                 l2[1] - l1[1],
916                 l2[2] - l1[2]
917         };
918
919         const float a = len_squared_v3(ldir);
920
921         const float b = 2.0f *
922                         (ldir[0] * (l1[0] - sp[0]) +
923                          ldir[1] * (l1[1] - sp[1]) +
924                          ldir[2] * (l1[2] - sp[2]));
925
926         const float c =
927             len_squared_v3(sp) +
928             len_squared_v3(l1) -
929             (2.0f * dot_v3v3(sp, l1)) -
930             (r * r);
931
932         const float i = b * b - 4.0f * a * c;
933
934         float mu;
935
936         if (i < 0.0f) {
937                 /* no intersections */
938                 return 0;
939         }
940         else if (i == 0.0f) {
941                 /* one intersection */
942                 mu = -b / (2.0f * a);
943                 madd_v3_v3v3fl(r_p1, l1, ldir, mu);
944                 return 1;
945         }
946         else if (i > 0.0f) {
947                 const float i_sqrt = sqrtf(i);  /* avoid calc twice */
948
949                 /* first intersection */
950                 mu = (-b + i_sqrt) / (2.0f * a);
951                 madd_v3_v3v3fl(r_p1, l1, ldir, mu);
952
953                 /* second intersection */
954                 mu = (-b - i_sqrt) / (2.0f * a);
955                 madd_v3_v3v3fl(r_p2, l1, ldir, mu);
956                 return 2;
957         }
958         else {
959                 /* math domain error - nan */
960                 return -1;
961         }
962 }
963
964 /* keep in sync with isect_line_sphere_v3 */
965 int isect_line_sphere_v2(const float l1[2], const float l2[2],
966                          const float sp[2], const float r,
967                          float r_p1[2], float r_p2[2])
968 {
969         const float ldir[2] = {l2[0] - l1[0],
970                                l2[1] - l1[1]};
971
972         const float a = dot_v2v2(ldir, ldir);
973
974         const float b = 2.0f *
975                         (ldir[0] * (l1[0] - sp[0]) +
976                          ldir[1] * (l1[1] - sp[1]));
977
978         const float c =
979             dot_v2v2(sp, sp) +
980             dot_v2v2(l1, l1) -
981             (2.0f * dot_v2v2(sp, l1)) -
982             (r * r);
983
984         const float i = b * b - 4.0f * a * c;
985
986         float mu;
987
988         if (i < 0.0f) {
989                 /* no intersections */
990                 return 0;
991         }
992         else if (i == 0.0f) {
993                 /* one intersection */
994                 mu = -b / (2.0f * a);
995                 madd_v2_v2v2fl(r_p1, l1, ldir, mu);
996                 return 1;
997         }
998         else if (i > 0.0f) {
999                 const float i_sqrt = sqrtf(i);  /* avoid calc twice */
1000
1001                 /* first intersection */
1002                 mu = (-b + i_sqrt) / (2.0f * a);
1003                 madd_v2_v2v2fl(r_p1, l1, ldir, mu);
1004
1005                 /* second intersection */
1006                 mu = (-b - i_sqrt) / (2.0f * a);
1007                 madd_v2_v2v2fl(r_p2, l1, ldir, mu);
1008                 return 2;
1009         }
1010         else {
1011                 /* math domain error - nan */
1012                 return -1;
1013         }
1014 }
1015
1016 /* point in polygon (keep float and int versions in sync) */
1017 #if 0
1018 bool isect_point_poly_v2(const float pt[2], const float verts[][2], const unsigned int nr,
1019                          const bool use_holes)
1020 {
1021         /* we do the angle rule, define that all added angles should be about zero or (2 * PI) */
1022         float angletot = 0.0;
1023         float fp1[2], fp2[2];
1024         unsigned int i;
1025         const float *p1, *p2;
1026
1027         p1 = verts[nr - 1];
1028
1029         /* first vector */
1030         fp1[0] = (float)(p1[0] - pt[0]);
1031         fp1[1] = (float)(p1[1] - pt[1]);
1032
1033         for (i = 0; i < nr; i++) {
1034                 p2 = verts[i];
1035
1036                 /* second vector */
1037                 fp2[0] = (float)(p2[0] - pt[0]);
1038                 fp2[1] = (float)(p2[1] - pt[1]);
1039
1040                 /* dot and angle and cross */
1041                 angletot += angle_signed_v2v2(fp1, fp2);
1042
1043                 /* circulate */
1044                 copy_v2_v2(fp1, fp2);
1045                 p1 = p2;
1046         }
1047
1048         angletot = fabsf(angletot);
1049         if (use_holes) {
1050                 const float nested = floorf((angletot / (float)(M_PI * 2.0)) + 0.00001f);
1051                 angletot -= nested * (float)(M_PI * 2.0);
1052                 return (angletot > 4.0f) != ((int)nested % 2);
1053         }
1054         else {
1055                 return (angletot > 4.0f);
1056         }
1057 }
1058 bool isect_point_poly_v2_int(const int pt[2], const int verts[][2], const unsigned int nr,
1059                              const bool use_holes)
1060 {
1061         /* we do the angle rule, define that all added angles should be about zero or (2 * PI) */
1062         float angletot = 0.0;
1063         float fp1[2], fp2[2];
1064         unsigned int i;
1065         const int *p1, *p2;
1066
1067         p1 = verts[nr - 1];
1068
1069         /* first vector */
1070         fp1[0] = (float)(p1[0] - pt[0]);
1071         fp1[1] = (float)(p1[1] - pt[1]);
1072
1073         for (i = 0; i < nr; i++) {
1074                 p2 = verts[i];
1075
1076                 /* second vector */
1077                 fp2[0] = (float)(p2[0] - pt[0]);
1078                 fp2[1] = (float)(p2[1] - pt[1]);
1079
1080                 /* dot and angle and cross */
1081                 angletot += angle_signed_v2v2(fp1, fp2);
1082
1083                 /* circulate */
1084                 copy_v2_v2(fp1, fp2);
1085                 p1 = p2;
1086         }
1087
1088         angletot = fabsf(angletot);
1089         if (use_holes) {
1090                 const float nested = floorf((angletot / (float)(M_PI * 2.0)) + 0.00001f);
1091                 angletot -= nested * (float)(M_PI * 2.0);
1092                 return (angletot > 4.0f) != ((int)nested % 2);
1093         }
1094         else {
1095                 return (angletot > 4.0f);
1096         }
1097 }
1098
1099 #else
1100
1101 bool isect_point_poly_v2(const float pt[2], const float verts[][2], const unsigned int nr,
1102                          const bool UNUSED(use_holes))
1103 {
1104         unsigned int i, j;
1105         bool isect = false;
1106         for (i = 0, j = nr - 1; i < nr; j = i++) {
1107                 if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
1108                     (pt[0] < (verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) + verts[i][0]))
1109                 {
1110                         isect = !isect;
1111                 }
1112         }
1113         return isect;
1114 }
1115 bool isect_point_poly_v2_int(const int pt[2], const int verts[][2], const unsigned int nr,
1116                              const bool UNUSED(use_holes))
1117 {
1118         unsigned int i, j;
1119         bool isect = false;
1120         for (i = 0, j = nr - 1; i < nr; j = i++) {
1121                 if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
1122                     (pt[0] < (verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) + verts[i][0]))
1123                 {
1124                         isect = !isect;
1125                 }
1126         }
1127         return isect;
1128 }
1129
1130 #endif
1131
1132 /* point in tri */
1133
1134 /* only single direction */
1135 bool isect_point_tri_v2_cw(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
1136 {
1137         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
1138                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
1139                         if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
1140                                 return true;
1141                         }
1142                 }
1143         }
1144
1145         return false;
1146 }
1147
1148 int isect_point_tri_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
1149 {
1150         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
1151                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
1152                         if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
1153                                 return 1;
1154                         }
1155                 }
1156         }
1157         else {
1158                 if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
1159                         if (!(line_point_side_v2(v3, v1, pt) >= 0.0f)) {
1160                                 return -1;
1161                         }
1162                 }
1163         }
1164
1165         return 0;
1166 }
1167
1168 /* point in quad - only convex quads */
1169 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])
1170 {
1171         if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
1172                 if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
1173                         if (line_point_side_v2(v3, v4, pt) >= 0.0f) {
1174                                 if (line_point_side_v2(v4, v1, pt) >= 0.0f) {
1175                                         return 1;
1176                                 }
1177                         }
1178                 }
1179         }
1180         else {
1181                 if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
1182                         if (!(line_point_side_v2(v3, v4, pt) >= 0.0f)) {
1183                                 if (!(line_point_side_v2(v4, v1, pt) >= 0.0f)) {
1184                                         return -1;
1185                                 }
1186                         }
1187                 }
1188         }
1189
1190         return 0;
1191 }
1192
1193 /* moved from effect.c
1194  * test if the line starting at p1 ending at p2 intersects the triangle v0..v2
1195  * return non zero if it does
1196  */
1197 bool isect_line_segment_tri_v3(
1198         const float p1[3], const float p2[3],
1199         const float v0[3], const float v1[3], const float v2[3],
1200         float *r_lambda, float r_uv[2])
1201 {
1202
1203         float p[3], s[3], d[3], e1[3], e2[3], q[3];
1204         float a, f, u, v;
1205
1206         sub_v3_v3v3(e1, v1, v0);
1207         sub_v3_v3v3(e2, v2, v0);
1208         sub_v3_v3v3(d, p2, p1);
1209
1210         cross_v3_v3v3(p, d, e2);
1211         a = dot_v3v3(e1, p);
1212         if (a == 0.0f) return false;
1213         f = 1.0f / a;
1214
1215         sub_v3_v3v3(s, p1, v0);
1216
1217         u = f * dot_v3v3(s, p);
1218         if ((u < 0.0f) || (u > 1.0f)) return false;
1219
1220         cross_v3_v3v3(q, s, e1);
1221
1222         v = f * dot_v3v3(d, q);
1223         if ((v < 0.0f) || ((u + v) > 1.0f)) return false;
1224
1225         *r_lambda = f * dot_v3v3(e2, q);
1226         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return false;
1227
1228         if (r_uv) {
1229                 r_uv[0] = u;
1230                 r_uv[1] = v;
1231         }
1232
1233         return true;
1234 }
1235
1236 /* like isect_line_segment_tri_v3, but allows epsilon tolerance around triangle */
1237 bool isect_line_segment_tri_epsilon_v3(
1238         const float p1[3], const float p2[3],
1239         const float v0[3], const float v1[3], const float v2[3],
1240         float *r_lambda, float r_uv[2], const float epsilon)
1241 {
1242
1243         float p[3], s[3], d[3], e1[3], e2[3], q[3];
1244         float a, f, u, v;
1245
1246         sub_v3_v3v3(e1, v1, v0);
1247         sub_v3_v3v3(e2, v2, v0);
1248         sub_v3_v3v3(d, p2, p1);
1249
1250         cross_v3_v3v3(p, d, e2);
1251         a = dot_v3v3(e1, p);
1252         if (a == 0.0f) return false;
1253         f = 1.0f / a;
1254
1255         sub_v3_v3v3(s, p1, v0);
1256
1257         u = f * dot_v3v3(s, p);
1258         if ((u < -epsilon) || (u > 1.0f + epsilon)) return false;
1259
1260         cross_v3_v3v3(q, s, e1);
1261
1262         v = f * dot_v3v3(d, q);
1263         if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) return false;
1264
1265         *r_lambda = f * dot_v3v3(e2, q);
1266         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return false;
1267
1268         if (r_uv) {
1269                 r_uv[0] = u;
1270                 r_uv[1] = v;
1271         }
1272
1273         return true;
1274 }
1275
1276 /* moved from effect.c
1277  * test if the ray starting at p1 going in d direction intersects the triangle v0..v2
1278  * return non zero if it does
1279  */
1280 bool isect_ray_tri_v3(
1281         const float ray_origin[3], const float ray_direction[3],
1282         const float v0[3], const float v1[3], const float v2[3],
1283         float *r_lambda, float r_uv[2])
1284 {
1285         /* note: these values were 0.000001 in 2.4x but for projection snapping on
1286          * a human head (1BU == 1m), subsurf level 2, this gave many errors - campbell */
1287         const float epsilon = 0.00000001f;
1288         float p[3], s[3], e1[3], e2[3], q[3];
1289         float a, f, u, v;
1290
1291         sub_v3_v3v3(e1, v1, v0);
1292         sub_v3_v3v3(e2, v2, v0);
1293
1294         cross_v3_v3v3(p, ray_direction, e2);
1295         a = dot_v3v3(e1, p);
1296         if ((a > -epsilon) && (a < epsilon)) return false;
1297         f = 1.0f / a;
1298
1299         sub_v3_v3v3(s, ray_origin, v0);
1300
1301         u = f * dot_v3v3(s, p);
1302         if ((u < 0.0f) || (u > 1.0f)) return false;
1303
1304         cross_v3_v3v3(q, s, e1);
1305
1306         v = f * dot_v3v3(ray_direction, q);
1307         if ((v < 0.0f) || ((u + v) > 1.0f)) return false;
1308
1309         *r_lambda = f * dot_v3v3(e2, q);
1310         if ((*r_lambda < 0.0f)) return false;
1311
1312         if (r_uv) {
1313                 r_uv[0] = u;
1314                 r_uv[1] = v;
1315         }
1316
1317         return true;
1318 }
1319
1320 /**
1321  * if clip is nonzero, will only return true if lambda is >= 0.0
1322  * (i.e. intersection point is along positive \a ray_direction)
1323  *
1324  * \note #line_plane_factor_v3() shares logic.
1325  */
1326 bool isect_ray_plane_v3(
1327         const float ray_origin[3], const float ray_direction[3],
1328         const float plane[4],
1329         float *r_lambda, const bool clip)
1330 {
1331         float h[3], plane_co[3];
1332         float dot;
1333
1334         dot = dot_v3v3(plane, ray_direction);
1335         if (dot == 0.0f) {
1336                 return false;
1337         }
1338         mul_v3_v3fl(plane_co, plane, (-plane[3] / len_squared_v3(plane)));
1339         sub_v3_v3v3(h, ray_origin, plane_co);
1340         *r_lambda = -dot_v3v3(plane, h) / dot;
1341         if (clip && (*r_lambda < 0.0f)) {
1342                 return false;
1343         }
1344         return true;
1345 }
1346
1347 bool isect_ray_tri_epsilon_v3(
1348         const float ray_origin[3], const float ray_direction[3],
1349         const float v0[3], const float v1[3], const float v2[3],
1350         float *r_lambda, float r_uv[2], const float epsilon)
1351 {
1352         float p[3], s[3], e1[3], e2[3], q[3];
1353         float a, f, u, v;
1354
1355         sub_v3_v3v3(e1, v1, v0);
1356         sub_v3_v3v3(e2, v2, v0);
1357
1358         cross_v3_v3v3(p, ray_direction, e2);
1359         a = dot_v3v3(e1, p);
1360         if (a == 0.0f) return false;
1361         f = 1.0f / a;
1362
1363         sub_v3_v3v3(s, ray_origin, v0);
1364
1365         u = f * dot_v3v3(s, p);
1366         if ((u < -epsilon) || (u > 1.0f + epsilon)) return false;
1367
1368         cross_v3_v3v3(q, s, e1);
1369
1370         v = f * dot_v3v3(ray_direction, q);
1371         if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) return false;
1372
1373         *r_lambda = f * dot_v3v3(e2, q);
1374         if ((*r_lambda < 0.0f)) return false;
1375
1376         if (r_uv) {
1377                 r_uv[0] = u;
1378                 r_uv[1] = v;
1379         }
1380
1381         return true;
1382 }
1383
1384 void isect_ray_tri_watertight_v3_precalc(struct IsectRayPrecalc *isect_precalc, const float ray_direction[3])
1385 {
1386         float inv_dir_z;
1387
1388         /* Calculate dimension where the ray direction is maximal. */
1389         int kz = axis_dominant_v3_single(ray_direction);
1390         int kx = (kz != 2) ? (kz + 1) : 0;
1391         int ky = (kx != 2) ? (kx + 1) : 0;
1392
1393         /* Swap kx and ky dimensions to preserve winding direction of triangles. */
1394         if (ray_direction[kz] < 0.0f) {
1395                 SWAP(int, kx, ky);
1396         }
1397
1398         /* Calculate the shear constants. */
1399         inv_dir_z = 1.0f / ray_direction[kz];
1400         isect_precalc->sx = ray_direction[kx] * inv_dir_z;
1401         isect_precalc->sy = ray_direction[ky] * inv_dir_z;
1402         isect_precalc->sz = inv_dir_z;
1403
1404         /* Store the dimensions. */
1405         isect_precalc->kx = kx;
1406         isect_precalc->ky = ky;
1407         isect_precalc->kz = kz;
1408 }
1409
1410 bool isect_ray_tri_watertight_v3(
1411         const float ray_origin[3], const struct IsectRayPrecalc *isect_precalc,
1412         const float v0[3], const float v1[3], const float v2[3],
1413         float *r_lambda, float r_uv[2])
1414 {
1415         const int kx = isect_precalc->kx;
1416         const int ky = isect_precalc->ky;
1417         const int kz = isect_precalc->kz;
1418         const float sx = isect_precalc->sx;
1419         const float sy = isect_precalc->sy;
1420         const float sz = isect_precalc->sz;
1421
1422         /* Calculate vertices relative to ray origin. */
1423         const float a[3] = {v0[0] - ray_origin[0], v0[1] - ray_origin[1], v0[2] - ray_origin[2]};
1424         const float b[3] = {v1[0] - ray_origin[0], v1[1] - ray_origin[1], v1[2] - ray_origin[2]};
1425         const float c[3] = {v2[0] - ray_origin[0], v2[1] - ray_origin[1], v2[2] - ray_origin[2]};
1426
1427         const float a_kx = a[kx], a_ky = a[ky], a_kz = a[kz];
1428         const float b_kx = b[kx], b_ky = b[ky], b_kz = b[kz];
1429         const float c_kx = c[kx], c_ky = c[ky], c_kz = c[kz];
1430
1431         /* Perform shear and scale of vertices. */
1432         const float ax = a_kx - sx * a_kz;
1433         const float ay = a_ky - sy * a_kz;
1434         const float bx = b_kx - sx * b_kz;
1435         const float by = b_ky - sy * b_kz;
1436         const float cx = c_kx - sx * c_kz;
1437         const float cy = c_ky - sy * c_kz;
1438
1439         /* Calculate scaled barycentric coordinates. */
1440         const float u = cx * by - cy * bx;
1441         const float v = ax * cy - ay * cx;
1442         const float w = bx * ay - by * ax;
1443         float det;
1444
1445         if ((u < 0.0f || v < 0.0f || w < 0.0f) &&
1446             (u > 0.0f || v > 0.0f || w > 0.0f))
1447         {
1448                 return false;
1449         }
1450
1451         /* Calculate determinant. */
1452         det = u + v + w;
1453         if (UNLIKELY(det == 0.0f)) {
1454                 return false;
1455         }
1456         else {
1457                 /* Calculate scaled z-coordinates of vertices and use them to calculate
1458                  * the hit distance.
1459                  */
1460                 const int sign_det = (float_as_int(det) & (int)0x80000000);
1461                 const float t = (u * a_kz + v * b_kz + w * c_kz) * sz;
1462                 const float sign_t = xor_fl(t, sign_det);
1463                 if ((sign_t < 0.0f)
1464                     /* differ from Cycles, don't read r_lambda's original value
1465                      * otherwise we won't match any of the other intersect functions here...
1466                      * which would be confusing */
1467 #if 0
1468                     ||
1469                     (sign_T > *r_lambda * xor_signmask(det, sign_mask))
1470 #endif
1471                     )
1472                 {
1473                         return false;
1474                 }
1475                 else {
1476                         /* Normalize u, v and t. */
1477                         const float inv_det = 1.0f / det;
1478                         if (r_uv) {
1479                                 r_uv[0] = u * inv_det;
1480                                 r_uv[1] = v * inv_det;
1481                         }
1482                         *r_lambda = t * inv_det;
1483                         return true;
1484                 }
1485         }
1486 }
1487
1488 bool isect_ray_tri_watertight_v3_simple(
1489         const float ray_origin[3], const float ray_direction[3],
1490         const float v0[3], const float v1[3], const float v2[3],
1491         float *r_lambda, float r_uv[2])
1492 {
1493         struct IsectRayPrecalc isect_precalc;
1494         isect_ray_tri_watertight_v3_precalc(&isect_precalc, ray_direction);
1495         return isect_ray_tri_watertight_v3(ray_origin, &isect_precalc, v0, v1, v2, r_lambda, r_uv);
1496 }
1497
1498 #if 0  /* UNUSED */
1499 /**
1500  * A version of #isect_ray_tri_v3 which takes a threshold argument
1501  * so rays slightly outside the triangle to be considered as intersecting.
1502  */
1503 bool isect_ray_tri_threshold_v3(
1504         const float ray_origin[3], const float ray_direction[3],
1505         const float v0[3], const float v1[3], const float v2[3],
1506         float *r_lambda, float r_uv[2], const float threshold)
1507 {
1508         const float epsilon = 0.00000001f;
1509         float p[3], s[3], e1[3], e2[3], q[3];
1510         float a, f, u, v;
1511         float du, dv;
1512
1513         sub_v3_v3v3(e1, v1, v0);
1514         sub_v3_v3v3(e2, v2, v0);
1515
1516         cross_v3_v3v3(p, ray_direction, e2);
1517         a = dot_v3v3(e1, p);
1518         if ((a > -epsilon) && (a < epsilon)) return false;
1519         f = 1.0f / a;
1520
1521         sub_v3_v3v3(s, ray_origin, v0);
1522
1523         cross_v3_v3v3(q, s, e1);
1524         *r_lambda = f * dot_v3v3(e2, q);
1525         if ((*r_lambda < 0.0f)) return false;
1526
1527         u = f * dot_v3v3(s, p);
1528         v = f * dot_v3v3(ray_direction, q);
1529
1530         if (u > 0 && v > 0 && u + v > 1) {
1531                 float t = (u + v - 1) / 2;
1532                 du = u - t;
1533                 dv = v - t;
1534         }
1535         else {
1536                 if      (u < 0) du = u;
1537                 else if (u > 1) du = u - 1;
1538                 else            du = 0.0f;
1539
1540                 if      (v < 0) dv = v;
1541                 else if (v > 1) dv = v - 1;
1542                 else            dv = 0.0f;
1543         }
1544
1545         mul_v3_fl(e1, du);
1546         mul_v3_fl(e2, dv);
1547
1548         if (len_squared_v3(e1) + len_squared_v3(e2) > threshold * threshold) {
1549                 return false;
1550         }
1551
1552         if (r_uv) {
1553                 r_uv[0] = u;
1554                 r_uv[1] = v;
1555         }
1556
1557         return true;
1558 }
1559 #endif
1560
1561
1562 bool isect_ray_seg_v2(
1563         const float ray_origin[2], const float ray_direction[2],
1564         const float v0[2], const float v1[2],
1565         float *r_lambda, float *r_u)
1566 {
1567         float v0_local[2], v1_local[2];
1568         sub_v2_v2v2(v0_local, v0, ray_origin);
1569         sub_v2_v2v2(v1_local, v1, ray_origin);
1570
1571         float s10[2];
1572         float det;
1573
1574         sub_v2_v2v2(s10, v1_local, v0_local);
1575
1576         det = cross_v2v2(ray_direction, s10);
1577         if (det != 0.0f) {
1578                 const float v = cross_v2v2(v0_local, v1_local);
1579                 float p[2] = {(ray_direction[0] * v) / det, (ray_direction[1] * v) / det};
1580
1581                 const float t = (dot_v2v2(p, ray_direction) / dot_v2v2(ray_direction, ray_direction));
1582                 if ((t >= 0.0f) == 0) {
1583                         return false;
1584                 }
1585
1586                 float h[2];
1587                 sub_v2_v2v2(h, v1_local, p);
1588                 const float u = (dot_v2v2(s10, h) / dot_v2v2(s10, s10));
1589                 if ((u >= 0.0f && u <= 1.0f) == 0) {
1590                         return false;
1591                 }
1592
1593                 if (r_lambda) {
1594                         *r_lambda = t;
1595                 }
1596                 if (r_u) {
1597                         *r_u = u;
1598                 }
1599
1600                 return true;
1601         }
1602
1603         return false;
1604 }
1605
1606 /**
1607  * Check if a point is behind all planes.
1608  */
1609 bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3])
1610 {
1611         int i;
1612
1613         for (i = 0; i < totplane; i++) {
1614                 if (plane_point_side_v3(planes[i], p) > 0.0f) {
1615                         return false;
1616                 }
1617         }
1618
1619         return true;
1620 }
1621
1622 /**
1623  * Intersect line/plane.
1624  *
1625  * \param r_isect_co The intersection point.
1626  * \param l1 The first point of the line.
1627  * \param l2 The second point of the line.
1628  * \param plane_co A point on the plane to intersect with.
1629  * \param plane_no The direction of the plane (does not need to be normalized).
1630  *
1631  * \note #line_plane_factor_v3() shares logic.
1632  */
1633 bool isect_line_plane_v3(
1634         float r_isect_co[3],
1635         const float l1[3], const float l2[3],
1636         const float plane_co[3], const float plane_no[3])
1637 {
1638         float u[3], h[3];
1639         float dot;
1640
1641         sub_v3_v3v3(u, l2, l1);
1642         sub_v3_v3v3(h, l1, plane_co);
1643         dot = dot_v3v3(plane_no, u);
1644
1645         if (fabsf(dot) > FLT_EPSILON) {
1646                 float lambda = -dot_v3v3(plane_no, h) / dot;
1647                 madd_v3_v3v3fl(r_isect_co, l1, u, lambda);
1648                 return true;
1649         }
1650         else {
1651                 /* The segment is parallel to plane */
1652                 return false;
1653         }
1654 }
1655
1656 /**
1657  * Intersect three planes, return the point where all 3 meet.
1658  * See Graphics Gems 1 pg 305
1659  *
1660  * \param plane_a, plane_b, plane_c: Planes.
1661  * \param r_isect_co: The resulting intersection point.
1662  */
1663 bool isect_plane_plane_plane_v3(
1664         const float plane_a[4], const float plane_b[4], const float plane_c[4],
1665         float r_isect_co[3])
1666 {
1667         float det;
1668
1669         det = determinant_m3(UNPACK3(plane_a), UNPACK3(plane_b), UNPACK3(plane_c));
1670
1671         if (det != 0.0f) {
1672                 float tmp[3];
1673
1674                 /* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
1675                  *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3] +
1676                  *  plane_a.xyz.cross(plane_b.xyz) * -plane_c[3]) / det; */
1677
1678                 cross_v3_v3v3(tmp, plane_c, plane_b);
1679                 mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
1680
1681                 cross_v3_v3v3(tmp, plane_a, plane_c);
1682                 madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
1683
1684                 cross_v3_v3v3(tmp, plane_b, plane_a);
1685                 madd_v3_v3fl(r_isect_co, tmp, plane_c[3]);
1686
1687                 mul_v3_fl(r_isect_co, 1.0f / det);
1688
1689                 return true;
1690         }
1691         else {
1692                 return false;
1693         }
1694 }
1695
1696 /**
1697  * Intersect two planes, return a point on the intersection and a vector
1698  * that runs on the direction of the intersection.
1699  *
1700  *
1701  * \note this is a slightly reduced version of #isect_plane_plane_plane_v3
1702  *
1703  * \param plane_a, plane_b: Planes.
1704  * \param r_isect_co: The resulting intersection point.
1705  * \param r_isect_no: The resulting vector of the intersection.
1706  *
1707  * \note \a r_isect_no isn't unit length.
1708  */
1709 bool isect_plane_plane_v3(
1710         const float plane_a[4], const float plane_b[4],
1711         float r_isect_co[3], float r_isect_no[3])
1712 {
1713         float det, plane_c[3];
1714
1715         /* direction is simply the cross product */
1716         cross_v3_v3v3(plane_c, plane_a, plane_b);
1717
1718         /* in this case we don't need to use 'determinant_m3' */
1719         det = len_squared_v3(plane_c);
1720
1721         if (det != 0.0f) {
1722                 float tmp[3];
1723
1724                 /* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
1725                  *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3]) / det; */
1726                 cross_v3_v3v3(tmp, plane_c, plane_b);
1727                 mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
1728
1729                 cross_v3_v3v3(tmp, plane_a, plane_c);
1730                 madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
1731
1732                 mul_v3_fl(r_isect_co, 1.0f / det);
1733
1734                 copy_v3_v3(r_isect_no, plane_c);
1735
1736                 return true;
1737         }
1738         else {
1739                 return false;
1740         }
1741 }
1742
1743 /**
1744  * Intersect two triangles.
1745  *
1746  * \param r_i1, r_i2: Optional arguments to retrieve the overlapping edge between the 2 triangles.
1747  * \return true when the triangles intersect.
1748  *
1749  * \note intersections between coplanar triangles are currently undetected.
1750  */
1751 bool isect_tri_tri_epsilon_v3(
1752         const float t_a0[3], const float t_a1[3], const float t_a2[3],
1753         const float t_b0[3], const float t_b1[3], const float t_b2[3],
1754         float r_i1[3], float r_i2[3],
1755         const float epsilon)
1756 {
1757         const float *tri_pair[2][3] = {{t_a0, t_a1, t_a2}, {t_b0, t_b1, t_b2}};
1758         float plane_a[4], plane_b[4];
1759         float plane_co[3], plane_no[3];
1760
1761         BLI_assert((r_i1 != NULL) == (r_i2 != NULL));
1762
1763         /* normalizing is needed for small triangles T46007 */
1764         normal_tri_v3(plane_a, UNPACK3(tri_pair[0]));
1765         normal_tri_v3(plane_b, UNPACK3(tri_pair[1]));
1766
1767         plane_a[3] = -dot_v3v3(plane_a, t_a0);
1768         plane_b[3] = -dot_v3v3(plane_b, t_b0);
1769
1770         if (isect_plane_plane_v3(plane_a, plane_b, plane_co, plane_no) &&
1771             (normalize_v3(plane_no) > epsilon))
1772         {
1773                 /**
1774                  * Implementation note: its simpler to project the triangles onto the intersection plane
1775                  * before intersecting their edges with the ray, defined by 'isect_plane_plane_v3'.
1776                  * This way we can use 'line_point_factor_v3_ex' to see if an edge crosses 'co_proj',
1777                  * then use the factor to calculate the world-space point.
1778                  */
1779                 struct {
1780                         float min, max;
1781                 } range[2] = {{FLT_MAX, -FLT_MAX}, {FLT_MAX, -FLT_MAX}};
1782                 int t;
1783                 float co_proj[3];
1784
1785                 closest_to_plane3_normalized_v3(co_proj, plane_no, plane_co);
1786
1787                 /* For both triangles, find the overlap with the line defined by the ray [co_proj, plane_no].
1788                  * When the ranges overlap we know the triangles do too. */
1789                 for (t = 0; t < 2; t++) {
1790                         int j, j_prev;
1791                         float tri_proj[3][3];
1792
1793                         closest_to_plane3_normalized_v3(tri_proj[0], plane_no, tri_pair[t][0]);
1794                         closest_to_plane3_normalized_v3(tri_proj[1], plane_no, tri_pair[t][1]);
1795                         closest_to_plane3_normalized_v3(tri_proj[2], plane_no, tri_pair[t][2]);
1796
1797                         for (j = 0, j_prev = 2; j < 3; j_prev = j++) {
1798                                 /* note that its important to have a very small nonzero epsilon here
1799                                  * otherwise this fails for very small faces.
1800                                  * However if its too small, large adjacent faces will count as intersecting */
1801                                 const float edge_fac = line_point_factor_v3_ex(co_proj, tri_proj[j_prev], tri_proj[j], 1e-10f, -1.0f);
1802                                 /* ignore collinear lines, they are either an edge shared between 2 tri's
1803                                  * (which runs along [co_proj, plane_no], but can be safely ignored).
1804                                  *
1805                                  * or a collinear edge placed away from the ray - which we don't intersect with & can ignore. */
1806                                 if (UNLIKELY(edge_fac == -1.0f)) {
1807                                         /* pass */
1808                                 }
1809                                 else if (edge_fac > 0.0f && edge_fac < 1.0f) {
1810                                         float ix_tri[3];
1811                                         float span_fac;
1812
1813                                         interp_v3_v3v3(ix_tri, tri_pair[t][j_prev], tri_pair[t][j], edge_fac);
1814                                         /* the actual distance, since 'plane_no' is normalized */
1815                                         span_fac = dot_v3v3(plane_no, ix_tri);
1816
1817                                         range[t].min = min_ff(range[t].min, span_fac);
1818                                         range[t].max = max_ff(range[t].max, span_fac);
1819                                 }
1820                         }
1821
1822                         if (range[t].min == FLT_MAX) {
1823                                 return false;
1824                         }
1825                 }
1826
1827                 if (((range[0].min > range[1].max) ||
1828                      (range[0].max < range[1].min)) == 0)
1829                 {
1830                         if (r_i1 && r_i2) {
1831                                 project_plane_normalized_v3_v3v3(plane_co, plane_co, plane_no);
1832                                 madd_v3_v3v3fl(r_i1, plane_co, plane_no, max_ff(range[0].min, range[1].min));
1833                                 madd_v3_v3v3fl(r_i2, plane_co, plane_no, min_ff(range[0].max, range[1].max));
1834                         }
1835
1836                         return true;
1837                 }
1838         }
1839
1840         return false;
1841 }
1842
1843 /* Adapted from the paper by Kasper Fauerby */
1844
1845 /* "Improved Collision detection and Response" */
1846 static bool getLowestRoot(const float a, const float b, const float c, const float maxR, float *root)
1847 {
1848         /* Check if a solution exists */
1849         const float determinant = b * b - 4.0f * a * c;
1850
1851         /* If determinant is negative it means no solutions. */
1852         if (determinant >= 0.0f) {
1853                 /* calculate the two roots: (if determinant == 0 then
1854                  * x1==x2 but lets disregard that slight optimization) */
1855                 const float sqrtD = sqrtf(determinant);
1856                 float r1 = (-b - sqrtD) / (2.0f * a);
1857                 float r2 = (-b + sqrtD) / (2.0f * a);
1858
1859                 /* Sort so x1 <= x2 */
1860                 if (r1 > r2)
1861                         SWAP(float, r1, r2);
1862
1863                 /* Get lowest root: */
1864                 if (r1 > 0.0f && r1 < maxR) {
1865                         *root = r1;
1866                         return true;
1867                 }
1868
1869                 /* It is possible that we want x2 - this can happen */
1870                 /* if x1 < 0 */
1871                 if (r2 > 0.0f && r2 < maxR) {
1872                         *root = r2;
1873                         return true;
1874                 }
1875         }
1876         /* No (valid) solutions */
1877         return false;
1878 }
1879
1880 bool isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const float radius,
1881                                   const float v0[3], const float v1[3], const float v2[3],
1882                                   float *r_lambda, float ipoint[3])
1883 {
1884         float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
1885         float a, b, c, d, e, x, y, z, radius2 = radius * radius;
1886         float elen2, edotv, edotbv, nordotv;
1887         float newLambda;
1888         bool found_by_sweep = false;
1889
1890         sub_v3_v3v3(e1, v1, v0);
1891         sub_v3_v3v3(e2, v2, v0);
1892         sub_v3_v3v3(vel, p2, p1);
1893
1894         /*---test plane of tri---*/
1895         cross_v3_v3v3(nor, e1, e2);
1896         normalize_v3(nor);
1897
1898         /* flip normal */
1899         if (dot_v3v3(nor, vel) > 0.0f) negate_v3(nor);
1900
1901         a = dot_v3v3(p1, nor) - dot_v3v3(v0, nor);
1902         nordotv = dot_v3v3(nor, vel);
1903
1904         if (fabsf(nordotv) < 0.000001f) {
1905                 if (fabsf(a) >= radius) {
1906                         return false;
1907                 }
1908         }
1909         else {
1910                 float t0 = (-a + radius) / nordotv;
1911                 float t1 = (-a - radius) / nordotv;
1912
1913                 if (t0 > t1)
1914                         SWAP(float, t0, t1);
1915
1916                 if (t0 > 1.0f || t1 < 0.0f) return false;
1917
1918                 /* clamp to [0, 1] */
1919                 CLAMP(t0, 0.0f, 1.0f);
1920                 CLAMP(t1, 0.0f, 1.0f);
1921
1922                 /*---test inside of tri---*/
1923                 /* plane intersection point */
1924
1925                 point[0] = p1[0] + vel[0] * t0 - nor[0] * radius;
1926                 point[1] = p1[1] + vel[1] * t0 - nor[1] * radius;
1927                 point[2] = p1[2] + vel[2] * t0 - nor[2] * radius;
1928
1929
1930                 /* is the point in the tri? */
1931                 a = dot_v3v3(e1, e1);
1932                 b = dot_v3v3(e1, e2);
1933                 c = dot_v3v3(e2, e2);
1934
1935                 sub_v3_v3v3(temp, point, v0);
1936                 d = dot_v3v3(temp, e1);
1937                 e = dot_v3v3(temp, e2);
1938
1939                 x = d * c - e * b;
1940                 y = e * a - d * b;
1941                 z = x + y - (a * c - b * b);
1942
1943
1944                 if (z <= 0.0f && (x >= 0.0f && y >= 0.0f)) {
1945                         //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000) {
1946                         *r_lambda = t0;
1947                         copy_v3_v3(ipoint, point);
1948                         return true;
1949                 }
1950         }
1951
1952
1953         *r_lambda = 1.0f;
1954
1955         /*---test points---*/
1956         a = dot_v3v3(vel, vel);
1957
1958         /*v0*/
1959         sub_v3_v3v3(temp, p1, v0);
1960         b = 2.0f * dot_v3v3(vel, temp);
1961         c = dot_v3v3(temp, temp) - radius2;
1962
1963         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1964                 copy_v3_v3(ipoint, v0);
1965                 found_by_sweep = true;
1966         }
1967
1968         /*v1*/
1969         sub_v3_v3v3(temp, p1, v1);
1970         b = 2.0f * dot_v3v3(vel, temp);
1971         c = dot_v3v3(temp, temp) - radius2;
1972
1973         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1974                 copy_v3_v3(ipoint, v1);
1975                 found_by_sweep = true;
1976         }
1977
1978         /*v2*/
1979         sub_v3_v3v3(temp, p1, v2);
1980         b = 2.0f * dot_v3v3(vel, temp);
1981         c = dot_v3v3(temp, temp) - radius2;
1982
1983         if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
1984                 copy_v3_v3(ipoint, v2);
1985                 found_by_sweep = true;
1986         }
1987
1988         /*---test edges---*/
1989         sub_v3_v3v3(e3, v2, v1);  /* wasnt yet calculated */
1990
1991
1992         /*e1*/
1993         sub_v3_v3v3(bv, v0, p1);
1994
1995         elen2 = dot_v3v3(e1, e1);
1996         edotv = dot_v3v3(e1, vel);
1997         edotbv = dot_v3v3(e1, bv);
1998
1999         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2000         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2001         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2002
2003         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2004                 e = (edotv * newLambda - edotbv) / elen2;
2005
2006                 if (e >= 0.0f && e <= 1.0f) {
2007                         *r_lambda = newLambda;
2008                         copy_v3_v3(ipoint, e1);
2009                         mul_v3_fl(ipoint, e);
2010                         add_v3_v3(ipoint, v0);
2011                         found_by_sweep = true;
2012                 }
2013         }
2014
2015         /*e2*/
2016         /*bv is same*/
2017         elen2 = dot_v3v3(e2, e2);
2018         edotv = dot_v3v3(e2, vel);
2019         edotbv = dot_v3v3(e2, bv);
2020
2021         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2022         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2023         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2024
2025         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2026                 e = (edotv * newLambda - edotbv) / elen2;
2027
2028                 if (e >= 0.0f && e <= 1.0f) {
2029                         *r_lambda = newLambda;
2030                         copy_v3_v3(ipoint, e2);
2031                         mul_v3_fl(ipoint, e);
2032                         add_v3_v3(ipoint, v0);
2033                         found_by_sweep = true;
2034                 }
2035         }
2036
2037         /*e3*/
2038         /* sub_v3_v3v3(bv, v0, p1); */ /* UNUSED */
2039         /* elen2 = dot_v3v3(e1, e1); */ /* UNUSED */
2040         /* edotv = dot_v3v3(e1, vel); */ /* UNUSED */
2041         /* edotbv = dot_v3v3(e1, bv); */ /* UNUSED */
2042
2043         sub_v3_v3v3(bv, v1, p1);
2044         elen2 = dot_v3v3(e3, e3);
2045         edotv = dot_v3v3(e3, vel);
2046         edotbv = dot_v3v3(e3, bv);
2047
2048         a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2049         b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2050         c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2051
2052         if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2053                 e = (edotv * newLambda - edotbv) / elen2;
2054
2055                 if (e >= 0.0f && e <= 1.0f) {
2056                         *r_lambda = newLambda;
2057                         copy_v3_v3(ipoint, e3);
2058                         mul_v3_fl(ipoint, e);
2059                         add_v3_v3(ipoint, v1);
2060                         found_by_sweep = true;
2061                 }
2062         }
2063
2064
2065         return found_by_sweep;
2066 }
2067
2068 bool isect_axial_line_segment_tri_v3(
2069         const int axis, const float p1[3], const float p2[3],
2070         const float v0[3], const float v1[3], const float v2[3], float *r_lambda)
2071 {
2072         const float epsilon = 0.000001f;
2073         float p[3], e1[3], e2[3];
2074         float u, v, f;
2075         int a0 = axis, a1 = (axis + 1) % 3, a2 = (axis + 2) % 3;
2076
2077 #if 0
2078         return isect_line_segment_tri_v3(p1, p2, v0, v1, v2, lambda);
2079
2080         /* first a simple bounding box test */
2081         if (min_fff(v0[a1], v1[a1], v2[a1]) > p1[a1]) return false;
2082         if (min_fff(v0[a2], v1[a2], v2[a2]) > p1[a2]) return false;
2083         if (max_fff(v0[a1], v1[a1], v2[a1]) < p1[a1]) return false;
2084         if (max_fff(v0[a2], v1[a2], v2[a2]) < p1[a2]) return false;
2085
2086         /* then a full intersection test */
2087 #endif
2088
2089         sub_v3_v3v3(e1, v1, v0);
2090         sub_v3_v3v3(e2, v2, v0);
2091         sub_v3_v3v3(p, v0, p1);
2092
2093         f = (e2[a1] * e1[a2] - e2[a2] * e1[a1]);
2094         if ((f > -epsilon) && (f < epsilon)) return false;
2095
2096         v = (p[a2] * e1[a1] - p[a1] * e1[a2]) / f;
2097         if ((v < 0.0f) || (v > 1.0f)) return false;
2098
2099         f = e1[a1];
2100         if ((f > -epsilon) && (f < epsilon)) {
2101                 f = e1[a2];
2102                 if ((f > -epsilon) && (f < epsilon)) return false;
2103                 u = (-p[a2] - v * e2[a2]) / f;
2104         }
2105         else
2106                 u = (-p[a1] - v * e2[a1]) / f;
2107
2108         if ((u < 0.0f) || ((u + v) > 1.0f)) return false;
2109
2110         *r_lambda = (p[a0] + u * e1[a0] + v * e2[a0]) / (p2[a0] - p1[a0]);
2111
2112         if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return false;
2113
2114         return true;
2115 }
2116
2117 /**
2118  * \return The number of point of interests
2119  * 0 - lines are collinear
2120  * 1 - lines are coplanar, i1 is set to intersection
2121  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively
2122  */
2123 int isect_line_line_epsilon_v3(
2124         const float v1[3], const float v2[3],
2125         const float v3[3], const float v4[3],
2126         float r_i1[3], float r_i2[3],
2127         const float epsilon)
2128 {
2129         float a[3], b[3], c[3], ab[3], cb[3];
2130         float d, div;
2131
2132         sub_v3_v3v3(c, v3, v1);
2133         sub_v3_v3v3(a, v2, v1);
2134         sub_v3_v3v3(b, v4, v3);
2135
2136         cross_v3_v3v3(ab, a, b);
2137         d = dot_v3v3(c, ab);
2138         div = dot_v3v3(ab, ab);
2139
2140         /* important not to use an epsilon here, see: T45919 */
2141         /* test zero length line */
2142         if (UNLIKELY(div == 0.0f)) {
2143                 return 0;
2144         }
2145         /* test if the two lines are coplanar */
2146         else if (UNLIKELY(fabsf(d) <= epsilon)) {
2147                 cross_v3_v3v3(cb, c, b);
2148
2149                 mul_v3_fl(a, dot_v3v3(cb, ab) / div);
2150                 add_v3_v3v3(r_i1, v1, a);
2151                 copy_v3_v3(r_i2, r_i1);
2152
2153                 return 1; /* one intersection only */
2154         }
2155         /* if not */
2156         else {
2157                 float n[3], t[3];
2158                 float v3t[3], v4t[3];
2159                 sub_v3_v3v3(t, v1, v3);
2160
2161                 /* offset between both plane where the lines lies */
2162                 cross_v3_v3v3(n, a, b);
2163                 project_v3_v3v3(t, t, n);
2164
2165                 /* for the first line, offset the second line until it is coplanar */
2166                 add_v3_v3v3(v3t, v3, t);
2167                 add_v3_v3v3(v4t, v4, t);
2168
2169                 sub_v3_v3v3(c, v3t, v1);
2170                 sub_v3_v3v3(a, v2, v1);
2171                 sub_v3_v3v3(b, v4t, v3t);
2172
2173                 cross_v3_v3v3(ab, a, b);
2174                 cross_v3_v3v3(cb, c, b);
2175
2176                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
2177                 add_v3_v3v3(r_i1, v1, a);
2178
2179                 /* for the second line, just substract the offset from the first intersection point */
2180                 sub_v3_v3v3(r_i2, r_i1, t);
2181
2182                 return 2; /* two nearest points */
2183         }
2184 }
2185
2186 int isect_line_line_v3(
2187         const float v1[3], const float v2[3],
2188         const float v3[3], const float v4[3],
2189         float r_i1[3], float r_i2[3])
2190 {
2191         const float epsilon = 0.000001f;
2192         return isect_line_line_epsilon_v3(v1, v2, v3, v4, r_i1, r_i2, epsilon);
2193 }
2194
2195 /** Intersection point strictly between the two lines
2196  * \return false when no intersection is found
2197  */
2198 bool isect_line_line_strict_v3(const float v1[3], const float v2[3],
2199                                const float v3[3], const float v4[3],
2200                                float vi[3], float *r_lambda)
2201 {
2202         const float epsilon = 0.000001f;
2203         float a[3], b[3], c[3], ab[3], cb[3], ca[3];
2204         float d, div;
2205
2206         sub_v3_v3v3(c, v3, v1);
2207         sub_v3_v3v3(a, v2, v1);
2208         sub_v3_v3v3(b, v4, v3);
2209
2210         cross_v3_v3v3(ab, a, b);
2211         d = dot_v3v3(c, ab);
2212         div = dot_v3v3(ab, ab);
2213
2214         /* important not to use an epsilon here, see: T45919 */
2215         /* test zero length line */
2216         if (UNLIKELY(div == 0.0f)) {
2217                 return false;
2218         }
2219         /* test if the two lines are coplanar */
2220         else if (UNLIKELY(fabsf(d) < epsilon)) {
2221                 return false;
2222         }
2223         else {
2224                 float f1, f2;
2225                 cross_v3_v3v3(cb, c, b);
2226                 cross_v3_v3v3(ca, c, a);
2227
2228                 f1 = dot_v3v3(cb, ab) / div;
2229                 f2 = dot_v3v3(ca, ab) / div;
2230
2231                 if (f1 >= 0 && f1 <= 1 &&
2232                     f2 >= 0 && f2 <= 1)
2233                 {
2234                         mul_v3_fl(a, f1);
2235                         add_v3_v3v3(vi, v1, a);
2236
2237                         if (r_lambda) *r_lambda = f1;
2238
2239                         return true; /* intersection found */
2240                 }
2241                 else {
2242                         return false;
2243                 }
2244         }
2245 }
2246
2247 bool isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min2[3], const float max2[3])
2248 {
2249         return (min1[0] < max2[0] && min1[1] < max2[1] && min1[2] < max2[2] &&
2250                 min2[0] < max1[0] && min2[1] < max1[1] && min2[2] < max1[2]);
2251 }
2252
2253 void isect_ray_aabb_v3_precalc(
2254         struct IsectRayAABB_Precalc *data,
2255         const float ray_origin[3], const float ray_direction[3])
2256 {
2257         copy_v3_v3(data->ray_origin, ray_origin);
2258
2259         data->ray_inv_dir[0] = 1.0f / ray_direction[0];
2260         data->ray_inv_dir[1] = 1.0f / ray_direction[1];
2261         data->ray_inv_dir[2] = 1.0f / ray_direction[2];
2262
2263         data->sign[0] = data->ray_inv_dir[0] < 0.0f;
2264         data->sign[1] = data->ray_inv_dir[1] < 0.0f;
2265         data->sign[2] = data->ray_inv_dir[2] < 0.0f;
2266 }
2267
2268 /* Adapted from http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
2269 bool isect_ray_aabb_v3(
2270         const struct IsectRayAABB_Precalc *data, const float bb_min[3],
2271         const float bb_max[3], float *tmin_out)
2272 {
2273         float bbox[2][3];
2274
2275         copy_v3_v3(bbox[0], bb_min);
2276         copy_v3_v3(bbox[1], bb_max);
2277
2278         float tmin = (bbox[data->sign[0]][0] - data->ray_origin[0]) * data->ray_inv_dir[0];
2279         float tmax = (bbox[1 - data->sign[0]][0] - data->ray_origin[0]) * data->ray_inv_dir[0];
2280
2281         const float tymin = (bbox[data->sign[1]][1] - data->ray_origin[1]) * data->ray_inv_dir[1];
2282         const float tymax = (bbox[1 - data->sign[1]][1] - data->ray_origin[1]) * data->ray_inv_dir[1];
2283
2284         if ((tmin > tymax) || (tymin > tmax))
2285                 return false;
2286
2287         if (tymin > tmin)
2288                 tmin = tymin;
2289
2290         if (tymax < tmax)
2291                 tmax = tymax;
2292
2293         const float tzmin = (bbox[data->sign[2]][2] - data->ray_origin[2]) * data->ray_inv_dir[2];
2294         const float tzmax = (bbox[1 - data->sign[2]][2] - data->ray_origin[2]) * data->ray_inv_dir[2];
2295
2296         if ((tmin > tzmax) || (tzmin > tmax))
2297                 return false;
2298
2299         if (tzmin > tmin)
2300                 tmin = tzmin;
2301
2302         /* Note: tmax does not need to be updated since we don't use it
2303          * keeping this here for future reference - jwilkins */
2304         //if (tzmax < tmax) tmax = tzmax;
2305
2306         if (tmin_out)
2307                 (*tmin_out) = tmin;
2308
2309         return true;
2310 }
2311
2312 /*
2313  * Test a bounding box (AABB) for ray intersection
2314  * assumes the ray is already local to the boundbox space
2315  */
2316 bool isect_ray_aabb_v3_simple(
2317         const float orig[3], const float dir[3],
2318         const float bb_min[3], const float bb_max[3],
2319         float *tmin, float *tmax)
2320 {
2321         double t[7];
2322         float hit_dist[2];
2323         t[1] = (double)(bb_min[0] - orig[0]) / dir[0];
2324         t[2] = (double)(bb_max[0] - orig[0]) / dir[0];
2325         t[3] = (double)(bb_min[1] - orig[1]) / dir[1];
2326         t[4] = (double)(bb_max[1] - orig[1]) / dir[1];
2327         t[5] = (double)(bb_min[2] - orig[2]) / dir[2];
2328         t[6] = (double)(bb_max[2] - orig[2]) / dir[2];
2329         hit_dist[0] = (float)fmax(fmax(fmin(t[1], t[2]), fmin(t[3], t[4])), fmin(t[5], t[6]));
2330         hit_dist[1] = (float)fmin(fmin(fmax(t[1], t[2]), fmax(t[3], t[4])), fmax(t[5], t[6]));
2331         if ((hit_dist[1] < 0 || hit_dist[0] > hit_dist[1]))
2332                 return false;
2333         else {
2334                 if (tmin) *tmin = hit_dist[0];
2335                 if (tmax) *tmax = hit_dist[1];
2336                 return true;
2337         }
2338 }
2339
2340 /* find closest point to p on line through (l1, l2) and return lambda,
2341  * where (0 <= lambda <= 1) when cp is in the line segment (l1, l2)
2342  */
2343 float closest_to_line_v3(float r_close[3], const float p[3], const float l1[3], const float l2[3])
2344 {
2345         float h[3], u[3], lambda;
2346         sub_v3_v3v3(u, l2, l1);
2347         sub_v3_v3v3(h, p, l1);
2348         lambda = dot_v3v3(u, h) / dot_v3v3(u, u);
2349         r_close[0] = l1[0] + u[0] * lambda;
2350         r_close[1] = l1[1] + u[1] * lambda;
2351         r_close[2] = l1[2] + u[2] * lambda;
2352         return lambda;
2353 }
2354
2355 float closest_to_line_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2])
2356 {
2357         float h[2], u[2], lambda;
2358         sub_v2_v2v2(u, l2, l1);
2359         sub_v2_v2v2(h, p, l1);
2360         lambda = dot_v2v2(u, h) / dot_v2v2(u, u);
2361         r_close[0] = l1[0] + u[0] * lambda;
2362         r_close[1] = l1[1] + u[1] * lambda;
2363         return lambda;
2364 }
2365
2366 float ray_point_factor_v3_ex(
2367         const float p[3], const float ray_origin[3], const float ray_direction[3],
2368         const float epsilon, const float fallback)
2369 {
2370         float p_relative[3];
2371         sub_v3_v3v3(p_relative, p, ray_origin);
2372         const float dot = len_squared_v3(ray_direction);
2373         return (dot > epsilon) ? (dot_v3v3(ray_direction, p_relative) / dot) : fallback;
2374 }
2375
2376 float ray_point_factor_v3(
2377         const float p[3], const float ray_origin[3], const float ray_direction[3])
2378 {
2379         return ray_point_factor_v3_ex(p, ray_origin, ray_direction, 0.0f, 0.0f);
2380 }
2381
2382 /**
2383  * A simplified version of #closest_to_line_v3
2384  * we only need to return the ``lambda``
2385  *
2386  * \param epsilon: avoid approaching divide-by-zero.
2387  * Passing a zero will just check for nonzero division.
2388  */
2389 float line_point_factor_v3_ex(
2390         const float p[3], const float l1[3], const float l2[3],
2391         const float epsilon, const float fallback)
2392 {
2393         float h[3], u[3];
2394         float dot;
2395         sub_v3_v3v3(u, l2, l1);
2396         sub_v3_v3v3(h, p, l1);
2397 #if 0
2398         return (dot_v3v3(u, h) / dot_v3v3(u, u));
2399 #else
2400         /* better check for zero */
2401         dot = len_squared_v3(u);
2402         return (dot > epsilon) ? (dot_v3v3(u, h) / dot) : fallback;
2403 #endif
2404 }
2405 float line_point_factor_v3(
2406         const float p[3], const float l1[3], const float l2[3])
2407 {
2408         return line_point_factor_v3_ex(p, l1, l2, 0.0f, 0.0f);
2409 }
2410
2411 float line_point_factor_v2_ex(
2412         const float p[2], const float l1[2], const float l2[2],
2413         const float epsilon, const float fallback)
2414 {
2415         float h[2], u[2];
2416         float dot;
2417         sub_v2_v2v2(u, l2, l1);
2418         sub_v2_v2v2(h, p, l1);
2419 #if 0
2420         return (dot_v2v2(u, h) / dot_v2v2(u, u));
2421 #else
2422         /* better check for zero */
2423         dot = len_squared_v2(u);
2424         return (dot > epsilon) ? (dot_v2v2(u, h) / dot) : fallback;
2425 #endif
2426 }
2427
2428 float line_point_factor_v2(const float p[2], const float l1[2], const float l2[2])
2429 {
2430         return line_point_factor_v2_ex(p, l1, l2, 0.0f, 0.0f);
2431 }
2432
2433 /**
2434  * \note #isect_line_plane_v3() shares logic
2435  */
2436 float line_plane_factor_v3(const float plane_co[3], const float plane_no[3],
2437                            const float l1[3], const float l2[3])
2438 {
2439         float u[3], h[3];
2440         float dot;
2441         sub_v3_v3v3(u, l2, l1);
2442         sub_v3_v3v3(h, l1, plane_co);
2443         dot = dot_v3v3(plane_no, u);
2444         return (dot != 0.0f) ? -dot_v3v3(plane_no, h) / dot : 0.0f;
2445 }
2446
2447 /** Ensure the distance between these points is no greater than 'dist'.
2448  *  If it is, scale then both into the center.
2449  */
2450 void limit_dist_v3(float v1[3], float v2[3], const float dist)
2451 {
2452         const float dist_old = len_v3v3(v1, v2);
2453
2454         if (dist_old > dist) {
2455                 float v1_old[3];
2456                 float v2_old[3];
2457                 float fac = (dist / dist_old) * 0.5f;
2458
2459                 copy_v3_v3(v1_old, v1);
2460                 copy_v3_v3(v2_old, v2);
2461
2462                 interp_v3_v3v3(v1, v1_old, v2_old, 0.5f - fac);
2463                 interp_v3_v3v3(v2, v1_old, v2_old, 0.5f + fac);
2464         }
2465 }
2466
2467 /*
2468  *     x1,y2
2469  *     |  \
2470  *     |   \     .(a,b)
2471  *     |    \
2472  *     x1,y1-- x2,y1
2473  */
2474 int isect_point_tri_v2_int(const int x1, const int y1, const int x2, const int y2, const int a, const int b)
2475 {
2476         float v1[2], v2[2], v3[2], p[2];
2477
2478         v1[0] = (float)x1;
2479         v1[1] = (float)y1;
2480
2481         v2[0] = (float)x1;
2482         v2[1] = (float)y2;
2483
2484         v3[0] = (float)x2;
2485         v3[1] = (float)y1;
2486
2487         p[0] = (float)a;
2488         p[1] = (float)b;
2489
2490         return isect_point_tri_v2(p, v1, v2, v3);
2491 }
2492
2493 static bool point_in_slice(const float p[3], const float v1[3], const float l1[3], const float l2[3])
2494 {
2495         /*
2496          * what is a slice ?
2497          * some maths:
2498          * a line including (l1, l2) and a point not on the line
2499          * define a subset of R3 delimited by planes parallel to the line and orthogonal
2500          * to the (point --> line) distance vector, one plane on the line one on the point,
2501          * the room inside usually is rather small compared to R3 though still infinite
2502          * useful for restricting (speeding up) searches
2503          * e.g. all points of triangular prism are within the intersection of 3 'slices'
2504          * another trivial case : cube
2505          * but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
2506          */
2507         float h, rp[3], cp[3], q[3];
2508
2509         closest_to_line_v3(cp, v1, l1, l2);
2510         sub_v3_v3v3(q, cp, v1);
2511
2512         sub_v3_v3v3(rp, p, v1);
2513         h = dot_v3v3(q, rp) / dot_v3v3(q, q);
2514         /* note: when 'h' is nan/-nan, this check returns false
2515          * without explicit check - covering the degenerate case */
2516         return (h >= 0.0f && h <= 1.0f);
2517 }
2518
2519 #if 0
2520
2521 /* adult sister defining the slice planes by the origin and the normal
2522  * NOTE |normal| may not be 1 but defining the thickness of the slice */
2523 static int point_in_slice_as(float p[3], float origin[3], float normal[3])
2524 {
2525         float h, rp[3];
2526         sub_v3_v3v3(rp, p, origin);
2527         h = dot_v3v3(normal, rp) / dot_v3v3(normal, normal);
2528         if (h < 0.0f || h > 1.0f) return 0;
2529         return 1;
2530 }
2531
2532 /*mama (knowing the squared length of the normal) */
2533 static int point_in_slice_m(float p[3], float origin[3], float normal[3], float lns)
2534 {
2535         float h, rp[3];
2536         sub_v3_v3v3(rp, p, origin);
2537         h = dot_v3v3(normal, rp) / lns;
2538         if (h < 0.0f || h > 1.0f) return 0;
2539         return 1;
2540 }
2541 #endif
2542
2543 bool isect_point_tri_prism_v3(const float p[3], const float v1[3], const float v2[3], const float v3[3])
2544 {
2545         if (!point_in_slice(p, v1, v2, v3)) return false;
2546         if (!point_in_slice(p, v2, v3, v1)) return false;
2547         if (!point_in_slice(p, v3, v1, v2)) return false;
2548         return true;
2549 }
2550
2551 /**
2552  * \param r_isect_co: The point \a p projected onto the triangle.
2553  * \return True when \a p is inside the triangle.
2554  * \note Its up to the caller to check the distance between \a p and \a r_vi against an error margin.
2555  */
2556 bool isect_point_tri_v3(
2557         const float p[3], const float v1[3], const float v2[3], const float v3[3],
2558         float r_isect_co[3])
2559 {
2560         if (isect_point_tri_prism_v3(p, v1, v2, v3)) {
2561                 float plane[4];
2562                 float no[3];
2563
2564                 /* Could use normal_tri_v3, but doesn't have to be unit-length */
2565                 cross_tri_v3(no, v1, v2, v3);
2566                 BLI_assert(len_squared_v3(no) != 0.0f);
2567
2568                 plane_from_point_normal_v3(plane, v1, no);
2569                 closest_to_plane_v3(r_isect_co, plane, p);
2570
2571                 return true;
2572         }
2573         else {
2574                 return false;
2575         }
2576 }
2577
2578 bool clip_segment_v3_plane(
2579         const float p1[3], const float p2[3],
2580         const float plane[4],
2581         float r_p1[3], float r_p2[3])
2582 {
2583         float dp[3], div;
2584
2585         sub_v3_v3v3(dp, p2, p1);
2586         div = dot_v3v3(dp, plane);
2587
2588         if (div == 0.0f) /* parallel */
2589                 return true;
2590
2591         float t = -plane_point_side_v3(plane, p1);
2592
2593         if (div > 0.0f) {
2594                 /* behind plane, completely clipped */
2595                 if (t >= div) {
2596                         return false;
2597                 }
2598                 else if (t > 0.0f) {
2599                         const float p1_copy[3] = {UNPACK3(p1)};
2600                         copy_v3_v3(r_p2, p2);
2601                         madd_v3_v3v3fl(r_p1, p1_copy, dp, t / div);
2602                         return true;
2603                 }
2604         }
2605         else {
2606                 /* behind plane, completely clipped */
2607                 if (t >= 0.0f) {
2608                         return false;
2609                 }
2610                 else if (t > div) {
2611                         const float p1_copy[3] = {UNPACK3(p1)};
2612                         copy_v3_v3(r_p1, p1);
2613                         madd_v3_v3v3fl(r_p2, p1_copy, dp, t / div);
2614                         return true;
2615                 }
2616         }
2617
2618         /* incase input/output values match (above also) */
2619         const float p1_copy[3] = {UNPACK3(p1)};
2620         copy_v3_v3(r_p2, p2);
2621         copy_v3_v3(r_p1, p1_copy);
2622         return true;
2623 }
2624
2625 bool clip_segment_v3_plane_n(
2626         const float p1[3], const float p2[3],
2627         const float plane_array[][4], const int plane_tot,
2628         float r_p1[3], float r_p2[3])
2629 {
2630         /* intersect from both directions */
2631         float p1_fac = 0.0f, p2_fac = 1.0f;
2632
2633         float dp[3];
2634         sub_v3_v3v3(dp, p2, p1);
2635
2636         for (int i = 0; i < plane_tot; i++) {
2637                 const float *plane = plane_array[i];
2638                 const float div = dot_v3v3(dp, plane);
2639
2640                 if (div != 0.0f) {
2641                         float t = -plane_point_side_v3(plane, p1);
2642                         if (div > 0.0f) {
2643                                 /* clip p1 lower bounds */
2644                                 if (t >= div) {
2645                                         return false;
2646                                 }
2647                                 else if (t > 0.0f) {
2648                                         t /= div;
2649                                         if (t > p1_fac) {
2650                                                 p1_fac = t;
2651                                                 if (p1_fac > p2_fac) {
2652                                                         return false;
2653                                                 }
2654                                         }
2655                                 }
2656                         }
2657                         else if (div < 0.0f) {
2658                                 /* clip p2 upper bounds */
2659                                 if (t >= 0.0f) {
2660                                         return false;
2661                                 }
2662                                 else if (t > div) {
2663                                         t /= div;
2664                                         if (t < p2_fac) {
2665                                                 p2_fac = t;
2666                                                 if (p1_fac > p2_fac) {
2667                                                         return false;
2668                                                 }
2669                                         }
2670                                 }
2671                         }
2672                 }
2673         }
2674
2675         /* incase input/output values match */
2676         const float p1_copy[3] = {UNPACK3(p1)};
2677
2678         madd_v3_v3v3fl(r_p1, p1_copy, dp, p1_fac);
2679         madd_v3_v3v3fl(r_p2, p1_copy, dp, p2_fac);
2680
2681         return true;
2682 }
2683
2684 /****************************** Axis Utils ********************************/
2685
2686 /**
2687  * \brief Normal to x,y matrix
2688  *
2689  * Creates a 3x3 matrix from a normal.
2690  * This matrix can be applied to vectors so their 'z' axis runs along \a normal.
2691  * In practice it means you can use x,y as 2d coords. \see
2692  *
2693  * \param r_mat The matrix to return.
2694  * \param normal A unit length vector.
2695  */
2696 void axis_dominant_v3_to_m3(float r_mat[3][3], const float normal[3])
2697 {
2698         BLI_ASSERT_UNIT_V3(normal);
2699
2700         copy_v3_v3(r_mat[2], normal);
2701         ortho_basis_v3v3_v3(r_mat[0], r_mat[1], r_mat[2]);
2702
2703         BLI_ASSERT_UNIT_V3(r_mat[0]);
2704         BLI_ASSERT_UNIT_V3(r_mat[1]);
2705
2706         transpose_m3(r_mat);
2707
2708         BLI_assert(!is_negative_m3(r_mat));
2709         BLI_assert((fabsf(dot_m3_v3_row_z(r_mat, normal) - 1.0f) < BLI_ASSERT_UNIT_EPSILON) || is_zero_v3(normal));
2710 }
2711
2712 /**
2713  * Same as axis_dominant_v3_to_m3, but flips the normal
2714  */
2715 void axis_dominant_v3_to_m3_negate(float r_mat[3][3], const float normal[3])
2716 {
2717         BLI_ASSERT_UNIT_V3(normal);
2718
2719         negate_v3_v3(r_mat[2], normal);
2720         ortho_basis_v3v3_v3(r_mat[0], r_mat[1], r_mat[2]);
2721
2722         BLI_ASSERT_UNIT_V3(r_mat[0]);
2723         BLI_ASSERT_UNIT_V3(r_mat[1]);
2724
2725         transpose_m3(r_mat);
2726
2727         BLI_assert(!is_negative_m3(r_mat));
2728         BLI_assert((dot_m3_v3_row_z(r_mat, normal) < BLI_ASSERT_UNIT_EPSILON) || is_zero_v3(normal));
2729 }
2730
2731 /****************************** Interpolation ********************************/
2732
2733 static float tri_signed_area(const float v1[3], const float v2[3], const float v3[3], const int i, const int j)
2734 {
2735         return 0.5f * ((v1[i] - v2[i]) * (v2[j] - v3[j]) + (v1[j] - v2[j]) * (v3[i] - v2[i]));
2736 }
2737
2738 /* return 1 when degenerate */
2739 static bool 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])
2740 {
2741         float wtot;
2742         int i, j;
2743
2744         axis_dominant_v3(&i, &j, n);
2745
2746         w[0] = tri_signed_area(v2, v3, co, i, j);
2747         w[1] = tri_signed_area(v3, v1, co, i, j);
2748         w[2] = tri_signed_area(v1, v2, co, i, j);
2749
2750         wtot = w[0] + w[1] + w[2];
2751
2752         if (fabsf(wtot) > FLT_EPSILON) {
2753                 mul_v3_fl(w, 1.0f / wtot);
2754                 return false;
2755         }
2756         else {
2757                 /* zero area triangle */
2758                 copy_v3_fl(w, 1.0f / 3.0f);
2759                 return true;
2760         }
2761 }
2762
2763 void interp_weights_tri_v3(float w[3], const float v1[3], const float v2[3], const float v3[3], const float co[3])
2764 {
2765         float n[3];
2766
2767         normal_tri_v3(n, v1, v2, v3);
2768         barycentric_weights(v1, v2, v3, co, n, w);
2769 }
2770
2771 void interp_weights_quad_v3(float w[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3], const float co[3])
2772 {
2773         float w2[3];
2774
2775         w[0] = w[1] = w[2] = w[3] = 0.0f;
2776
2777         /* first check for exact match */
2778         if (equals_v3v3(co, v1))
2779                 w[0] = 1.0f;
2780         else if (equals_v3v3(co, v2))
2781                 w[1] = 1.0f;
2782         else if (equals_v3v3(co, v3))
2783                 w[2] = 1.0f;
2784         else if (equals_v3v3(co, v4))
2785                 w[3] = 1.0f;
2786         else {
2787                 /* otherwise compute barycentric interpolation weights */
2788                 float n1[3], n2[3], n[3];
2789                 bool degenerate;
2790
2791                 sub_v3_v3v3(n1, v1, v3);
2792                 sub_v3_v3v3(n2, v2, v4);
2793                 cross_v3_v3v3(n, n1, n2);
2794
2795                 degenerate = barycentric_weights(v1, v2, v4, co, n, w);
2796                 SWAP(float, w[2], w[3]);
2797
2798                 if (degenerate || (w[0] < 0.0f)) {
2799                         /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2800                          * so we interpolate using the other triangle */
2801                         degenerate = barycentric_weights(v2, v3, v4, co, n, w2);
2802
2803                         if (!degenerate) {
2804                                 w[0] = 0.0f;
2805                                 w[1] = w2[0];
2806                                 w[2] = w2[1];
2807                                 w[3] = w2[2];
2808                         }
2809                 }
2810         }
2811 }
2812
2813 /* return 1 of point is inside triangle, 2 if it's on the edge, 0 if point is outside of triangle */
2814 int barycentric_inside_triangle_v2(const float w[3])
2815 {
2816         if (IN_RANGE(w[0], 0.0f, 1.0f) &&
2817             IN_RANGE(w[1], 0.0f, 1.0f) &&
2818             IN_RANGE(w[2], 0.0f, 1.0f))
2819         {
2820                 return 1;
2821         }
2822         else if (IN_RANGE_INCL(w[0], 0.0f, 1.0f) &&
2823                  IN_RANGE_INCL(w[1], 0.0f, 1.0f) &&
2824                  IN_RANGE_INCL(w[2], 0.0f, 1.0f))
2825         {
2826                 return 2;
2827         }
2828
2829         return 0;
2830 }
2831
2832 /* returns 0 for degenerated triangles */
2833 bool barycentric_coords_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
2834 {
2835         const float x = co[0], y = co[1];
2836         const float x1 = v1[0], y1 = v1[1];
2837         const float x2 = v2[0], y2 = v2[1];
2838         const float x3 = v3[0], y3 = v3[1];
2839         const float det = (y2 - y3) * (x1 - x3) + (x3 - x2) * (y1 - y3);
2840
2841         if (fabsf(det) > FLT_EPSILON) {
2842                 w[0] = ((y2 - y3) * (x - x3) + (x3 - x2) * (y - y3)) / det;
2843                 w[1] = ((y3 - y1) * (x - x3) + (x1 - x3) * (y - y3)) / det;
2844                 w[2] = 1.0f - w[0] - w[1];
2845
2846                 return true;
2847         }
2848
2849         return false;
2850 }
2851
2852 /**
2853  * \note: using #cross_tri_v2 means locations outside the triangle are correctly weighted
2854  *
2855  * \note This is *exactly* the same calculation as #resolve_tri_uv_v2,
2856  * although it has double precision and is used for texture baking, so keep both.
2857  */
2858 void barycentric_weights_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
2859 {
2860         float wtot;
2861
2862         w[0] = cross_tri_v2(v2, v3, co);
2863         w[1] = cross_tri_v2(v3, v1, co);
2864         w[2] = cross_tri_v2(v1, v2, co);
2865         wtot = w[0] + w[1] + w[2];
2866
2867         if (wtot != 0.0f) {
2868                 mul_v3_fl(w, 1.0f / wtot);
2869         }
2870         else { /* dummy values for zero area face */
2871                 copy_v3_fl(w, 1.0f / 3.0f);
2872         }
2873 }
2874
2875 /**
2876  * still use 2D X,Y space but this works for verts transformed by a perspective matrix,
2877  * using their 4th component as a weight
2878  */
2879 void barycentric_weights_v2_persp(const float v1[4], const float v2[4], const float v3[4], const float co[2], float w[3])
2880 {
2881         float wtot;
2882
2883         w[0] = cross_tri_v2(v2, v3, co) / v1[3];
2884         w[1] = cross_tri_v2(v3, v1, co) / v2[3];
2885         w[2] = cross_tri_v2(v1, v2, co) / v3[3];
2886         wtot = w[0] + w[1] + w[2];
2887
2888         if (wtot != 0.0f) {
2889                 mul_v3_fl(w, 1.0f / wtot);
2890         }
2891         else { /* dummy values for zero area face */
2892                 w[0] = w[1] = w[2] = 1.0f / 3.0f;
2893         }
2894 }
2895
2896 /**
2897  * same as #barycentric_weights_v2 but works with a quad,
2898  * note: untested for values outside the quad's bounds
2899  * this is #interp_weights_poly_v2 expanded for quads only
2900  */
2901 void barycentric_weights_v2_quad(const float v1[2], const float v2[2], const float v3[2], const float v4[2],
2902                                  const float co[2], float w[4])
2903 {
2904         /* note: fabsf() here is not needed for convex quads (and not used in interp_weights_poly_v2).
2905          *       but in the case of concave/bow-tie quads for the mask rasterizer it gives unreliable results
2906          *       without adding absf(). If this becomes an issue for more general usage we could have
2907          *       this optional or use a different function - Campbell */
2908 #define MEAN_VALUE_HALF_TAN_V2(_area, i1, i2) \
2909                 ((_area = cross_v2v2(dirs[i1], dirs[i2])) != 0.0f ? \
2910                  fabsf(((lens[i1] * lens[i2]) - dot_v2v2(dirs[i1], dirs[i2])) / _area) : 0.0f)
2911
2912         const float dirs[4][2] = {
2913             {v1[0] - co[0], v1[1] - co[1]},
2914             {v2[0] - co[0], v2[1] - co[1]},
2915             {v3[0] - co[0], v3[1] - co[1]},
2916             {v4[0] - co[0], v4[1] - co[1]},
2917         };
2918
2919         const float lens[4] = {
2920             len_v2(dirs[0]),
2921             len_v2(dirs[1]),
2922             len_v2(dirs[2]),
2923             len_v2(dirs[3]),
2924         };
2925
2926         /* avoid divide by zero */
2927         if      (UNLIKELY(lens[0] < FLT_EPSILON)) { w[0] = 1.0f; w[1] = w[2] = w[3] = 0.0f; }
2928         else if (UNLIKELY(lens[1] < FLT_EPSILON)) { w[1] = 1.0f; w[0] = w[2] = w[3] = 0.0f; }
2929         else if (UNLIKELY(lens[2] < FLT_EPSILON)) { w[2] = 1.0f; w[0] = w[1] = w[3] = 0.0f; }
2930         else if (UNLIKELY(lens[3] < FLT_EPSILON)) { w[3] = 1.0f; w[0] = w[1] = w[2] = 0.0f; }
2931         else {
2932                 float wtot, area;
2933
2934                 /* variable 'area' is just for storage,
2935                  * the order its initialized doesn't matter */
2936 #ifdef __clang__
2937 #  pragma clang diagnostic push
2938 #  pragma clang diagnostic ignored "-Wunsequenced"
2939 #endif
2940
2941                 /* inline mean_value_half_tan four times here */
2942                 const float t[4] = {
2943                         MEAN_VALUE_HALF_TAN_V2(area, 0, 1),
2944                         MEAN_VALUE_HALF_TAN_V2(area, 1, 2),
2945                         MEAN_VALUE_HALF_TAN_V2(area, 2, 3),
2946                         MEAN_VALUE_HALF_TAN_V2(area, 3, 0),
2947                 };
2948
2949 #ifdef __clang__
2950 #  pragma clang diagnostic pop
2951 #endif
2952
2953 #undef MEAN_VALUE_HALF_TAN_V2
2954
2955                 w[0] = (t[3] + t[0]) / lens[0];
2956                 w[1] = (t[0] + t[1]) / lens[1];
2957                 w[2] = (t[1] + t[2]) / lens[2];
2958                 w[3] = (t[2] + t[3]) / lens[3];
2959
2960                 wtot = w[0] + w[1] + w[2] + w[3];
2961
2962                 if (wtot != 0.0f) {
2963                         mul_v4_fl(w, 1.0f / wtot);
2964                 }
2965                 else { /* dummy values for zero area face */
2966                         copy_v4_fl(w, 1.0f / 4.0f);
2967                 }
2968         }
2969 }
2970
2971 /* given 2 triangles in 3D space, and a point in relation to the first triangle.
2972  * calculate the location of a point in relation to the second triangle.
2973  * Useful for finding relative positions with geometry */
2974 void transform_point_by_tri_v3(
2975         float pt_tar[3], float const pt_src[3],
2976         const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3],
2977         const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3])
2978 {
2979         /* this works by moving the source triangle so its normal is pointing on the Z
2980          * axis where its barycentric weights can be calculated in 2D and its Z offset can
2981          *  be re-applied. The weights are applied directly to the targets 3D points and the
2982          *  z-depth is used to scale the targets normal as an offset.
2983          * This saves transforming the target into its Z-Up orientation and back (which could also work) */
2984         float no_tar[3], no_src[3];
2985         float mat_src[3][3];
2986         float pt_src_xy[3];
2987         float tri_xy_src[3][3];
2988         float w_src[3];
2989         float area_tar, area_src;
2990         float z_ofs_src;
2991
2992         normal_tri_v3(no_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3);
2993         normal_tri_v3(no_src, tri_src_p1, tri_src_p2, tri_src_p3);
2994
2995         axis_dominant_v3_to_m3(mat_src, no_src);
2996
2997         /* make the source tri xy space */
2998         mul_v3_m3v3(pt_src_xy,     mat_src, pt_src);
2999         mul_v3_m3v3(tri_xy_src[0], mat_src, tri_src_p1);
3000         mul_v3_m3v3(tri_xy_src[1], mat_src, tri_src_p2);
3001         mul_v3_m3v3(tri_xy_src[2], mat_src, tri_src_p3);
3002
3003
3004         barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src);
3005         interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src);
3006
3007         area_tar = sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3));
3008         area_src = sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2]));
3009
3010         z_ofs_src = pt_src_xy[2] - tri_xy_src[0][2];
3011         madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar);
3012 }
3013
3014 /**
3015  * Simply re-interpolates,
3016  * assumes p_src is between \a l_src_p1-l_src_p2
3017  */
3018 void transform_point_by_seg_v3(
3019         float p_dst[3], const float p_src[3],
3020         const float l_dst_p1[3], const float l_dst_p2[3],
3021         const float l_src_p1[3], const float l_src_p2[3])
3022 {
3023         float t = line_point_factor_v3(p_src, l_src_p1, l_src_p2);
3024         interp_v3_v3v3(p_dst, l_dst_p1, l_dst_p2, t);
3025 }
3026
3027 /* given an array with some invalid values this function interpolates valid values
3028  * replacing the invalid ones */
3029 int interp_sparse_array(float *array, const int list_size, const float skipval)
3030 {
3031         int found_invalid = 0;
3032         int found_valid = 0;
3033         int i;
3034
3035         for (i = 0; i < list_size; i++) {
3036                 if (array[i] == skipval)
3037                         found_invalid = 1;
3038                 else
3039                         found_valid = 1;
3040         }
3041
3042         if (found_valid == 0) {
3043                 return -1;
3044         }
3045         else if (found_invalid == 0) {
3046                 return 0;
3047         }
3048         else {
3049                 /* found invalid depths, interpolate */
3050                 float valid_last = skipval;
3051                 int valid_ofs = 0;
3052
3053                 float *array_up = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
3054                 float *array_down = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
3055
3056                 int *ofs_tot_up = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tup");
3057                 int *ofs_tot_down = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tdown");
3058
3059                 for (i = 0; i < list_size; i++) {
3060                         if (array[i] == skipval) {
3061                                 array_up[i] = valid_last;
3062                                 ofs_tot_up[i] = ++valid_ofs;
3063                         }
3064                         else {
3065                                 valid_last = array[i];
3066                                 valid_ofs = 0;
3067                         }
3068                 }
3069
3070                 valid_last = skipval;
3071                 valid_ofs = 0;
3072
3073                 for (i = list_size - 1; i >= 0; i--) {
3074                         if (array[i] == skipval) {
3075                                 array_down[i] = valid_last;
3076                                 ofs_tot_down[i] = ++valid_ofs;
3077                         }
3078                         else {
3079                                 valid_last = array[i];
3080                                 valid_ofs = 0;
3081                         }
3082                 }
3083
3084                 /* now blend */
3085                 for (i = 0; i < list_size; i++) {
3086                         if (array[i] == skipval) {
3087                                 if (array_up[i] != skipval && array_down[i] != skipval) {
3088                                         array[i] = ((array_up[i]   * (float)ofs_tot_down[i]) +
3089                                                     (array_down[i] * (float)ofs_tot_up[i])) / (float)(ofs_tot_down[i] + ofs_tot_up[i]);
3090                                 }
3091                                 else if (array_up[i] != skipval) {
3092                                         array[i] = array_up[i];
3093                                 }
3094                                 else if (array_down[i] != skipval) {
3095                                         array[i] = array_down[i];
3096                                 }
3097                         }
3098                 }
3099
3100                 MEM_freeN(array_up);
3101                 MEM_freeN(array_down);
3102
3103                 MEM_freeN(ofs_tot_up);
3104                 MEM_freeN(ofs_tot_down);
3105         }
3106
3107         return 1;
3108 }
3109
3110 /** \name interp_weights_poly_v2, v3
3111  * \{ */
3112
3113 #define IS_POINT_IX     (1 << 0)
3114 #define IS_SEGMENT_IX   (1 << 1)
3115
3116 #define DIR_V3_SET(d_len, va, vb)  { \
3117         sub_v3_v3v3((d_len)->dir, va, vb); \
3118         (d_len)->len = len_v3((d_len)->dir); \
3119 } (void)0
3120
3121 #define DIR_V2_SET(d_len, va, vb)  { \
3122         sub_v2_v2v2((d_len)->dir, va, vb); \
3123         (d_len)->len = len_v2((d_len)->dir); \
3124 } (void)0
3125
3126 struct Float3_Len {
3127         float dir[3], len;
3128 };
3129
3130 struct Float2_Len {
3131         float dir[2], len;
3132 };
3133
3134 /* Mean value weights - smooth interpolation weights for polygons with
3135  * more than 3 vertices */
3136 static float mean_value_half_tan_v3(const struct Float3_Len *d_curr, const struct Float3_Len *d_next)
3137 {
3138         float cross[3], area;
3139         cross_v3_v3v3(cross, d_curr->dir, d_next->dir);
3140         area = len_v3(cross);
3141         if (LIKELY(fabsf(area) > FLT_EPSILON)) {
3142                 const float dot = dot_v3v3(d_curr->dir, d_next->dir);
3143                 const float len = d_curr->len * d_next->len;
3144                 return (len - dot) / area;
3145         }
3146         else {
3147                 return 0.0f;
3148         }
3149 }
3150
3151 static float mean_value_half_tan_v2(const struct Float2_Len *d_curr, const struct Float2_Len *d_next)
3152 {
3153         float area;
3154         /* different from the 3d version but still correct */
3155         area = cross_v2v2(d_curr->dir, d_next->dir);
3156         if (LIKELY(fabsf(area) > FLT_EPSILON)) {
3157                 const float dot = dot_v2v2(d_curr->dir, d_next->dir);
3158                 const float len = d_curr->len * d_next->len;
3159                 return (len - dot) / area;
3160         }
3161         else {
3162                 return 0.0f;
3163         }
3164 }
3165
3166 void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[3])
3167 {
3168         const float eps = 1e-5f;  /* take care, low values cause [#36105] */
3169         const float eps_sq = eps * eps;
3170         const float *v_curr, *v_next;
3171         float ht_prev, ht;  /* half tangents */
3172         float totweight = 0.0f;
3173         int i_curr, i_next;
3174         char ix_flag = 0;
3175         struct Float3_Len d_curr, d_next;
3176
3177         /* loop over 'i_next' */
3178         i_curr = n - 1;
3179         i_next = 0;
3180
3181         v_curr = v[i_curr];
3182         v_next = v[i_next];
3183
3184         DIR_V3_SET(&d_curr, v_curr - 3 /* v[n - 2] */, co);
3185         DIR_V3_SET(&d_next, v_curr     /* v[n - 1] */, co);
3186         ht_prev = mean_value_half_tan_v3(&d_curr, &d_next);
3187
3188         while (i_next < n) {
3189                 /* Mark Mayer et al algorithm that is used here does not operate well if vertex is close
3190                  * to borders of face. In that case, do simple linear interpolation between the two edge vertices */
3191
3192                 /* 'd_next.len' is infact 'd_curr.len', just avoid copy to begin with */
3193                 if (UNLIKELY(d_next.len < eps)) {
3194                         ix_flag = IS_POINT_IX;
3195                         break;
3196                 }
3197                 else if (UNLIKELY(dist_squared_to_line_segment_v3(co, v_curr, v_next) < eps_sq)) {
3198                         ix_flag = IS_SEGMENT_IX;
3199                         break;
3200                 }
3201
3202                 d_curr = d_next;
3203                 DIR_V3_SET(&d_next, v_next, co);
3204                 ht = mean_value_half_tan_v3(&d_curr, &d_next);
3205                 w[i_curr] = (ht_prev + ht) / d_curr.len;
3206                 totweight += w[i_curr];
3207
3208                 /* step */
3209                 i_curr = i_next++;
3210                 v_curr = v_next;
3211                 v_next = v[i_next];
3212
3213                 ht_prev = ht;
3214         }
3215
3216         if (ix_flag) {
3217                 memset(w, 0, sizeof(*w) * (size_t)n);
3218
3219                 if (ix_flag & IS_POINT_IX) {
3220                         w[i_curr] = 1.0f;
3221                 }
3222                 else {
3223                         float fac = line_point_factor_v3(co, v_curr, v_next);
3224                         CLAMP(fac, 0.0f, 1.0f);
3225                         w[i_curr] = 1.0f - fac;
3226                         w[i_next] = fac;
3227                 }
3228         }
3229         else {
3230                 if (totweight != 0.0f) {
3231                         for (i_curr = 0; i_curr < n; i_curr++) {
3232                                 w[i_curr] /= totweight;
3233                         }
3234                 }
3235         }
3236 }
3237
3238
3239 void interp_weights_poly_v2(float *w, float v[][2], const int n, const float co[2])
3240 {
3241         const float eps = 1e-5f;  /* take care, low values cause [#36105] */
3242         const float eps_sq = eps * eps;
3243         const float *v_curr, *v_next;
3244         float ht_prev, ht;  /* half tangents */
3245         float totweight = 0.0f;
3246         int i_curr, i_next;
3247         char ix_flag = 0;
3248         struct Float2_Len d_curr, d_next;
3249
3250         /* loop over 'i_next' */
3251         i_curr = n - 1;
3252         i_next = 0;
3253
3254         v_curr = v[i_curr];
3255         v_next = v[i_next];
3256
3257         DIR_V2_SET(&d_curr, v_curr - 2 /* v[n - 2] */, co);
3258         DIR_V2_SET(&d_next, v_curr     /* v[n - 1] */, co);
3259         ht_prev = mean_value_half_tan_v2(&d_curr, &d_next);
3260
3261         while (i_next < n) {
3262                 /* Mark Mayer et al algorithm that is used here does not operate well if vertex is close
3263                  * to borders of face. In that case, do simple linear interpolation between the two edge vertices */
3264
3265                 /* 'd_next.len' is infact 'd_curr.len', just avoid copy to begin with */
3266                 if (UNLIKELY(d_next.len < eps)) {
3267                         ix_flag = IS_POINT_IX;
3268                         break;
3269                 }
3270                 else if (UNLIKELY(dist_squared_to_line_segment_v2(co, v_curr, v_next) < eps_sq)) {
3271                         ix_flag = IS_SEGMENT_IX;
3272                         break;
3273                 }
3274
3275                 d_curr = d_next;
3276                 DIR_V2_SET(&d_next, v_next, co);
3277                 ht = mean_value_half_tan_v2(&d_curr, &d_next);
3278                 w[i_curr] = (ht_prev + ht) / d_curr.len;
3279                 totweight += w[i_curr];
3280
3281                 /* step */
3282                 i_curr = i_next++;
3283                 v_curr = v_next;
3284                 v_next = v[i_next];
3285
3286                 ht_prev = ht;
3287         }
3288
3289         if (ix_flag) {
3290                 memset(w, 0, sizeof(*w) * (size_t)n);
3291
3292                 if (ix_flag & IS_POINT_IX) {
3293                         w[i_curr] = 1.0f;
3294                 }
3295                 else {
3296                         float fac = line_point_factor_v2(co, v_curr, v_next);
3297                         CLAMP(fac, 0.0f, 1.0f);
3298                         w[i_curr] = 1.0f - fac;
3299                         w[i_next] = fac;
3300                 }
3301         }
3302         else {
3303                 if (totweight != 0.0f) {
3304                         for (i_curr = 0; i_curr < n; i_curr++) {
3305                                 w[i_curr] /= totweight;
3306                         }
3307                 }
3308         }
3309 }
3310
3311 #undef IS_POINT_IX
3312 #undef IS_SEGMENT_IX
3313
3314 #undef DIR_V3_SET
3315 #undef DIR_V2_SET
3316
3317 /** \} */
3318
3319
3320 /* (x1, v1)(t1=0)------(x2, v2)(t2=1), 0<t<1 --> (x, v)(t) */
3321 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)
3322 {
3323         float a[3], b[3];
3324         const float t2 = t * t;
3325         const float t3 = t2 * t;
3326
3327         /* cubic interpolation */
3328         a[0] = v1[0] + v2[0] + 2 * (x1[0] - x2[0]);
3329         a[1] = v1[1] + v2[1] + 2 * (x1[1] - x2[1]);
3330         a[2] = v1[2] + v2[2] + 2 * (x1[2] - x2[2]);
3331
3332         b[0] = -2 * v1[0] - v2[0] - 3 * (x1[0] - x2[0]);
3333         b[1] = -2 * v1[1] - v2[1] - 3 * (x1[1] - x2[1]);
3334         b[2] = -2 * v1[2] - v2[2] - 3 * (x1[2] - x2[2]);
3335
3336         x[0] = a[0] * t3 + b[0] * t2 + v1[0] * t + x1[0];
3337         x[1] = a[1] * t3 + b[1] * t2 + v1[1] * t + x1[1];
3338         x[2] = a[2] * t3 + b[2] * t2 + v1[2] * t + x1[2];
3339
3340         v[0] = 3 * a[0] * t2 + 2 * b[0] * t + v1[0];
3341         v[1] = 3 * a[1] * t2 + 2 * b[1] * t + v1[1];
3342         v[2] = 3 * a[2] * t2 + 2 * b[2] * t + v1[2];
3343 }
3344
3345 /* unfortunately internal calculations have to be done at double precision to achieve correct/stable results. */
3346
3347 #define IS_ZERO(x) ((x > (-DBL_EPSILON) && x < DBL_EPSILON) ? 1 : 0)
3348
3349 /**
3350  * Barycentric reverse
3351  *
3352  * Compute coordinates (u, v) for point \a st with respect to triangle (\a st0, \a st1, \a st2)
3353  *
3354  * \note same basic result as #barycentric_weights_v2, see it's comment for details.
3355  */
3356 void resolve_tri_uv_v2(float r_uv[2], const float st[2],
3357                        const float st0[2], const float st1[2], const float st2[2])
3358 {
3359         /* find UV such that
3360          * t = u * t0 + v * t1 + (1 - u - v) * t2
3361          * u * (t0 - t2) + v * (t1 - t2) = t - t2 */
3362         const double a = st0[0] - st2[0], b = st1[0] - st2[0];
3363         const double c = st0[1] - st2[1], d = st1[1] - st2[1];
3364         const double det = a * d - c * b;
3365
3366         /* det should never be zero since the determinant is the signed ST area of the triangle. */
3367         if (IS_ZERO(det) == 0) {
3368                 const double x[2] = {st[0] - st2[0], st[1] - st2[1]};
3369
3370                 r_uv[0] = (float)((d * x[0] - b * x[1]) / det);
3371                 r_uv[1] = (float)(((-c) * x[0] + a * x[1]) / det);
3372         }
3373         else {
3374                 zero_v2(r_uv);
3375         }
3376 }
3377
3378 /**
3379  * Barycentric reverse 3d
3380  *
3381  * Compute coordinates (u, v) for point \a st with respect to triangle (\a st0, \a st1, \a st2)
3382  */
3383 void resolve_tri_uv_v3(float r_uv[2], const float st[3], const float st0[3], const float st1[3], const float st2[3])
3384 {
3385         float v0[3], v1[3], v2[3];
3386         double d00, d01, d11, d20, d21, det;
3387
3388         sub_v3_v3v3(v0, st1, st0);
3389         sub_v3_v3v3(v1, st2, st0);
3390         sub_v3_v3v3(v2, st, st0);
3391
3392         d00 = dot_v3v3(v0, v0);
3393         d01 = dot_v3v3(v0, v1);
3394         d11 = dot_v3v3(v1, v1);
3395         d20 = dot_v3v3(v2, v0);
3396         d21 = dot_v3v3(v2, v1);
3397
3398         det = d00 * d11 - d01 * d01;
3399
3400         /* det should never be zero since the determinant is the signed ST area of the triangle. */
3401         if (IS_ZERO(det) == 0) {
3402                 float w;
3403