Cleanup: comments (long lines) in blenlib
[blender.git] / source / blender / blenlib / intern / math_geom.c
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
17  * All rights reserved.
18  *
19  * The Original Code is: some of this file.
20  *
21  * */
22
23 /** \file
24  * \ingroup bli
25  */
26
27 #include "MEM_guardedalloc.h"
28
29 #include "BLI_math.h"
30 #include "BLI_math_bits.h"
31 #include "BLI_utildefines.h"
32
33 #include "BLI_strict_flags.h"
34
35 /********************************** Polygons *********************************/
36
37 void cross_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
38 {
39   float n1[3], n2[3];
40
41   n1[0] = v1[0] - v2[0];
42   n2[0] = v2[0] - v3[0];
43   n1[1] = v1[1] - v2[1];
44   n2[1] = v2[1] - v3[1];
45   n1[2] = v1[2] - v2[2];
46   n2[2] = v2[2] - v3[2];
47   n[0] = n1[1] * n2[2] - n1[2] * n2[1];
48   n[1] = n1[2] * n2[0] - n1[0] * n2[2];
49   n[2] = n1[0] * n2[1] - n1[1] * n2[0];
50 }
51
52 float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
53 {
54   float n1[3], n2[3];
55
56   n1[0] = v1[0] - v2[0];
57   n2[0] = v2[0] - v3[0];
58   n1[1] = v1[1] - v2[1];
59   n2[1] = v2[1] - v3[1];
60   n1[2] = v1[2] - v2[2];
61   n2[2] = v2[2] - v3[2];
62   n[0] = n1[1] * n2[2] - n1[2] * n2[1];
63   n[1] = n1[2] * n2[0] - n1[0] * n2[2];
64   n[2] = n1[0] * n2[1] - n1[1] * n2[0];
65
66   return normalize_v3(n);
67 }
68
69 float normal_quad_v3(
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 doesnt 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   f = 1.0f / a;
1965
1966   sub_v3_v3v3(s, ray_origin, v0);
1967
1968   cross_v3_v3v3(q, s, e1);
1969   *r_lambda = f * dot_v3v3(e2, q);
1970   if ((*r_lambda < 0.0f))
1971     return false;
1972
1973   u = f * dot_v3v3(s, p);
1974   v = f * dot_v3v3(ray_direction, q);
1975
1976   if (u > 0 && v > 0 && u + v > 1) {
1977     float t = (u + v - 1) / 2;
1978     du = u - t;
1979     dv = v - t;
1980   }
1981   else {
1982     if (u < 0)
1983       du = u;
1984     else if (u > 1)
1985       du = u - 1;
1986     else
1987       du = 0.0f;
1988
1989     if (v < 0)
1990       dv = v;
1991     else if (v > 1)
1992       dv = v - 1;
1993     else
1994       dv = 0.0f;
1995   }
1996
1997   mul_v3_fl(e1, du);
1998   mul_v3_fl(e2, dv);
1999
2000   if (len_squared_v3(e1) + len_squared_v3(e2) > threshold * threshold) {
2001     return false;
2002   }
2003
2004   if (r_uv) {
2005     r_uv[0] = u;
2006     r_uv[1] = v;
2007   }
2008
2009   return true;
2010 }
2011 #endif
2012
2013 bool isect_ray_seg_v2(const float ray_origin[2],
2014                       const float ray_direction[2],
2015                       const float v0[2],
2016                       const float v1[2],
2017                       float *r_lambda,
2018                       float *r_u)
2019 {
2020   float v0_local[2], v1_local[2];
2021   sub_v2_v2v2(v0_local, v0, ray_origin);
2022   sub_v2_v2v2(v1_local, v1, ray_origin);
2023
2024   float s10[2];
2025   float det;
2026
2027   sub_v2_v2v2(s10, v1_local, v0_local);
2028
2029   det = cross_v2v2(ray_direction, s10);
2030   if (det != 0.0f) {
2031     const float v = cross_v2v2(v0_local, v1_local);
2032     float p[2] = {(ray_direction[0] * v) / det, (ray_direction[1] * v) / det};
2033
2034     const float t = (dot_v2v2(p, ray_direction) / dot_v2v2(ray_direction, ray_direction));
2035     if ((t >= 0.0f) == 0) {
2036       return false;
2037     }
2038
2039     float h[2];
2040     sub_v2_v2v2(h, v1_local, p);
2041     const float u = (dot_v2v2(s10, h) / dot_v2v2(s10, s10));
2042     if ((u >= 0.0f && u <= 1.0f) == 0) {
2043       return false;
2044     }
2045
2046     if (r_lambda) {
2047       *r_lambda = t;
2048     }
2049     if (r_u) {
2050       *r_u = u;
2051     }
2052
2053     return true;
2054   }
2055
2056   return false;
2057 }
2058
2059 bool isect_ray_seg_v3(const float ray_origin[3],
2060                       const float ray_direction[3],
2061                       const float v0[3],
2062                       const float v1[3],
2063                       float *r_lambda)
2064 {
2065   float a[3], t[3], n[3];
2066   sub_v3_v3v3(a, v1, v0);
2067   sub_v3_v3v3(t, v0, ray_origin);
2068   cross_v3_v3v3(n, a, ray_direction);
2069   const float nlen = len_squared_v3(n);
2070
2071   if (nlen == 0.0f) {
2072     /* the lines are parallel.*/
2073     return false;
2074   }
2075
2076   float c[3], cray[3];
2077   sub_v3_v3v3(c, n, t);
2078   cross_v3_v3v3(cray, c, ray_direction);
2079
2080   *r_lambda = dot_v3v3(cray, n) / nlen;
2081
2082   return true;
2083 }
2084
2085 /**
2086  * Check if a point is behind all planes.
2087  */
2088 bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3])
2089 {
2090   int i;
2091
2092   for (i = 0; i < totplane; i++) {
2093     if (plane_point_side_v3(planes[i], p) > 0.0f) {
2094       return false;
2095     }
2096   }
2097
2098   return true;
2099 }
2100
2101 /**
2102  * Check if a point is in front all planes.
2103  * Same as isect_point_planes_v3 but with planes facing the opposite direction.
2104  */
2105 bool isect_point_planes_v3_negated(const float (*planes)[4], const int totplane, const float p[3])
2106 {
2107   for (int i = 0; i < totplane; i++) {
2108     if (plane_point_side_v3(planes[i], p) <= 0.0f) {
2109       return false;
2110     }
2111   }
2112
2113   return true;
2114 }
2115
2116 /**
2117  * Intersect line/plane.
2118  *
2119  * \param r_isect_co: The intersection point.
2120  * \param l1: The first point of the line.
2121  * \param l2: The second point of the line.
2122  * \param plane_co: A point on the plane to intersect with.
2123  * \param plane_no: The direction of the plane (does not need to be normalized).
2124  *
2125  * \note #line_plane_factor_v3() shares logic.
2126  */
2127 bool isect_line_plane_v3(float r_isect_co[3],
2128                          const float l1[3],
2129                          const float l2[3],
2130                          const float plane_co[3],
2131                          const float plane_no[3])
2132 {
2133   float u[3], h[3];
2134   float dot;
2135
2136   sub_v3_v3v3(u, l2, l1);
2137   sub_v3_v3v3(h, l1, plane_co);
2138   dot = dot_v3v3(plane_no, u);
2139
2140   if (fabsf(dot) > FLT_EPSILON) {
2141     float lambda = -dot_v3v3(plane_no, h) / dot;
2142     madd_v3_v3v3fl(r_isect_co, l1, u, lambda);
2143     return true;
2144   }
2145   else {
2146     /* The segment is parallel to plane */
2147     return false;
2148   }
2149 }
2150
2151 /**
2152  * Intersect three planes, return the point where all 3 meet.
2153  * See Graphics Gems 1 pg 305
2154  *
2155  * \param plane_a, plane_b, plane_c: Planes.
2156  * \param r_isect_co: The resulting intersection point.
2157  */
2158 bool isect_plane_plane_plane_v3(const float plane_a[4],
2159                                 const float plane_b[4],
2160                                 const float plane_c[4],
2161                                 float r_isect_co[3])
2162 {
2163   float det;
2164
2165   det = determinant_m3(UNPACK3(plane_a), UNPACK3(plane_b), UNPACK3(plane_c));
2166
2167   if (det != 0.0f) {
2168     float tmp[3];
2169
2170     /* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
2171      *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3] +
2172      *  plane_a.xyz.cross(plane_b.xyz) * -plane_c[3]) / det; */
2173
2174     cross_v3_v3v3(tmp, plane_c, plane_b);
2175     mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
2176
2177     cross_v3_v3v3(tmp, plane_a, plane_c);
2178     madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
2179
2180     cross_v3_v3v3(tmp, plane_b, plane_a);
2181     madd_v3_v3fl(r_isect_co, tmp, plane_c[3]);
2182
2183     mul_v3_fl(r_isect_co, 1.0f / det);
2184
2185     return true;
2186   }
2187   else {
2188     return false;
2189   }
2190 }
2191
2192 /**
2193  * Intersect two planes, return a point on the intersection and a vector
2194  * that runs on the direction of the intersection.
2195  * \note this is a slightly reduced version of #isect_plane_plane_plane_v3
2196  *
2197  * \param plane_a, plane_b: Planes.
2198  * \param r_isect_co: The resulting intersection point.
2199  * \param r_isect_no: The resulting vector of the intersection.
2200  *
2201  * \note \a r_isect_no isn't unit length.
2202  */
2203 bool isect_plane_plane_v3(const float plane_a[4],
2204                           const float plane_b[4],
2205                           float r_isect_co[3],
2206                           float r_isect_no[3])
2207 {
2208   float det, plane_c[3];
2209
2210   /* direction is simply the cross product */
2211   cross_v3_v3v3(plane_c, plane_a, plane_b);
2212
2213   /* in this case we don't need to use 'determinant_m3' */
2214   det = len_squared_v3(plane_c);
2215
2216   if (det != 0.0f) {
2217     float tmp[3];
2218
2219     /* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
2220      *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3]) / det; */
2221     cross_v3_v3v3(tmp, plane_c, plane_b);
2222     mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
2223
2224     cross_v3_v3v3(tmp, plane_a, plane_c);
2225     madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
2226
2227     mul_v3_fl(r_isect_co, 1.0f / det);
2228
2229     copy_v3_v3(r_isect_no, plane_c);
2230
2231     return true;
2232   }
2233   else {
2234     return false;
2235   }
2236 }
2237
2238 /**
2239  * Intersect two triangles.
2240  *
2241  * \param r_i1, r_i2: Optional arguments to retrieve the overlapping edge between the 2 triangles.
2242  * \return true when the triangles intersect.
2243  *
2244  * \note intersections between coplanar triangles are currently undetected.
2245  */
2246 bool isect_tri_tri_epsilon_v3(const float t_a0[3],
2247                               const float t_a1[3],
2248                               const float t_a2[3],
2249                               const float t_b0[3],
2250                               const float t_b1[3],
2251                               const float t_b2[3],
2252                               float r_i1[3],
2253                               float r_i2[3],
2254                               const float epsilon)
2255 {
2256   const float *tri_pair[2][3] = {{t_a0, t_a1, t_a2}, {t_b0, t_b1, t_b2}};
2257   float plane_a[4], plane_b[4];
2258   float plane_co[3], plane_no[3];
2259
2260   BLI_assert((r_i1 != NULL) == (r_i2 != NULL));
2261
2262   /* normalizing is needed for small triangles T46007 */
2263   normal_tri_v3(plane_a, UNPACK3(tri_pair[0]));
2264   normal_tri_v3(plane_b, UNPACK3(tri_pair[1]));
2265
2266   plane_a[3] = -dot_v3v3(plane_a, t_a0);
2267   plane_b[3] = -dot_v3v3(plane_b, t_b0);
2268
2269   if (isect_plane_plane_v3(plane_a, plane_b, plane_co, plane_no) &&
2270       (normalize_v3(plane_no) > epsilon)) {
2271     /**
2272      * Implementation note: its simpler to project the triangles onto the intersection plane
2273      * before intersecting their edges with the ray, defined by 'isect_plane_plane_v3'.
2274      * This way we can use 'line_point_factor_v3_ex' to see if an edge crosses 'co_proj',
2275      * then use the factor to calculate the world-space point.
2276      */
2277     struct {
2278       float min, max;
2279     } range[2] = {{FLT_MAX, -FLT_MAX}, {FLT_MAX, -FLT_MAX}};
2280     int t;
2281     float co_proj[3];
2282
2283     closest_to_plane3_normalized_v3(co_proj, plane_no, plane_co);
2284
2285     /* For both triangles, find the overlap with the line defined by the ray [co_proj, plane_no].
2286      * When the ranges overlap we know the triangles do too. */
2287     for (t = 0; t < 2; t++) {
2288       int j, j_prev;
2289       float tri_proj[3][3];
2290
2291       closest_to_plane3_normalized_v3(tri_proj[0], plane_no, tri_pair[t][0]);
2292       closest_to_plane3_normalized_v3(tri_proj[1], plane_no, tri_pair[t][1]);
2293       closest_to_plane3_normalized_v3(tri_proj[2], plane_no, tri_pair[t][2]);
2294
2295       for (j = 0, j_prev = 2; j < 3; j_prev = j++) {
2296         /* note that its important to have a very small nonzero epsilon here
2297          * otherwise this fails for very small faces.
2298          * However if its too small, large adjacent faces will count as intersecting */
2299         const float edge_fac = line_point_factor_v3_ex(
2300             co_proj, tri_proj[j_prev], tri_proj[j], 1e-10f, -1.0f);
2301         /* ignore collinear lines, they are either an edge shared between 2 tri's
2302          * (which runs along [co_proj, plane_no], but can be safely ignored).
2303          *
2304          * or a collinear edge placed away from the ray -
2305          * which we don't intersect with & can ignore. */
2306         if (UNLIKELY(edge_fac == -1.0f)) {
2307           /* pass */
2308         }
2309         else if (edge_fac > 0.0f && edge_fac < 1.0f) {
2310           float ix_tri[3];
2311           float span_fac;
2312
2313           interp_v3_v3v3(ix_tri, tri_pair[t][j_prev], tri_pair[t][j], edge_fac);
2314           /* the actual distance, since 'plane_no' is normalized */
2315           span_fac = dot_v3v3(plane_no, ix_tri);
2316
2317           range[t].min = min_ff(range[t].min, span_fac);
2318           range[t].max = max_ff(range[t].max, span_fac);
2319         }
2320       }
2321
2322       if (range[t].min == FLT_MAX) {
2323         return false;
2324       }
2325     }
2326
2327     if (((range[0].min > range[1].max) || (range[0].max < range[1].min)) == 0) {
2328       if (r_i1 && r_i2) {
2329         project_plane_normalized_v3_v3v3(plane_co, plane_co, plane_no);
2330         madd_v3_v3v3fl(r_i1, plane_co, plane_no, max_ff(range[0].min, range[1].min));
2331         madd_v3_v3v3fl(r_i2, plane_co, plane_no, min_ff(range[0].max, range[1].max));
2332       }
2333
2334       return true;
2335     }
2336   }
2337
2338   return false;
2339 }
2340
2341 /* Adapted from the paper by Kasper Fauerby */
2342
2343 /* "Improved Collision detection and Response" */
2344 static bool getLowestRoot(
2345     const float a, const float b, const float c, const float maxR, float *root)
2346 {
2347   /* Check if a solution exists */
2348   const float determinant = b * b - 4.0f * a * c;
2349
2350   /* If determinant is negative it means no solutions. */
2351   if (determinant >= 0.0f) {
2352     /* calculate the two roots: (if determinant == 0 then
2353      * x1==x2 but lets disregard that slight optimization) */
2354     const float sqrtD = sqrtf(determinant);
2355     float r1 = (-b - sqrtD) / (2.0f * a);
2356     float r2 = (-b + sqrtD) / (2.0f * a);
2357
2358     /* Sort so x1 <= x2 */
2359     if (r1 > r2) {
2360       SWAP(float, r1, r2);
2361     }
2362
2363     /* Get lowest root: */
2364     if (r1 > 0.0f && r1 < maxR) {
2365       *root = r1;
2366       return true;
2367     }
2368
2369     /* It is possible that we want x2 - this can happen */
2370     /* if x1 < 0 */
2371     if (r2 > 0.0f && r2 < maxR) {
2372       *root = r2;
2373       return true;
2374     }
2375   }
2376   /* No (valid) solutions */
2377   return false;
2378 }
2379
2380 /**
2381  * Checks status of an AABB in relation to a list of planes.
2382  *
2383  * \returns intersection type:
2384  * - ISECT_AABB_PLANE_BEHIND_ONE   (0): AABB is completely behind at least 1 plane;
2385  * - ISECT_AABB_PLANE_CROSS_ANY    (1): AABB intersects at least 1 plane;
2386  * - ISECT_AABB_PLANE_IN_FRONT_ALL (2): AABB is completely in front of all planes;
2387  */
2388 int isect_aabb_planes_v3(const float (*planes)[4],
2389                          const int totplane,
2390                          const float bbmin[3],
2391                          const float bbmax[3])
2392 {
2393   int ret = ISECT_AABB_PLANE_IN_FRONT_ALL;
2394
2395   float bb_near[3], bb_far[3];
2396   for (int i = 0; i < totplane; i++) {
2397     aabb_get_near_far_from_plane(planes[i], bbmin, bbmax, bb_near, bb_far);
2398
2399     if (plane_point_side_v3(planes[i], bb_far) < 0.0f) {
2400       return ISECT_AABB_PLANE_BEHIND_ANY;
2401     }
2402     else if ((ret != ISECT_AABB_PLANE_CROSS_ANY) &&
2403              (plane_point_side_v3(planes[i], bb_near) < 0.0f)) {
2404       ret = ISECT_AABB_PLANE_CROSS_ANY;
2405     }
2406   }
2407
2408   return ret;
2409 }
2410
2411 bool isect_sweeping_sphere_tri_v3(const float p1[3],
2412                                   const float p2[3],
2413                                   const float radius,
2414                                   const float v0[3],
2415                                   const float v1[3],
2416                                   const float v2[3],
2417                                   float *r_lambda,
2418                                   float ipoint[3])
2419 {
2420   float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
2421   float a, b, c, d, e, x, y, z, radius2 = radius * radius;
2422   float elen2, edotv, edotbv, nordotv;
2423   float newLambda;
2424   bool found_by_sweep = false;
2425
2426   sub_v3_v3v3(e1, v1, v0);
2427   sub_v3_v3v3(e2, v2, v0);
2428   sub_v3_v3v3(vel, p2, p1);
2429
2430   /*---test plane of tri---*/
2431   cross_v3_v3v3(nor, e1, e2);
2432   normalize_v3(nor);
2433
2434   /* flip normal */
2435   if (dot_v3v3(nor, vel) > 0.0f) {
2436     negate_v3(nor);
2437   }
2438
2439   a = dot_v3v3(p1, nor) - dot_v3v3(v0, nor);
2440   nordotv = dot_v3v3(nor, vel);
2441
2442   if (fabsf(nordotv) < 0.000001f) {
2443     if (fabsf(a) >= radius) {
2444       return false;
2445     }
2446   }
2447   else {
2448     float t0 = (-a + radius) / nordotv;
2449     float t1 = (-a - radius) / nordotv;
2450
2451     if (t0 > t1) {
2452       SWAP(float, t0, t1);
2453     }
2454
2455     if (t0 > 1.0f || t1 < 0.0f) {
2456       return false;
2457     }
2458
2459     /* clamp to [0, 1] */
2460     CLAMP(t0, 0.0f, 1.0f);
2461     CLAMP(t1, 0.0f, 1.0f);
2462
2463     /*---test inside of tri---*/
2464     /* plane intersection point */
2465
2466     point[0] = p1[0] + vel[0] * t0 - nor[0] * radius;
2467     point[1] = p1[1] + vel[1] * t0 - nor[1] * radius;
2468     point[2] = p1[2] + vel[2] * t0 - nor[2] * radius;
2469
2470     /* is the point in the tri? */
2471     a = dot_v3v3(e1, e1);
2472     b = dot_v3v3(e1, e2);
2473     c = dot_v3v3(e2, e2);
2474
2475     sub_v3_v3v3(temp, point, v0);
2476     d = dot_v3v3(temp, e1);
2477     e = dot_v3v3(temp, e2);
2478
2479     x = d * c - e * b;
2480     y = e * a - d * b;
2481     z = x + y - (a * c - b * b);
2482
2483     if (z <= 0.0f && (x >= 0.0f && y >= 0.0f)) {
2484       //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000) {
2485       *r_lambda = t0;
2486       copy_v3_v3(ipoint, point);
2487       return true;
2488     }
2489   }
2490
2491   *r_lambda = 1.0f;
2492
2493   /*---test points---*/
2494   a = dot_v3v3(vel, vel);
2495
2496   /*v0*/
2497   sub_v3_v3v3(temp, p1, v0);
2498   b = 2.0f * dot_v3v3(vel, temp);
2499   c = dot_v3v3(temp, temp) - radius2;
2500
2501   if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
2502     copy_v3_v3(ipoint, v0);
2503     found_by_sweep = true;
2504   }
2505
2506   /*v1*/
2507   sub_v3_v3v3(temp, p1, v1);
2508   b = 2.0f * dot_v3v3(vel, temp);
2509   c = dot_v3v3(temp, temp) - radius2;
2510
2511   if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
2512     copy_v3_v3(ipoint, v1);
2513     found_by_sweep = true;
2514   }
2515
2516   /*v2*/
2517   sub_v3_v3v3(temp, p1, v2);
2518   b = 2.0f * dot_v3v3(vel, temp);
2519   c = dot_v3v3(temp, temp) - radius2;
2520
2521   if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) {
2522     copy_v3_v3(ipoint, v2);
2523     found_by_sweep = true;
2524   }
2525
2526   /*---test edges---*/
2527   sub_v3_v3v3(e3, v2, v1); /* wasnt yet calculated */
2528
2529   /*e1*/
2530   sub_v3_v3v3(bv, v0, p1);
2531
2532   elen2 = dot_v3v3(e1, e1);
2533   edotv = dot_v3v3(e1, vel);
2534   edotbv = dot_v3v3(e1, bv);
2535
2536   a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2537   b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2538   c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2539
2540   if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2541     e = (edotv * newLambda - edotbv) / elen2;
2542
2543     if (e >= 0.0f && e <= 1.0f) {
2544       *r_lambda = newLambda;
2545       copy_v3_v3(ipoint, e1);
2546       mul_v3_fl(ipoint, e);
2547       add_v3_v3(ipoint, v0);
2548       found_by_sweep = true;
2549     }
2550   }
2551
2552   /*e2*/
2553   /*bv is same*/
2554   elen2 = dot_v3v3(e2, e2);
2555   edotv = dot_v3v3(e2, vel);
2556   edotbv = dot_v3v3(e2, bv);
2557
2558   a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2559   b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2560   c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2561
2562   if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2563     e = (edotv * newLambda - edotbv) / elen2;
2564
2565     if (e >= 0.0f && e <= 1.0f) {
2566       *r_lambda = newLambda;
2567       copy_v3_v3(ipoint, e2);
2568       mul_v3_fl(ipoint, e);
2569       add_v3_v3(ipoint, v0);
2570       found_by_sweep = true;
2571     }
2572   }
2573
2574   /*e3*/
2575   /* sub_v3_v3v3(bv, v0, p1); */   /* UNUSED */
2576   /* elen2 = dot_v3v3(e1, e1); */  /* UNUSED */
2577   /* edotv = dot_v3v3(e1, vel); */ /* UNUSED */
2578   /* edotbv = dot_v3v3(e1, bv); */ /* UNUSED */
2579
2580   sub_v3_v3v3(bv, v1, p1);
2581   elen2 = dot_v3v3(e3, e3);
2582   edotv = dot_v3v3(e3, vel);
2583   edotbv = dot_v3v3(e3, bv);
2584
2585   a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv;
2586   b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv);
2587   c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv;
2588
2589   if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) {
2590     e = (edotv * newLambda - edotbv) / elen2;
2591
2592     if (e >= 0.0f && e <= 1.0f) {
2593       *r_lambda = newLambda;
2594       copy_v3_v3(ipoint, e3);
2595       mul_v3_fl(ipoint, e);
2596       add_v3_v3(ipoint, v1);
2597       found_by_sweep = true;
2598     }
2599   }
2600
2601   return found_by_sweep;
2602 }
2603
2604 bool isect_axial_line_segment_tri_v3(const int axis,
2605                                      const float p1[3],
2606                                      const float p2[3],
2607                                      const float v0[3],
2608                                      const float v1[3],
2609                                      const float v2[3],
2610                                      float *r_lambda)
2611 {
2612   const float epsilon = 0.000001f;
2613   float p[3], e1[3], e2[3];
2614   float u, v, f;
2615   int a0 = axis, a1 = (axis + 1) % 3, a2 = (axis + 2) % 3;
2616
2617   sub_v3_v3v3(e1, v1, v0);
2618   sub_v3_v3v3(e2, v2, v0);
2619   sub_v3_v3v3(p, v0, p1);
2620
2621   f = (e2[a1] * e1[a2] - e2[a2] * e1[a1]);
2622   if ((f > -epsilon) && (f < epsilon)) {
2623     return false;
2624   }
2625
2626   v = (p[a2] * e1[a1] - p[a1] * e1[a2]) / f;
2627   if ((v < 0.0f) || (v > 1.0f)) {
2628     return false;
2629   }
2630
2631   f = e1[a1];
2632   if ((f > -epsilon) && (f < epsilon)) {
2633     f = e1[a2];
2634     if ((f > -epsilon) && (f < epsilon)) {
2635       return false;
2636     }
2637     u = (-p[a2] - v * e2[a2]) / f;
2638   }
2639   else {
2640     u = (-p[a1] - v * e2[a1]) / f;
2641   }
2642
2643   if ((u < 0.0f) || ((u + v) > 1.0f)) {
2644     return false;
2645   }
2646
2647   *r_lambda = (p[a0] + u * e1[a0] + v * e2[a0]) / (p2[a0] - p1[a0]);
2648
2649   if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) {
2650     return false;
2651   }
2652
2653   return true;
2654 }
2655
2656 /**
2657  * \return The number of point of interests
2658  * 0 - lines are collinear
2659  * 1 - lines are coplanar, i1 is set to intersection
2660  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively
2661  */
2662 int isect_line_line_epsilon_v3(const float v1[3],
2663                                const float v2[3],
2664                                const float v3[3],
2665                                const float v4[3],
2666                                float r_i1[3],
2667                                float r_i2[3],
2668                                const float epsilon)
2669 {
2670   float a[3], b[3], c[3], ab[3], cb[3];
2671   float d, div;
2672
2673   sub_v3_v3v3(c, v3, v1);
2674   sub_v3_v3v3(a, v2, v1);
2675   sub_v3_v3v3(b, v4, v3);
2676
2677   cross_v3_v3v3(ab, a, b);
2678   d = dot_v3v3(c, ab);
2679   div = dot_v3v3(ab, ab);
2680
2681   /* important not to use an epsilon here, see: T45919 */
2682   /* test zero length line */
2683   if (UNLIKELY(div == 0.0f)) {
2684     return 0;
2685   }
2686   /* test if the two lines are coplanar */
2687   else if (UNLIKELY(fabsf(d) <= epsilon)) {
2688     cross_v3_v3v3(cb, c, b);
2689
2690     mul_v3_fl(a, dot_v3v3(cb, ab) / div);
2691     add_v3_v3v3(r_i1, v1, a);
2692     copy_v3_v3(r_i2, r_i1);
2693
2694     return 1; /* one intersection only */
2695   }
2696   /* if not */
2697   else {
2698     float n[3], t[3];
2699     float v3t[3], v4t[3];
2700     sub_v3_v3v3(t, v1, v3);
2701
2702     /* offset between both plane where the lines lies */
2703     cross_v3_v3v3(n, a, b);
2704     project_v3_v3v3(t, t, n);
2705
2706     /* for the first line, offset the second line until it is coplanar */
2707     add_v3_v3v3(v3t, v3, t);
2708     add_v3_v3v3(v4t, v4, t);
2709
2710     sub_v3_v3v3(c, v3t, v1);
2711     sub_v3_v3v3(a, v2, v1);
2712     sub_v3_v3v3(b, v4t, v3t);
2713
2714     cross_v3_v3v3(ab, a, b);
2715     cross_v3_v3v3(cb, c, b);
2716
2717     mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
2718     add_v3_v3v3(r_i1, v1, a);
2719
2720     /* for the second line, just substract the offset from the first intersection point */
2721     sub_v3_v3v3(r_i2, r_i1, t);
2722
2723     return 2; /* two nearest points */
2724   }
2725 }
2726
2727 int isect_line_line_v3(const float v1[3],
2728                        const float v2[3],
2729                        const float v3[3],
2730                        const float v4[3],
2731                        float r_i1[3],
2732                        float r_i2[3])
2733 {
2734   const float epsilon = 0.000001f;
2735   return isect_line_line_epsilon_v3(v1, v2, v3, v4, r_i1, r_i2, epsilon);
2736 }
2737
2738 /** Intersection point strictly between the two lines
2739  * \return false when no intersection is found
2740  */
2741 bool isect_line_line_strict_v3(const float v1[3],
2742                                const float v2[3],
2743                                const float v3[3],
2744                                const float v4[3],
2745                                float vi[3],
2746                                float *r_lambda)
2747 {
2748   const float epsilon = 0.000001f;
2749   float a[3], b[3], c[3], ab[3], cb[3], ca[3];
2750   float d, div;
2751
2752   sub_v3_v3v3(c, v3, v1);
2753   sub_v3_v3v3(a, v2, v1);
2754   sub_v3_v3v3(b, v4, v3);
2755
2756   cross_v3_v3v3(ab, a, b);
2757   d = dot_v3v3(c, ab);
2758   div = dot_v3v3(ab, ab);
2759
2760   /* important not to use an epsilon here, see: T45919 */
2761   /* test zero length line */
2762   if (UNLIKELY(div == 0.0f)) {
2763     return false;
2764   }
2765   /* test if the two lines are coplanar */
2766   else if (UNLIKELY(fabsf(d) < epsilon)) {
2767     return false;
2768   }
2769   else {
2770     float f1, f2;
2771     cross_v3_v3v3(cb, c, b);
2772     cross_v3_v3v3(ca, c, a);
2773
2774     f1 = dot_v3v3(cb, ab) / div;
2775     f2 = dot_v3v3(ca, ab) / div;
2776
2777     if (f1 >= 0 && f1 <= 1 && f2 >= 0 && f2 <= 1) {
2778       mul_v3_fl(a, f1);
2779       add_v3_v3v3(vi, v1, a);
2780
2781       if (r_lambda) {
2782         *r_lambda = f1;
2783       }
2784
2785       return true; /* intersection found */
2786     }
2787     else {
2788       return false;
2789     }
2790   }
2791 }
2792
2793 bool isect_aabb_aabb_v3(const float min1[3],
2794                         const float max1[3],
2795                         const float min2[3],
2796                         const float max2[3])
2797 {
2798   return (min1[0] < max2[0] && min1[1] < max2[1] && min1[2] < max2[2] && min2[0] < max1[0] &&
2799           min2[1] < max1[1] && min2[2] < max1[2]);
2800 }
2801
2802 void isect_ray_aabb_v3_precalc(struct IsectRayAABB_Precalc *data,
2803                                const float ray_origin[3],
2804                                const float ray_direction[3])
2805 {
2806   copy_v3_v3(data->ray_origin, ray_origin);
2807
2808   data->ray_inv_dir[0] = 1.0f / ray_direction[0];
2809   data->ray_inv_dir[1] = 1.0f / ray_direction[1];
2810   data->ray_inv_dir[2] = 1.0f / ray_direction[2];
2811
2812   data->sign[0] = data->ray_inv_dir[0] < 0.0f;
2813   data->sign[1] = data->ray_inv_dir[1] < 0.0f;
2814   data->sign[2] = data->ray_inv_dir[2] < 0.0f;
2815 }
2816
2817 /* Adapted from http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
2818 bool isect_ray_aabb_v3(const struct IsectRayAABB_Precalc *data,
2819                        const float bb_min[3],
2820                        const float bb_max[3],
2821                        float *tmin_out)
2822 {
2823   float bbox[2][3];
2824
2825   copy_v3_v3(bbox[0], bb_min);
2826   copy_v3_v3(bbox[1], bb_max);
2827
2828   float tmin = (bbox[data->sign[0]][0] - data->ray_origin[0]) * data->ray_inv_dir[0];
2829   float tmax = (bbox[1 - data->sign[0]][0] - data->ray_origin[0]) * data->ray_inv_dir[0];
2830
2831   const float tymin = (bbox[data->sign[1]][1] - data->ray_origin[1]) * data->ray_inv_dir[1];
2832   const float tymax = (bbox[1 - data->sign[1]][1] - data->ray_origin[1]) * data->ray_inv_dir[1];
2833
2834   if ((tmin > tymax) || (tymin > tmax)) {
2835     return false;
2836   }
2837
2838   if (tymin > tmin) {
2839     tmin = tymin;
2840   }
2841
2842   if (tymax < tmax) {
2843     tmax = tymax;
2844   }
2845
2846   const float tzmin = (bbox[data->sign[2]][2] - data->ray_origin[2]) * data->ray_inv_dir[2];
2847   const float tzmax = (bbox[1 - data->sign[2]][2] - data->ray_origin[2]) * data->ray_inv_dir[2];
2848
2849   if ((tmin > tzmax) || (tzmin > tmax)) {
2850     return false;
2851   }
2852
2853   if (tzmin > tmin) {
2854     tmin = tzmin;
2855   }
2856
2857   /* Note: tmax does not need to be updated since we don't use it
2858    * keeping this here for future reference - jwilkins */
2859   //if (tzmax < tmax) tmax = tzmax;
2860
2861   if (tmin_out) {
2862     (*tmin_out) = tmin;
2863   }
2864
2865   return true;
2866 }
2867
2868 /**
2869  * Test a bounding box (AABB) for ray intersection.
2870  * Assumes the ray is already local to the boundbox space.
2871  *
2872  * \note: \a direction should be normalized
2873  * if you intend to use the \a tmin or \a tmax distance results!
2874  */
2875 bool isect_ray_aabb_v3_simple(const float orig[3],
2876                               const float dir[3],
2877                               const float bb_min[3],
2878                               const float bb_max[3],
2879                               float *tmin,
2880                               float *tmax)
2881 {
2882   double t[6];
2883   float hit_dist[2];
2884   const double invdirx = (dir[0] > 1e-35f || dir[0] < -1e-35f) ? 1.0 / (double)dir[0] : DBL_MAX;
2885   const double invdiry = (dir[1] > 1e-35f || dir[1] < -1e-35f) ? 1.0 / (double)dir[1] : DBL_MAX;
2886   const double invdirz = (dir[2] > 1e-35f || dir[2] < -1e-35f) ? 1.0 / (double)dir[2] : DBL_MAX;
2887   t[0] = (double)(bb_min[0] - orig[0]) * invdirx;
2888   t[1] = (double)(bb_max[0] - orig[0]) * invdirx;
2889   t[2] = (double)(bb_min[1] - orig[1]) * invdiry;
2890   t[3] = (double)(bb_max[1] - orig[1]) * invdiry;
2891   t[4] = (double)(bb_min[2] - orig[2]) * invdirz;
2892   t[5] = (double)(bb_max[2] - orig[2]) * invdirz;
2893   hit_dist[0] = (float)fmax(fmax(fmin(t[0], t[1]), fmin(t[2], t[3])), fmin(t[4], t[5]));
2894   hit_dist[1] = (float)fmin(fmin(fmax(t[0], t[1]), fmax(t[2], t[3])), fmax(t[4], t[5]));
2895   if ((hit_dist[1] < 0.0f || hit_dist[0] > hit_dist[1])) {
2896     return false;
2897   }
2898   else {
2899     if (tmin) {
2900       *tmin = hit_dist[0];
2901     }
2902     if (tmax) {
2903       *tmax = hit_dist[1];
2904     }
2905     return true;
2906   }
2907 }
2908
2909 /* find closest point to p on line through (l1, l2) and return lambda,
2910  * where (0 <= lambda <= 1) when cp is in the line segment (l1, l2)
2911  */
2912 float closest_to_line_v3(float r_close[3], const float p[3], const float l1[3], const float l2[3])
2913 {
2914   float h[3], u[3], lambda;
2915   sub_v3_v3v3(u, l2, l1);
2916   sub_v3_v3v3(h, p, l1);
2917   lambda = dot_v3v3(u, h) / dot_v3v3(u, u);
2918   r_close[0] = l1[0] + u[0] * lambda;
2919   r_close[1] = l1[1] + u[1] * lambda;
2920   r_close[2] = l1[2] + u[2] * lambda;
2921   return lambda;
2922 }
2923
2924 float closest_to_line_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2])
2925 {
2926   float h[2], u[2], lambda;
2927   sub_v2_v2v2(u, l2, l1);
2928   sub_v2_v2v2(h, p, l1);
2929   lambda = dot_v2v2(u, h) / dot_v2v2(u, u);
2930   r_close[0] = l1[0] + u[0] * lambda;
2931   r_close[1] = l1[1] + u[1] * lambda;
2932   return lambda;
2933 }
2934
2935 float ray_point_factor_v3_ex(const float p[3],
2936                              const float ray_origin[3],
2937                              const float ray_direction[3],
2938                              const float epsilon,
2939                              const float fallback)
2940 {
2941   float p_relative[3];
2942   sub_v3_v3v3(p_relative, p, ray_origin);
2943   const float dot = len_squared_v3(ray_direction);
2944   return (dot > epsilon) ? (dot_v3v3(ray_direction, p_relative) / dot) : fallback;
2945 }
2946
2947 float ray_point_factor_v3(const float p[3],
2948                           const float ray_origin[3],
2949                           const float ray_direction[3])
2950 {
2951   return ray_point_factor_v3_ex(p, ray_origin, ray_direction, 0.0f, 0.0f);
2952 }
2953
2954 /**
2955  * A simplified version of #closest_to_line_v3
2956  * we only need to return the ``lambda``
2957  *
2958  * \param epsilon: avoid approaching divide-by-zero.
2959  * Passing a zero will just check for nonzero division.
2960  */
2961 float line_point_factor_v3_ex(const float p[3],
2962                               const float l1[3],
2963                               const float l2[3],
2964                               const float epsilon,
2965                               const float fallback)
2966 {
2967   float h[3], u[3];
2968   float dot;
2969   sub_v3_v3v3(u, l2, l1);
2970   sub_v3_v3v3(h, p, l1);
2971
2972   /* better check for zero */
2973   dot = len_squared_v3(u);
2974   return (dot > epsilon) ? (dot_v3v3(u, h) / dot) : fallback;
2975 }
2976 float line_point_factor_v3(const float p[3], const float l1[3], const float l2[3])
2977 {
2978   return line_point_factor_v3_ex(p, l1, l2, 0.0f, 0.0f);
2979 }
2980
2981 float line_point_factor_v2_ex(const float p[2],
2982                               const float l1[2],
2983                               const float l2[2],
2984                               const float epsilon,
2985                               const float fallback)
2986 {
2987   float h[2], u[2];
2988   float dot;
2989   sub_v2_v2v2(u, l2, l1);
2990   sub_v2_v2v2(h, p, l1);
2991   /* better check for zero */
2992   dot = len_squared_v2(u);
2993   return (dot > epsilon) ? (dot_v2v2(u, h) / dot) : fallback;
2994 }
2995
2996 float line_point_factor_v2(const float p[2], const float l1[2], const float l2[2])
2997 {
2998   return line_point_factor_v2_ex(p, l1, l2, 0.0f, 0.0f);
2999 }
3000
3001 /**
3002  * \note #isect_line_plane_v3() shares logic
3003  */
3004 float line_plane_factor_v3(const float plane_co[3],
3005                            const float plane_no[3],
3006                            const float l1[3],
3007                            const float l2[3])
3008 {
3009   float u[3], h[3];
3010   float dot;
3011   sub_v3_v3v3(u, l2, l1);
3012   sub_v3_v3v3(h, l1, plane_co);
3013   dot = dot_v3v3(plane_no, u);
3014   return (dot != 0.0f) ? -dot_v3v3(plane_no, h) / dot : 0.0f;
3015 }
3016
3017 /**
3018  * Ensure the distance between these points is no greater than 'dist'.
3019  * If it is, scale then both into the center.
3020  */
3021 void limit_dist_v3(float v1[3], float v2[3], const float dist)
3022 {
3023   const float dist_old = len_v3v3(v1, v2);
3024
3025   if (dist_old > dist) {
3026     float v1_old[3];
3027     float v2_old[3];
3028     float fac = (dist / dist_old) * 0.5f;
3029
3030     copy_v3_v3(v1_old, v1);
3031     copy_v3_v3(v2_old, v2);
3032
3033     interp_v3_v3v3(v1, v1_old, v2_old, 0.5f - fac);
3034     interp_v3_v3v3(v2, v1_old, v2_old, 0.5f + fac);
3035   }
3036 }
3037
3038 /*
3039  *     x1,y2
3040  *     |  \
3041  *     |   \     .(a,b)
3042  *     |    \
3043  *     x1,y1-- x2,y1
3044  */
3045 int isect_point_tri_v2_int(
3046     const int x1, const int y1, const int x2, const int y2, const int a, const int b)
3047 {
3048   float v1[2], v2[2], v3[2], p[2];
3049
3050   v1[0] = (float)x1;
3051   v1[1] = (float)y1;
3052
3053   v2[0] = (float)x1;
3054   v2[1] = (float)y2;
3055
3056   v3[0] = (float)x2;
3057   v3[1] = (float)y1;
3058
3059   p[0] = (float)a;
3060   p[1] = (float)b;
3061
3062   return isect_point_tri_v2(p, v1, v2, v3);
3063 }
3064
3065 static bool point_in_slice(const float p[3],
3066                            const float v1[3],
3067                            const float l1[3],
3068                            const float l2[3])
3069 {
3070   /*
3071    * what is a slice ?
3072    * some maths:
3073    * a line including (l1, l2) and a point not on the line
3074    * define a subset of R3 delimited by planes parallel to the line and orthogonal
3075    * to the (point --> line) distance vector, one plane on the line one on the point,
3076    * the room inside usually is rather small compared to R3 though still infinite
3077    * useful for restricting (speeding up) searches
3078    * e.g. all points of triangular prism are within the intersection of 3 'slices'
3079    * another trivial case : cube
3080    * but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
3081    */
3082   float h, rp[3], cp[3], q[3];
3083
3084   closest_to_line_v3(cp, v1, l1, l2);
3085   sub_v3_v3v3(q, cp, v1);
3086
3087   sub_v3_v3v3(rp, p, v1);
3088   h = dot_v3v3(q, rp) / dot_v3v3(q, q);
3089   /* note: when 'h' is nan/-nan, this check returns false
3090    * without explicit check - covering the degenerate case */
3091   return (h >= 0.0f && h <= 1.0f);
3092 }
3093
3094 /* adult sister defining the slice planes by the origin and the normal
3095  * NOTE |normal| may not be 1 but defining the thickness of the slice */
3096 static bool point_in_slice_as(float p[3], float origin[3], float normal[3])
3097 {
3098   float h, rp[3];
3099   sub_v3_v3v3(rp, p, origin);
3100   h = dot_v3v3(normal, rp) / dot_v3v3(normal, normal);
3101   if (h < 0.0f || h > 1.0f) {
3102     return false;
3103   }
3104   return true;
3105 }
3106
3107 bool point_in_slice_seg(float p[3], float l1[3], float l2[3])
3108 {
3109   float normal[3];
3110
3111   sub_v3_v3v3(normal, l2, l1);
3112
3113   return point_in_slice_as(p, l1, normal);
3114 }
3115
3116 bool isect_point_tri_prism_v3(const float p[3],
3117                               const float v1[3],
3118                               const float v2[3],
3119                               const float v3[3])
3120 {
3121   if (!point_in_slice(p, v1, v2, v3)) {
3122     return false;
3123   }
3124   if (!point_in_slice(p, v2, v3, v1)) {
3125     return false;
3126   }
3127   if (!point_in_slice(p, v3, v1, v2)) {
3128     return false;
3129   }
3130   return true;
3131 }
3132
3133 /**
3134  * \param r_isect_co: The point \a p projected onto the triangle.
3135  * \return True when \a p is inside the triangle.
3136  * \note Its up to the caller to check the distance between \a p and \a r_vi
3137  * against an error margin.
3138  */
3139 bool isect_point_tri_v3(
3140     const float p[3], const float v1[3], const float v2[3], const float v3[3], float r_isect_co[3])
3141 {
3142   if (isect_point_tri_prism_v3(p, v1, v2, v3)) {
3143     float plane[4];
3144     float no[3];
3145
3146     /* Could use normal_tri_v3, but doesn't have to be unit-length */
3147     cross_tri_v3(no, v1, v2, v3);
3148     BLI_assert(len_squared_v3(no) != 0.0f);
3149
3150     plane_from_point_normal_v3(plane, v1, no);
3151     closest_to_plane_v3(r_isect_co, plane, p);
3152
3153     return true;
3154   }
3155   else {
3156     return false;
3157   }
3158 }
3159
3160 bool clip_segment_v3_plane(
3161     const float p1[3], const float p2[3], const float plane[4], float r_p1[3], float r_p2[3])
3162 {
3163   float dp[3], div;
3164
3165   sub_v3_v3v3(dp, p2, p1);
3166   div = dot_v3v3(dp, plane);
3167
3168   if (div == 0.0f) {
3169     /* parallel */
3170     return true;
3171   }
3172
3173   float t = -plane_point_side_v3(plane, p1);
3174
3175   if (div > 0.0f) {
3176     /* behind plane, completely clipped */
3177     if (t >= div) {
3178       return false;
3179     }
3180     else if (t > 0.0f) {
3181       const float p1_copy[3] = {UNPACK3(p1)};
3182       copy_v3_v3(r_p2, p2);
3183       madd_v3_v3v3fl(r_p1, p1_copy, dp, t / div);
3184       return true;
3185     }
3186   }
3187   else {
3188     /* behind plane, completely clipped */
3189     if (t >= 0.0f) {
3190       return false;
3191     }
3192     else if (t > div) {
3193       const float p1_copy[3] = {UNPACK3(p1)};
3194       copy_v3_v3(r_p1, p1);
3195       madd_v3_v3v3fl(r_p2, p1_copy, dp, t / div);
3196       return true;
3197     }
3198   }
3199
3200   /* incase input/output values match (above also) */
3201   const float p1_copy[3] = {UNPACK3(p1)};
3202   copy_v3_v3(r_p2, p2);
3203   copy_v3_v3(r_p1, p1_copy);
3204   return true;
3205 }
3206
3207 bool clip_segment_v3_plane_n(const float p1[3],
3208                              const float p2[3],
3209                              const float plane_array[][4],
3210                              const int plane_tot,
3211                              float r_p1[3],
3212                              float r_p2[3])
3213 {
3214   /* intersect from both directions */
3215   float p1_fac = 0.0f, p2_fac = 1.0f;
3216
3217   float dp[3];
3218   sub_v3_v3v3(dp, p2, p1);
3219
3220   for (int i = 0; i < plane_tot; i++) {
3221     const float *plane = plane_array[i];
3222     const float div = dot_v3v3(dp, plane);
3223
3224     if (div != 0.0f) {
3225       float t = -plane_point_side_v3(plane, p1);
3226       if (div > 0.0f) {
3227         /* clip p1 lower bounds */
3228         if (t >= div) {
3229           return false;
3230         }
3231         else if (t > 0.0f) {
3232           t /= div;
3233           if (t > p1_fac) {
3234             p1_fac = t;
3235             if (p1_fac > p2_fac) {
3236               return false;
3237             }
3238           }
3239         }
3240       }
3241       else if (div < 0.0f) {
3242         /* clip p2 upper bounds */
3243         if (t >= 0.0f) {
3244           return false;
3245         }
3246         else if (t > div) {
3247           t /= div;
3248           if (t < p2_fac) {
3249             p2_fac = t;
3250             if (p1_fac > p2_fac) {
3251               return false;
3252             }
3253           }
3254         }
3255       }
3256     }
3257   }
3258
3259   /* incase input/output values match */
3260   const float p1_copy[3] = {UNPACK3(p1)};
3261
3262   madd_v3_v3v3fl(r_p1, p1_copy, dp, p1_fac);
3263   madd_v3_v3v3fl(r_p2, p1_copy, dp, p2_fac);
3264
3265   return true;
3266 }
3267
3268 /****************************** Axis Utils ********************************/
3269
3270 /**
3271  * \brief Normal to x,y matrix
3272  *
3273  * Creates a 3x3 matrix from a normal.
3274  * This matrix can be applied to vectors so their 'z' axis runs along \a normal.
3275  * In practice it means you can use x,y as 2d coords. \see
3276  *
3277  * \param r_mat: The matrix to return.
3278  * \param normal: A unit length vector.
3279  */
3280 void axis_dominant_v3_to_m3(float r_mat[3][3], const float normal[3])
3281 {
3282   BLI_ASSERT_UNIT_V3(normal);
3283
3284   copy_v3_v3(r_mat[2], normal);
3285   ortho_basis_v3v3_v3(r_mat[0], r_mat[1], r_mat[2]);
3286
3287   BLI_ASSERT_UNIT_V3(r_mat[0]);
3288   BLI_ASSERT_UNIT_V3(r_mat[1]);
3289
3290   transpose_m3(r_mat);
3291
3292   BLI_assert(!is_negative_m3(r_mat));
3293   BLI_assert((fabsf(dot_m3_v3_row_z(r_mat, normal) - 1.0f) < BLI_ASSERT_UNIT_EPSILON) ||
3294              is_zero_v3(normal));
3295 }
3296
3297 /**
3298  * Same as axis_dominant_v3_to_m3, but flips the normal
3299  */
3300 void axis_dominant_v3_to_m3_negate(float r_mat[3][3], const float normal[3])
3301 {
3302   BLI_ASSERT_UNIT_V3(normal);
3303
3304   negate_v3_v3(r_mat[2], normal);
3305   ortho_basis_v3v3_v3(r_mat[0], r_mat[1], r_mat[2]);
3306
3307   BLI_ASSERT_UNIT_V3(r_mat[0]);
3308   BLI_ASSERT_UNIT_V3(r_mat[1]);
3309
3310   transpose_m3(r_mat);
3311
3312   BLI_assert(!is_negative_m3(r_mat));
3313   BLI_assert((dot_m3_v3_row_z(r_mat, normal) < BLI_ASSERT_UNIT_EPSILON) || is_zero_v3(normal));
3314 }
3315
3316 /****************************** Interpolation ********************************/
3317
3318 static float tri_signed_area(
3319     const float v1[3], const float v2[3], const float v3[3], const int i, const int j)
3320 {
3321   return 0.5f * ((v1[i] - v2[i]) * (v2[j] - v3[j]) + (v1[j] - v2[j]) * (v3[i] - v2[i]));
3322 }
3323
3324 /* return 1 when degenerate */
3325 static bool barycentric_weights(const float v1[3],
3326                                 const float v2[3],
3327                                 const float v3[3],
3328                                 const float co[3],
3329                                 const float n[3],
3330                                 float w[3])
3331 {
3332   float wtot;
3333   int i, j;
3334
3335   axis_dominant_v3(&i, &j, n);
3336
3337   w[0] = tri_signed_area(v2, v3, co, i, j);
3338   w[1] = tri_signed_area(v3, v1, co, i, j);
3339   w[2] = tri_signed_area(v1, v2, co, i, j);
3340
3341   wtot = w[0] + w[1] + w[2];
3342
3343   if (fabsf(wtot) > FLT_EPSILON) {
3344     mul_v3_fl(w, 1.0f / wtot);
3345     return false;
3346   }
3347   else {
3348     /* zero area triangle */
3349     copy_v3_fl(w, 1.0f / 3.0f);
3350     return true;
3351   }
3352 }
3353
3354 void interp_weights_tri_v3(
3355     float w[3], const float v1[3], const float v2[3], const float v3[3], const float co[3])
3356 {
3357   float n[3];
3358
3359   normal_tri_v3(n, v1, v2, v3);
3360   barycentric_weights(v1, v2, v3, co, n, w);
3361 }
3362
3363 void interp_weights_quad_v3(float w[4],
3364                             const float v1[3],
3365                             const float v2[3],
3366                             const float v3[3],
3367                             const float v4[3],
3368                             const float co[3])
3369 {
3370   float w2[3];
3371
3372   w[0] = w[1] = w[2] = w[3] = 0.0f;
3373
3374   /* first check for exact match */
3375   if (equals_v3v3(co, v1)) {
3376     w[0] = 1.0f;
3377   }
3378   else if (equals_v3v3(co, v2)) {
3379     w[1] = 1.0f;
3380   }
3381   else if (equals_v3v3(co, v3)) {
3382     w[2] = 1.0f;
3383   }
3384   else if (equals_v3v3(co, v4)) {
3385     w[3] = 1.0f;
3386   }
3387   else {
3388     /* otherwise compute barycentric interpolation weights */
3389     float n1[3], n2[3], n[3];
3390     bool degenerate;
3391
3392     sub_v3_v3v3(n1, v1, v3);
3393     sub_v3_v3v3(n2, v2, v4);
3394     cross_v3_v3v3(n, n1, n2);
3395
3396     degenerate = barycentric_weights(v1, v2, v4, co, n, w);
3397     SWAP(float, w[2], w[3]);
3398
3399     if (degenerate || (w[0] < 0.0f)) {
3400       /* if w[1] is negative, co is on the other side of the v1-v3 edge,
3401        * so we interpolate using the other triangle */
3402       degenerate = barycentric_weights(v2, v3, v4, co, n, w2);
3403
3404       if (!degenerate) {
3405         w[0] = 0.0f;
3406         w[1] = w2[0];
3407         w[2] = w2[1];
3408         w[3] = w2[2];
3409       }
3410     }
3411   }
3412 }
3413
3414 /**
3415  * \return
3416  * - 0 if the point is outside of triangle.
3417  * - 1 if the point is inside triangle.
3418  * - 2 if it's on the edge.
3419  * */
3420 int barycentric_inside_triangle_v2(const float w[3])
3421 {
3422   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)) {
3423     return 1;
3424   }
3425   else if (IN_RANGE_INCL(w[0], 0.0f, 1.0f) && IN_RANGE_INCL(w[1], 0.0f, 1.0f) &&
3426            IN_RANGE_INCL(w[2], 0.0f, 1.0f)) {
3427     return 2;
3428   }
3429
3430   return 0;
3431 }
3432
3433 /* returns 0 for degenerated triangles */
3434 bool barycentric_coords_v2(
3435     const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
3436 {
3437   const float x = co[0], y = co[1];
3438   const float x1 = v1[0], y1 = v1[1];
3439   const float x2 = v2[0], y2 = v2[1];
3440   const float x3 = v3[0], y3 = v3[1];
3441   const float det = (y2 - y3) * (x1 - x3) + (x3 - x2) * (y1 - y3);
3442
3443   if (fabsf(det) > FLT_EPSILON) {
3444     w[0] = ((y2 - y3) * (x - x3) + (x3 - x2) * (y - y3)) / det;
3445     w[1] = ((y3 - y1) * (x - x3) + (x1 - x3) * (y - y3)) / det;
3446     w[2] = 1.0f - w[0] - w[1];
3447
3448     return true;
3449   }
3450
3451   return false;
3452 }
3453
3454 /**
3455  * \note: using #cross_tri_v2 means locations outside the triangle are correctly weighted
3456  *
3457  * \note This is *exactly* the same calculation as #resolve_tri_uv_v2,
3458  * although it has double precision and is used for texture baking, so keep both.
3459  */
3460 void barycentric_weights_v2(
3461     const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
3462 {
3463   float wtot;
3464
3465   w[0] = cross_tri_v2(v2, v3, co);
3466   w[1] = cross_tri_v2(v3, v1, co);
3467   w[2] = cross_tri_v2(v1, v2, co);
3468   wtot = w[0] + w[1] + w[2];
3469
3470   if (wtot != 0.0f) {
3471     mul_v3_fl(w, 1.0f / wtot);
3472   }
3473   else { /* dummy values for zero area face */
3474     copy_v3_fl(w, 1.0f / 3.0f);
3475   }
3476 }
3477
3478 /**
3479  * A version of #barycentric_weights_v2 that doesn't allow negative weights.
3480  * Useful when negative values cause problems and points are only
3481  * ever slightly outside of the triangle.
3482  */
3483 void barycentric_weights_v2_clamped(
3484     const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
3485 {
3486   float wtot;
3487
3488   w[0] = max_ff(cross_tri_v2(v2, v3, co), 0.0f);
3489   w[1] = max_ff(cross_tri_v2(v3, v1, co), 0.0f);
3490   w[2] = max_ff(cross_tri_v2(v1, v2, co), 0.0f);
3491   wtot = w[0] + w[1] + w[2];
3492
3493   if (wtot != 0.0f) {
3494     mul_v3_fl(w, 1.0f / wtot);
3495   }
3496   else { /* dummy values for zero area face */
3497     copy_v3_fl(w, 1.0f / 3.0f);
3498   }
3499 }
3500
3501 /**
3502  * still use 2D X,Y space but this works for verts transformed by a perspective matrix,
3503  * using their 4th component as a weight
3504  */
3505 void barycentric_weights_v2_persp(
3506     const float v1[4], const float v2[4], const float v3[4], const float co[2], float w[3])
3507 {
3508   float wtot;
3509
3510   w[0] = cross_tri_v2(v2, v3, co) / v1[3];
3511   w[1] = cross_tri_v2(v3, v1, co) / v2[3];
3512   w[2] = cross_tri_v2(v1, v2, co) / v3[3];
3513   wtot = w[0] + w[1] + w[2];
3514
3515   if (wtot != 0.0f) {
3516     mul_v3_fl(w, 1.0f / wtot);
3517   }
3518   else { /* dummy values for zero area face */
3519     w[0] = w[1] = w[2] = 1.0f / 3.0f;
3520   }
3521 }
3522
3523 /**
3524  * same as #barycentric_weights_v2 but works with a quad,
3525  * note: untested for values outside the quad's bounds
3526  * this is #interp_weights_poly_v2 expanded for quads only
3527  */
3528 void barycentric_weights_v2_quad(const float v1[2],
3529                                  const float v2[2],
3530                                  const float v3[2],
3531                                  const float v4[2],
3532                                  const float co[2],
3533                                  float w[4])
3534 {
3535   /* note: fabsf() here is not needed for convex quads (and not used in interp_weights_poly_v2).
3536    * but in the case of concave/bow-tie quads for the mask rasterizer it gives unreliable results
3537    * without adding absf(). If this becomes an issue for more general usage we could have
3538    * this optional or use a different function - Campbell */
3539 #define MEAN_VALUE_HALF_TAN_V2(_area, i1, i2) \
3540   ((_area = cross_v2v2(dirs[i1], dirs[i2])) != 0.0f ? \
3541        fabsf(((lens[i1] * lens[i2]) - dot_v2v2(dirs[i1], dirs[i2])) / _area) : \
3542        0.0f)
3543
3544   const float dirs[4][2] = {
3545       {v1[0] - co[0], v1[1] - co[1]},
3546       {v2[0] - co[0], v2[1] - co[1]},
3547       {v3[0] - co[0], v3[1] - co[1]},
3548       {v4[0] - co[0], v4[1] - co[1]},
3549   };
3550
3551   const float lens[4] = {
3552       len_v2(dirs[0]),
3553       len_v2(dirs[1]),
3554       len_v2(dirs[2]),
3555       len_v2(dirs[3]),
3556   };
3557
3558   /* avoid divide by zero */
3559   if (UNLIKELY(lens[0] < FLT_EPSILON)) {
3560     w[0] = 1.0f;
3561     w[1] = w[2] = w[3] = 0.0f;
3562   }
3563   else if (UNLIKELY(lens[1] < FLT_EPSILON)) {
3564     w[1] = 1.0f;
3565     w[0] = w[2] = w[3] = 0.0f;
3566   }
3567   else if (UNLIKELY(lens[2] < FLT_EPSILON)) {
3568     w[2] = 1.0f;
3569     w[0] = w[1] = w[3] = 0.0f;
3570   }
3571   else if (UNLIKELY(lens[3] < FLT_EPSILON)) {
3572     w[3] = 1.0f;
3573     w[0] = w[1] = w[2] = 0.0f;
3574   }
3575   else {
3576     float wtot, area;
3577
3578     /* variable 'area' is just for storage,
3579      * the order its initialized doesn't matter */
3580 #ifdef __clang__
3581 #  pragma clang diagnostic push
3582 #  pragma clang diagnostic ignored "-Wunsequenced"
3583 #endif
3584
3585     /* inline mean_value_half_tan four times here */
3586     const float t[4] = {
3587         MEAN_VALUE_HALF_TAN_V2(area, 0, 1),
3588         MEAN_VALUE_HALF_TAN_V2(area, 1, 2),
3589         MEAN_VALUE_HALF_TAN_V2(area, 2, 3),
3590         MEAN_VALUE_HALF_TAN_V2(area, 3, 0),
3591     };
3592
3593 #ifdef __clang__
3594 #  pragma clang diagnostic pop
3595 #endif
3596
3597 #undef MEAN_VALUE_HALF_TAN_V2
3598
3599     w[0] = (t[3] + t[0]) / lens[0];
3600     w[1] = (t[0] + t[1]) / lens[1];
3601     w[2] = (t[1] + t[2]) / lens[2];
3602     w[3] = (t[2] + t[3]) / lens[3];
3603
3604     wtot = w[0] + w[1] + w[2] + w[3];
3605
3606     if (wtot != 0.0f) {
3607       mul_v4_fl(w, 1.0f / wtot);
3608     }
3609     else { /* dummy values for zero area face */
3610       copy_v4_fl(w, 1.0f / 4.0f);
3611     }
3612   }
3613 }
3614
3615 /* given 2 triangles in 3D space, and a point in relation to the first triangle.
3616  * calculate the location of a point in relation to the second triangle.
3617  * Useful for finding relative positions with geometry */
3618 void transform_point_by_tri_v3(float pt_tar[3],
3619                                float const pt_src[3],
3620                                const float tri_tar_p1[3],
3621                                const float tri_tar_p2[3],
3622                                const float tri_tar_p3[3],
3623                                const float tri_src_p1[3],
3624                                const float tri_src_p2[3],
3625                                const float tri_src_p3[3])
3626 {
3627   /* this works by moving the source triangle so its normal is pointing on the Z
3628    * axis where its barycentric weights can be calculated in 2D and its Z offset can
3629    * be re-applied. The weights are applied directly to the targets 3D points and the
3630    * z-depth is used to scale the targets normal as an offset.
3631    * This saves transforming the target into its Z-Up orientation and back
3632    * (which could also work) */
3633   float no_tar[3], no_src[3];
3634   float mat_src[3][3];
3635   float pt_src_xy[3];
3636   float tri_xy_src[3][3];
3637   float w_src[3];
3638   float area_tar, area_src;
3639   float z_ofs_src;
3640
3641   normal_tri_v3(no_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3);
3642   normal_tri_v3(no_src, tri_src_p1, tri_src_p2, tri_src_p3);
3643
3644   axis_dominant_v3_to_m3(mat_src, no_src);
3645
3646   /* make the source tri xy space */
3647   mul_v3_m3v3(pt_src_xy, mat_src, pt_src);
3648   mul_v3_m3v3(tri_xy_src[0], mat_src, tri_src_p1);
3649   mul_v3_m3v3(tri_xy_src[1], mat_src, tri_src_p2);
3650   mul_v3_m3v3(tri_xy_src[2], mat_src, tri_src_p3);
3651
3652   barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src);
3653   interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src);
3654
3655   area_tar = sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3));
3656   area_src = sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2]));
3657
3658   z_ofs_src = pt_src_xy[2] - tri_xy_src[0][2];
3659   madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar);
3660 }
3661
3662 /**
3663  * Simply re-interpolates,
3664  * assumes p_src is between \a l_src_p1-l_src_p2
3665  */
3666 void transform_point_by_seg_v3(float p_dst[3],
3667                                const float p_src[3],
3668                                const float l_dst_p1[3],
3669                                const float l_dst_p2[3],
3670                                const float l_src_p1[3],
3671                                const float l_src_p2[3])
3672 {
3673   float t = line_point_factor_v3(p_src, l_src_p1, l_src_p2);
3674   interp_v3_v3v3(p_dst, l_dst_p1, l_dst_p2, t);
3675 }
3676
3677 /* given an array with some invalid values this function interpolates valid values
3678  * replacing the invalid ones */
3679 int interp_sparse_array(float *array, const int list_size, const float skipval)
3680 {
3681   int found_invalid = 0;
3682   int found_valid = 0;
3683   int i;
3684
3685   for (i = 0; i < list_size; i++) {
3686     if (array[i] == skipval) {
3687       found_invalid = 1;
3688     }
3689     else {
3690       found_valid = 1;
3691     }
3692   }
3693
3694   if (found_valid == 0) {
3695     return -1;
3696   }
3697   else if (found_invalid == 0) {
3698     return 0;
3699   }
3700   else {
3701     /* found invalid depths, interpolate */
3702     float valid_last = skipval;
3703     int valid_ofs = 0;
3704
3705     float *array_up = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
3706     float *array_down = MEM_callocN(sizeof(float) * (size_t)list_size, "interp_sparse_array up");
3707
3708     int *ofs_tot_up = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tup");
3709     int *ofs_tot_down = MEM_callocN(sizeof(int) * (size_t)list_size, "interp_sparse_array tdown");
3710
3711     for (i = 0; i < list_size; i++) {
3712       if (array[i] == skipval) {
3713         array_up[i] = valid_last;
3714         ofs_tot_up[i] = ++valid_ofs;
3715       }
3716       else {