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