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