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