diff options
| -rw-r--r-- | source/blender/blenkernel/intern/collision.c | 43 | ||||
| -rw-r--r-- | source/blender/blenlib/BLI_math_geom.h | 5 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_geom.c | 64 |
3 files changed, 75 insertions, 37 deletions
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index d0eac3bb713..a3e13820ee0 100644 --- a/source/blender/blenkernel/intern/collision.c +++ b/source/blender/blenkernel/intern/collision.c @@ -192,36 +192,6 @@ void bvhtree_update_from_mvert( Collision modifier code end ***********************************/ -static void clamp_point_seg(float a[3], float b[3], float p[3]) -{ - float ap[3], bp[3], ab[3]; - - sub_v3_v3v3(ap, p, a); - sub_v3_v3v3(bp, p, b); - sub_v3_v3v3(ab, b, a); - - if (dot_v3v3(ap, bp) > 0.0f) { - if (dot_v3v3(ap, ab) > 0.0f) { - copy_v3_v3(p, b); - } - else { - copy_v3_v3(p, a); - } - } -} - -static bool isect_seg_seg(float a1[3], float a2[3], float b1[3], float b2[3], float r_a[3], float r_b[3]) -{ - if (isect_line_line_epsilon_v3(a1, a2, b1, b2, r_a, r_b, 0.0f)) { - clamp_point_seg(a1, a2, r_a); - clamp_point_seg(b1, b2, r_b); - - return true; - } - - return false; -} - BLI_INLINE int next_ind(int i) { return (++i < 3) ? i : 0; @@ -410,14 +380,13 @@ static float compute_collision_point(float a1[3], float a2[3], float a3[3], floa if (isect_count == 0) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - if (isect_seg_seg(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2)) { - tmp = len_squared_v3v3(tmp_co1, tmp_co2); + isect_seg_seg_v3(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2); + tmp = len_squared_v3v3(tmp_co1, tmp_co2); - if (tmp < dist) { - dist = tmp; - copy_v3_v3(r_a, tmp_co1); - copy_v3_v3(r_b, tmp_co2); - } + if (tmp < dist) { + dist = tmp; + copy_v3_v3(r_a, tmp_co1); + copy_v3_v3(r_b, tmp_co2); } } } diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h index ccdb94c3317..d5287e8d8aa 100644 --- a/source/blender/blenlib/BLI_math_geom.h +++ b/source/blender/blenlib/BLI_math_geom.h @@ -207,6 +207,11 @@ void limit_dist_v3(float v1[3], float v2[3], const float dist); #define ISECT_LINE_LINE_CROSS 2 int isect_seg_seg_v2(const float a1[2], const float a2[2], const float b1[2], const float b2[2]); +void isect_seg_seg_v3( + const float a0[3], const float a1[3], + const float b0[3], const float b1[3], + float r_a[3], float r_b[3]); + int isect_seg_seg_v2_int(const int a1[2], const int a2[2], const int b1[2], const int b2[2]); int isect_seg_seg_v2_point_ex( const float v0[2], const float v1[2], const float v2[2], const float v3[2], const float endpoint_bias, diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c index fb2a1e47895..a5c84ed7645 100644 --- a/source/blender/blenlib/intern/math_geom.c +++ b/source/blender/blenlib/intern/math_geom.c @@ -1123,6 +1123,70 @@ int isect_seg_seg_v2(const float v1[2], const float v2[2], const float v3[2], co return ISECT_LINE_LINE_NONE; } +/* Returns a point on each segment that is closest to the other. */ +void isect_seg_seg_v3( + const float a0[3], const float a1[3], + const float b0[3], const float b1[3], + float r_a[3], float r_b[3]) +{ + float fac_a, fac_b; + float a_dir[3], b_dir[3], a0b0[3], crs_ab[3]; + sub_v3_v3v3(a_dir, a1, a0); + sub_v3_v3v3(b_dir, b1, b0); + sub_v3_v3v3(a0b0, b0, a0); + cross_v3_v3v3(crs_ab, b_dir, a_dir); + const float nlen = len_squared_v3(crs_ab); + + if (nlen == 0.0f) { + /* Parallel Lines */ + /* In this case return any point that + * is between the closest segments. */ + float a0b1[3], a1b0[3], len_a, len_b, fac1, fac2; + sub_v3_v3v3(a0b1, b1, a0); + sub_v3_v3v3(a1b0, b0, a1); + len_a = len_squared_v3(a_dir); + len_b = len_squared_v3(b_dir); + + if (len_a) { + fac1 = dot_v3v3(a0b0, a_dir); + fac2 = dot_v3v3(a0b1, a_dir); + CLAMP(fac1, 0.0f, len_a); + CLAMP(fac2, 0.0f, len_a); + fac_a = (fac1 + fac2) / (2 * len_a); + } + else { + fac_a = 0.0f; + } + + if (len_b) { + fac1 = -dot_v3v3(a0b0, b_dir); + fac2 = -dot_v3v3(a1b0, b_dir); + CLAMP(fac1, 0.0f, len_b); + CLAMP(fac2, 0.0f, len_b); + fac_b = (fac1 + fac2) / (2 * len_b); + } + else { + fac_b = 0.0f; + } + } + else { + float c[3], cray[3]; + sub_v3_v3v3(c, crs_ab, a0b0); + + cross_v3_v3v3(cray, c, b_dir); + fac_a = dot_v3v3(cray, crs_ab) / nlen; + + cross_v3_v3v3(cray, c, a_dir); + fac_b = dot_v3v3(cray, crs_ab) / nlen; + + CLAMP(fac_a, 0.0f, 1.0f); + CLAMP(fac_b, 0.0f, 1.0f); + } + + madd_v3_v3v3fl(r_a, a0, a_dir, fac_a); + madd_v3_v3v3fl(r_b, b0, b_dir, fac_b); +} + /** * Get intersection point of two 2D segments. * |