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