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