From 9b0db26988dd47ec461263db12946b0f031a259c Mon Sep 17 00:00:00 2001 From: rms Date: Wed, 30 Nov 2016 12:56:36 -0500 Subject: [PATCH] misc improvements, added Line/Ray and Ray/Ray distance --- math/Frame3f.cs | 17 +++- math/Line3.cs | 56 ++++++++++++ math/MathUtil.cs | 16 +++- math/Ray3.cs | 12 +++ queries/DistLine3Ray3.cs | 114 +++++++++++++++++++++++++ queries/DistRay3Ray3.cs | 164 ++++++++++++++++++++++++++++++++++++ queries/DistRay3Segment3.cs | 14 +++ 7 files changed, 389 insertions(+), 4 deletions(-) create mode 100644 math/Line3.cs create mode 100644 queries/DistLine3Ray3.cs create mode 100644 queries/DistRay3Ray3.cs diff --git a/math/Frame3f.cs b/math/Frame3f.cs index 6b6ea8cd..79776e0c 100644 --- a/math/Frame3f.cs +++ b/math/Frame3f.cs @@ -3,7 +3,10 @@ using System.Text; using g3; - +#if G3_USING_UNITY +using UnityEngine; +using f3; // yuck? will go away though... +#endif namespace g3 { @@ -204,5 +207,17 @@ public virtual string ToString(string fmt) { } + +#if G3_USING_UNITY + public static implicit operator Frame3f(f3.Frame3 f) + { + return new Frame3f(f.Origin, f.Rotation); + } + public static implicit operator Frame3(Frame3f f) + { + return new Frame3(f.origin, f.rotation); + } +#endif + } } diff --git a/math/Line3.cs b/math/Line3.cs new file mode 100644 index 00000000..9c3bf52f --- /dev/null +++ b/math/Line3.cs @@ -0,0 +1,56 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class Line3d + { + public Vector3d Origin; + public Vector3d Direction; + + public Line3d(Vector3d origin, Vector3d direction) + { + this.Origin = origin; + this.Direction = direction; + } + + // parameter is distance along Line + public Vector3d PointAt(double d) { + return Origin + d * Direction; + } + + + // conversion operators + public static implicit operator Line3d(Line3f v) + { + return new Line3d(v.Origin, v.Direction); + } + public static explicit operator Line3f(Line3d v) + { + return new Line3f((Vector3f)v.Origin, (Vector3f)v.Direction); + } + + + } + + + public class Line3f + { + public Vector3f Origin; + public Vector3f Direction; + + public Line3f(Vector3f origin, Vector3f direction) + { + this.Origin = origin; + this.Direction = direction; + } + + // parameter is distance along Line + public Vector3f PointAt(float d) + { + return Origin + d * Direction; + } + } +} diff --git a/math/MathUtil.cs b/math/MathUtil.cs index 053ec3fa..4dc8ea57 100644 --- a/math/MathUtil.cs +++ b/math/MathUtil.cs @@ -7,15 +7,15 @@ namespace g3 public class MathUtil { - public const double Deg2Rad = (180.0 / Math.PI); - public const double Rad2Deg = (Math.PI / 180.0); + public const double Deg2Rad = (Math.PI / 180.0); + public const double Rad2Deg = (180.0 / Math.PI); public const double TwoPI = 2.0 * Math.PI; public const double HalfPI = 0.5 * Math.PI; public const double ZeroTolerance = 1e-08; public const double Epsilon = 2.2204460492503131e-016; - public const float Rad2Degf = (float)(180.0 / Math.PI); public const float Deg2Radf = (float)(Math.PI / 180.0); + public const float Rad2Degf = (float)(180.0 / Math.PI); public const float PIf = (float)(Math.PI); public const float TwoPIf = 2.0f * PIf; public const float HalfPIf = 0.5f * PIf; @@ -89,6 +89,16 @@ public static float PlaneAngleSignedD(Vector3f vFrom, Vector3f vTo, Vector3f pla + public static int MostParallelAxis(Frame3f f, Vector3f vDir) { + double dot0 = Math.Abs(f.X.Dot(vDir)); + double dot1 = Math.Abs(f.Y.Dot(vDir)); + double dot2 = Math.Abs(f.Z.Dot(vDir)); + double m = Math.Max(dot0, Math.Max(dot1, dot2)); + return (m == dot0) ? 0 : (m == dot1) ? 1 : 2; + } + + + //! if yshift is 0, function approaches y=1 at xZero from y=0. //! speed (> 0) controls how fast it gets there diff --git a/math/Ray3.cs b/math/Ray3.cs index 1918f3f9..429b93db 100644 --- a/math/Ray3.cs +++ b/math/Ray3.cs @@ -25,6 +25,18 @@ public Vector3d PointAt(double d) { return Origin + d * Direction; } + + // conversion operators + public static implicit operator Ray3d(Ray3f v) + { + return new Ray3d(v.Origin, v.Direction); + } + public static explicit operator Ray3f(Ray3d v) + { + return new Ray3f((Vector3f)v.Origin, (Vector3f)v.Direction); + } + + #if G3_USING_UNITY public static implicit operator Ray3d(UnityEngine.Ray r) { diff --git a/queries/DistLine3Ray3.cs b/queries/DistLine3Ray3.cs new file mode 100644 index 00000000..9ad4c509 --- /dev/null +++ b/queries/DistLine3Ray3.cs @@ -0,0 +1,114 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class DistLine3Ray3 + { + Line3d line; + public Line3d Line + { + get { return line; } + set { line = value; DistanceSquared = -1.0; } + } + + Ray3d ray; + public Ray3d Ray + { + get { return ray; } + set { ray = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d LineClosest; + public double LineParameter; + public Vector3d RayClosest; + public double RayParameter; + + + public DistLine3Ray3(Ray3d ray, Line3d Line) + { + this.ray = ray; this.line = Line; + } + + // have to do this if you are changing Ray/Line yourself and want to reuse this object + public void Reset() + { + DistanceSquared = -1.0f; + } + + + static public double MinDistance(Ray3d r, Line3d s) + { + return new DistLine3Ray3(r, s).Get(); + } + static public double MinDistanceLineParam(Ray3d r, Line3d s) + { + return new DistLine3Ray3(r, s).Compute().LineParameter; + } + + + public DistLine3Ray3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared () + { + Vector3d kDiff = line.Origin - ray.Origin; + double a01 = -line.Direction.Dot(ray.Direction); + double b0 = kDiff.Dot(line.Direction); + double c = kDiff.LengthSquared; + double det = Math.Abs(1.0 - a01 * a01); + double b1, s0, s1, sqrDist; + + if (det >= MathUtil.ZeroTolerance) { + b1 = -kDiff.Dot(ray.Direction); + s1 = a01 * b0 - b1; + + if (s1 >= (double)0) { + // Two interior points are closest, one on line and one on ray. + double invDet = ((double)1) / det; + s0 = (a01 * b1 - b0) * invDet; + s1 *= invDet; + sqrDist = s0 * (s0 + a01 * s1 + ((double)2) * b0) + + s1 * (a01 * s0 + s1 + ((double)2) * b1) + c; + } else { + // Origin of ray and interior point of line are closest. + s0 = -b0; + s1 = (double)0; + sqrDist = b0 * s0 + c; + } + } else { + // Lines are parallel, closest pair with one point at ray origin. + s0 = -b0; + s1 = (double)0; + sqrDist = b0 * s0 + c; + } + + LineClosest = line.Origin + s0 * line.Direction; + RayClosest = ray.Origin + s1 * ray.Direction; + LineParameter = s0; + RayParameter = s1; + + // Account for numerical round-off errors. + if (sqrDist < (double)0) { + sqrDist = (double)0; + } + return sqrDist; + } + + + + } +} diff --git a/queries/DistRay3Ray3.cs b/queries/DistRay3Ray3.cs new file mode 100644 index 00000000..ae57cdcc --- /dev/null +++ b/queries/DistRay3Ray3.cs @@ -0,0 +1,164 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class DistRay3Ray3 + { + Ray3d ray1; + public Ray3d Ray1 + { + get { return ray1; } + set { ray1 = value; DistanceSquared = -1.0; } + } + + Ray3d ray2; + public Ray3d Ray2 + { + get { return ray2; } + set { ray2 = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d Ray1Closest; + public double Ray1Parameter; + public Vector3d Ray2Closest; + public double Ray2Parameter; + + + public DistRay3Ray3(Ray3d ray1, Ray3d ray2) + { + this.ray1 = ray1; this.ray2 = ray2; + } + + // have to do this if you are changing Ray/Segment yourself and want to reuse this object + public void Reset() + { + DistanceSquared = -1.0f; + } + + + static public double MinDistance(Ray3d r1, Ray3d r2) { + return new DistRay3Ray3(r1, r2).Get(); + } + static public double MinDistanceRay2Param(Ray3d r1, Ray3d r2) { + return new DistRay3Ray3(r1, r2).Compute().Ray2Parameter; + } + + + public DistRay3Ray3 Compute() { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared() + { + Vector3d diff = ray1.Origin - ray2.Origin; + double a01 = -ray1.Direction.Dot(ray2.Direction); + double b0 = diff.Dot(ray1.Direction); + double c = diff.LengthSquared; + double det = Math.Abs(1.0 - a01 * a01); + double b1, s0, s1, sqrDist; + + if (det >= MathUtil.ZeroTolerance) { + // Rays are not parallel. + b1 = -diff.Dot(ray2.Direction); + s0 = a01 * b1 - b0; + s1 = a01 * b0 - b1; + + if (s0 >= 0) { + if (s1 >= 0) { // region 0 (interior) + // Minimum at two interior points of rays. + double invDet = (1.0) / det; + s0 *= invDet; + s1 *= invDet; + sqrDist = s0 * (s0 + a01 * s1 + (2.0) * b0) + + s1 * (a01 * s0 + s1 + (2.0) * b1) + c; + } else { // region 3 (side) + s1 = 0; + if (b0 >= 0) { + s0 = 0; + sqrDist = c; + } else { + s0 = -b0; + sqrDist = b0 * s0 + c; + } + } + } else { + if (s1 >= 0) { // region 1 (side) + s0 = 0; + if (b1 >= 0) { + s1 = 0; + sqrDist = c; + } else { + s1 = -b1; + sqrDist = b1 * s1 + c; + } + } else { // region 2 (corner) + if (b0 < 0) { + s0 = -b0; + s1 = 0; + sqrDist = b0 * s0 + c; + } else { + s0 = 0; + if (b1 >= 0) { + s1 = 0; + sqrDist = c; + } else { + s1 = -b1; + sqrDist = b1 * s1 + c; + } + } + } + } + } else { + // Rays are parallel. + if (a01 > 0) { + // Opposite direction vectors. + s1 = 0; + if (b0 >= 0) { + s0 = 0; + sqrDist = c; + } else { + s0 = -b0; + sqrDist = b0 * s0 + c; + } + } else { + // Same direction vectors. + if (b0 >= 0) { + b1 = -diff.Dot(ray2.Direction); + s0 = 0; + s1 = -b1; + sqrDist = b1 * s1 + c; + } else { + s0 = -b0; + s1 = 0; + sqrDist = b0 * s0 + c; + } + } + } + + Ray1Closest = ray1.Origin + s0 * ray1.Direction; + Ray2Closest = ray2.Origin + s1 * ray2.Direction; + Ray1Parameter = s0; + Ray2Parameter = s1; + + // Account for numerical round-off errors. + if (sqrDist < 0) { + sqrDist = 0; + } + return sqrDist; + } + + + } +} diff --git a/queries/DistRay3Segment3.cs b/queries/DistRay3Segment3.cs index 1587ea23..7d602682 100644 --- a/queries/DistRay3Segment3.cs +++ b/queries/DistRay3Segment3.cs @@ -39,6 +39,20 @@ public void Reset() { DistanceSquared = -1.0f; } + + static public double MinDistance(Ray3d r, Segment3d s) { + return new DistRay3Segment3(r, s).Get(); + } + static public double MinDistanceSegmentParam(Ray3d r, Segment3d s) { + return new DistRay3Segment3(r, s).Compute().SegmentParameter; + } + + + public DistRay3Segment3 Compute() { + GetSquared(); + return this; + } + public double Get() { return Math.Sqrt(GetSquared()); }