Fix T67389: Transform constraints fail at large distances

2 files changed, 47 insertions, 0 deletions

diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h
index 484d5af194d..d5485765844 100644
--- a/source/blender/blenlib/BLI_math_geom.h
+++ b/source/blender/blenlib/BLI_math_geom.h
@@ -302,6 +302,13 @@ bool isect_line_line_strict_v3(const float v1[3],
                                float vi[3],
                                float *r_lambda);
 
+bool isect_ray_ray_v3(const float ray_origin_a[3],
+                      const float ray_direction_a[3],
+                      const float ray_origin_b[3],
+                      const float ray_direction_b[3],
+                      float *r_lambda_a,
+                      float *r_lambda_b);
+
 bool isect_ray_plane_v3(const float ray_origin[3],
                         const float ray_direction[3],
                         const float plane[4],
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
index 5dbd2a52d07..8b715ebf87b 100644
--- a/source/blender/blenlib/intern/math_geom.c
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -2798,6 +2798,46 @@ bool isect_line_line_strict_v3(const float v1[3],
   }
 }
 
+/**
+ * Check if two rays are not parallel and returns a factor that indicates
+ * the distance from \a ray_origin_b to the closest point on ray-a to ray-b.
+ *
+ * \note Neither directions need to be normalized.
+ */
+bool isect_ray_ray_v3(const float ray_origin_a[3],
+                      const float ray_direction_a[3],
+                      const float ray_origin_b[3],
+                      const float ray_direction_b[3],
+                      float *r_lambda_a,
+                      float *r_lambda_b)
+{
+  BLI_assert(r_lambda_a || r_lambda_b);
+  float n[3];
+  cross_v3_v3v3(n, ray_direction_b, ray_direction_a);
+  const float nlen = len_squared_v3(n);
+
+  if (UNLIKELY(nlen == 0.0f)) {
+    /* The lines are parallel. */
+    return false;
+  }
+
+  float t[3], c[3], cray[3];
+  sub_v3_v3v3(t, ray_origin_b, ray_origin_a);
+  sub_v3_v3v3(c, n, t);
+
+  if (r_lambda_a != NULL) {
+    cross_v3_v3v3(cray, c, ray_direction_a);
+    *r_lambda_a = dot_v3v3(cray, n) / nlen;
+  }
+
+  if (r_lambda_b != NULL) {
+    cross_v3_v3v3(cray, c, ray_direction_b);
+    *r_lambda_b = dot_v3v3(cray, n) / nlen;
+  }
+
+  return true;
+}
+
 bool isect_aabb_aabb_v3(const float min1[3],
                         const float max1[3],
                         const float min2[3],