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