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