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