From d2cf2ba6a24af22e5ac5b51c07a0cebd959642a5 Mon Sep 17 00:00:00 2001 From: rms Date: Thu, 24 Nov 2016 14:26:48 -0500 Subject: [PATCH] added Ray3, Segment2/3, AABox3, DistRay3Seg3, CurveUtils.FindClosestRayIntersection --- curve/CurveUtils.cs | 39 +++++ curve/DCurve3.cs | 11 +- curve/ICurve.cs | 1 + implicit/ImplicitField.cs | 6 +- implicit/ImplicitOperators.cs | 4 +- implicit/MarchingQuads.cs | 12 +- math/{Box2f.cs => AxisAlignedBox2f.cs} | 32 ++--- math/AxisAlignedBox3d.cs | 190 +++++++++++++++++++++++++ math/Ray3.cs | 71 +++++++++ math/Segment2.cs | 82 +++++++++++ math/Segment3.cs | 110 ++++++++++++++ math/Vector2d.cs | 10 ++ math/Vector3d.cs | 10 ++ queries/DistRay3Segment3.cs | 176 +++++++++++++++++++++++ queries/RayIntersection.cs | 19 +++ 15 files changed, 745 insertions(+), 28 deletions(-) rename math/{Box2f.cs => AxisAlignedBox2f.cs} (86%) create mode 100644 math/AxisAlignedBox3d.cs create mode 100644 math/Ray3.cs create mode 100644 math/Segment2.cs create mode 100644 math/Segment3.cs create mode 100644 queries/DistRay3Segment3.cs diff --git a/curve/CurveUtils.cs b/curve/CurveUtils.cs index 1463d7a7..cf07fa62 100644 --- a/curve/CurveUtils.cs +++ b/curve/CurveUtils.cs @@ -23,5 +23,44 @@ public static int FindNearestIndex(ICurve c, Vector3d v) } return iNearest; } + + + + public static bool FindClosestRayIntersection(ICurve c, double segRadius, Ray3d ray, out double rayT) + { + if (c.Closed) + throw new InvalidOperationException("CurveUtils.FindClosestRayIntersection doesn't support closed curves yet"); + + DistRay3Segment3 dist = new DistRay3Segment3(ray, new Segment3d(Vector3d.Zero, Vector3d.Zero) ); + + rayT = double.MaxValue; + int nNearSegment = -1; + double fNearSegT = 0.0; + + int N = c.VertexCount; + for (int i = 0; i < N-1; ++i) { + dist.Segment.SetEndpoints(c.GetVertex(i), c.GetVertex(i + 1)); + dist.Reset(); + + // raycast to line bounding-sphere first (is this going ot be faster??) + double fSphereHitT; + bool bHitBoundSphere = RayIntersection.SphereSigned(ray.Origin, ray.Direction, + dist.Segment.Center, dist.Segment.Extent + segRadius, out fSphereHitT); + if (bHitBoundSphere == false) + continue; + + // find ray/seg min-distance and use ray T + double dSqr = dist.GetSquared(); + if ( dSqr < segRadius*segRadius) { + if (dist.RayParameter < rayT) { + rayT = dist.RayParameter; + fNearSegT = dist.SegmentParameter; + nNearSegment = i; + } + } + } + return (nNearSegment >= 0); + } + } } diff --git a/curve/DCurve3.cs b/curve/DCurve3.cs index 6e1140d1..ebc73b63 100644 --- a/curve/DCurve3.cs +++ b/curve/DCurve3.cs @@ -10,7 +10,7 @@ public class DCurve3 : ICurve // [TODO] use dvector? or double-indirection indexing? // question is how to insert efficiently... public List vertices; - public bool Closed; + public bool Closed { get; set; } public int Timestamp; public DCurve3() @@ -47,5 +47,14 @@ public Vector3d End { public IEnumerable Vertices() { return vertices; } + + + public AxisAlignedBox3d GetBoundingBox() + { + AxisAlignedBox3d box = AxisAlignedBox3d.Empty; + foreach (Vector3d v in vertices) + box.Contain(v); + return box; + } } } diff --git a/curve/ICurve.cs b/curve/ICurve.cs index 3e30baa8..d46db4c7 100644 --- a/curve/ICurve.cs +++ b/curve/ICurve.cs @@ -8,6 +8,7 @@ namespace g3 public interface ICurve { int VertexCount { get; } + bool Closed { get; } Vector3d GetVertex(int i); diff --git a/implicit/ImplicitField.cs b/implicit/ImplicitField.cs index 38853e81..3aa7f7af 100644 --- a/implicit/ImplicitField.cs +++ b/implicit/ImplicitField.cs @@ -12,7 +12,7 @@ public interface IImplicitField2D void Gradient( float fX, float fY, ref float fGX, ref float fGY ); - Box2f Bounds { get; } + AxisAlignedBox2f Bounds { get; } } public interface IImplicitOperator2D : IImplicitField2D @@ -52,9 +52,9 @@ public float Value( float fX, float fY ) { return fDist2 * fDist2 * fDist2; } - public Box2f Bounds { + public AxisAlignedBox2f Bounds { get { - return new Box2f(LowX, LowY, HighX, HighY); + return new AxisAlignedBox2f(LowX, LowY, HighX, HighY); } } diff --git a/implicit/ImplicitOperators.cs b/implicit/ImplicitOperators.cs index c6f6fc3f..92def9bf 100644 --- a/implicit/ImplicitOperators.cs +++ b/implicit/ImplicitOperators.cs @@ -32,10 +32,10 @@ virtual public void Gradient(float fX, float fY, ref float fGX, ref float fGY) fGY = (Value(fX, fY + fDelta) - fValue) / fDelta; } - virtual public Box2f Bounds + virtual public AxisAlignedBox2f Bounds { get { - Box2f box = new Box2f(); + AxisAlignedBox2f box = new AxisAlignedBox2f(); for (int i = 0; i < m_vChildren.Count; ++i) box.Contain(m_vChildren[i].Bounds); return box; diff --git a/implicit/MarchingQuads.cs b/implicit/MarchingQuads.cs index 6257cb33..185a78e1 100644 --- a/implicit/MarchingQuads.cs +++ b/implicit/MarchingQuads.cs @@ -11,7 +11,7 @@ public class MarchingQuads { PolyLine m_stroke; - Box2f m_bounds; + AxisAlignedBox2f m_bounds; float m_fXShift; float m_fYShift; float m_fScale; @@ -72,9 +72,9 @@ struct SeedPoint { bool[] m_bEdgeSigns; - public MarchingQuads(int nSubdivisions, Box2f bounds, float fIsoValue) { + public MarchingQuads(int nSubdivisions, AxisAlignedBox2f bounds, float fIsoValue) { m_stroke = new PolyLine(); - m_bounds = new Box2f(); + m_bounds = new AxisAlignedBox2f(); m_nCells = nSubdivisions; SetBounds(bounds); @@ -95,7 +95,7 @@ public int Subdivisions { set { m_nCells = value; SetBounds( m_bounds ); InitializeCells(); } } - public Box2f Bounds { + public AxisAlignedBox2f Bounds { get { return m_bounds; } set { SetBounds(value); } } @@ -105,7 +105,7 @@ public PolyLine Stroke { } - public Box2f GetBounds() { + public AxisAlignedBox2f GetBounds() { return m_bounds; } @@ -437,7 +437,7 @@ void InitializeCells() { } } - void SetBounds( Box2f bounds ) { + void SetBounds( AxisAlignedBox2f bounds ) { m_bounds = bounds; m_fXShift = (bounds.Min.x < 0) ? bounds.Min.x : -bounds.Min.x; diff --git a/math/Box2f.cs b/math/AxisAlignedBox2f.cs similarity index 86% rename from math/Box2f.cs rename to math/AxisAlignedBox2f.cs index 95e10457..f8b645ba 100644 --- a/math/Box2f.cs +++ b/math/AxisAlignedBox2f.cs @@ -2,50 +2,50 @@ namespace g3 { - public struct Box2f + public struct AxisAlignedBox2f { public Vector2f Min; public Vector2f Max; - public static Box2f Empty = new Box2f(false); - public static Box2f Infinite = new Box2f(Single.MinValue, Single.MinValue, Single.MaxValue, Single.MaxValue); + public static AxisAlignedBox2f Empty = new AxisAlignedBox2f(false); + public static AxisAlignedBox2f Infinite = new AxisAlignedBox2f(Single.MinValue, Single.MinValue, Single.MaxValue, Single.MaxValue); - public Box2f(bool bIgnore) { + public AxisAlignedBox2f(bool bIgnore) { Min = new Vector2f(Single.MaxValue, Single.MaxValue); Max = new Vector2f(Single.MinValue, Single.MinValue); } - public Box2f(float xmin, float ymin, float xmax, float ymax) { + public AxisAlignedBox2f(float xmin, float ymin, float xmax, float ymax) { Min = new Vector2f(xmin, ymin); Max = new Vector2f(xmax, ymax); } - public Box2f(float fSquareSize) { + public AxisAlignedBox2f(float fSquareSize) { Min = new Vector2f(0, 0); Max = new Vector2f(fSquareSize, fSquareSize); } - public Box2f(float fWidth, float fHeight) { + public AxisAlignedBox2f(float fWidth, float fHeight) { Min = new Vector2f(0, 0); Max = new Vector2f(fWidth, fHeight); } - public Box2f(Vector2f vMin, Vector2f vMax) { + public AxisAlignedBox2f(Vector2f vMin, Vector2f vMax) { Min = new Vector2f(Math.Min(vMin.x, vMax.x), Math.Min(vMin.y, vMax.y)); Max = new Vector2f(Math.Max(vMin.x, vMax.x), Math.Max(vMin.y, vMax.y)); } - public Box2f(Vector2f vCenter, float fHalfWidth, float fHalfHeight) { + public AxisAlignedBox2f(Vector2f vCenter, float fHalfWidth, float fHalfHeight) { Min = new Vector2f(vCenter.x - fHalfWidth, vCenter.y - fHalfHeight); Max = new Vector2f(vCenter.x + fHalfWidth, vCenter.y + fHalfHeight); } - public Box2f(Vector2f vCenter, float fHalfWidth) { + public AxisAlignedBox2f(Vector2f vCenter, float fHalfWidth) { Min = new Vector2f(vCenter.x - fHalfWidth, vCenter.y - fHalfWidth); Max = new Vector2f(vCenter.x + fHalfWidth, vCenter.y + fHalfWidth); } - public Box2f(Box2f o) { + public AxisAlignedBox2f(AxisAlignedBox2f o) { Min = new Vector2f(o.Min); Max = new Vector2f(o.Max); } @@ -145,17 +145,17 @@ public void Contain(Vector2f v) { Max.y = Math.Max(Max.y, v.y); } - public void Contain(Box2f box) { + public void Contain(AxisAlignedBox2f box) { Contain(box.Min); Contain(box.Max); } - public Box2f Intersect(Box2f box) { - Box2f intersect = new Box2f( + public AxisAlignedBox2f Intersect(AxisAlignedBox2f box) { + AxisAlignedBox2f intersect = new AxisAlignedBox2f( Math.Max(Min.x, box.Min.x), Math.Max(Min.y, box.Min.y), Math.Min(Max.x, box.Max.x), Math.Min(Max.y, box.Max.y)); if (intersect.Height <= 0 || intersect.Width <= 0) - return Box2f.Empty; + return AxisAlignedBox2f.Empty; else return intersect; } @@ -165,7 +165,7 @@ public Box2f Intersect(Box2f box) { public bool Contains(Vector2f v) { return (Min.x < v.x) && (Min.y < v.y) && (Max.x > v.x) && (Max.y > v.y); } - public bool Intersects(Box2f box) { + public bool Intersects(AxisAlignedBox2f box) { return !((box.Max.x < Min.x) || (box.Min.x > Max.x) || (box.Max.y < Min.y) || (box.Min.y > Max.y)); } diff --git a/math/AxisAlignedBox3d.cs b/math/AxisAlignedBox3d.cs new file mode 100644 index 00000000..e48dd048 --- /dev/null +++ b/math/AxisAlignedBox3d.cs @@ -0,0 +1,190 @@ +using System; + +namespace g3 +{ + public struct AxisAlignedBox3d + { + public Vector3d Min; + public Vector3d Max; + + public static AxisAlignedBox3d Empty = new AxisAlignedBox3d(false); + public static AxisAlignedBox3d Infinite = + new AxisAlignedBox3d(Double.MinValue, Double.MinValue, Double.MinValue, Double.MaxValue, Double.MaxValue, Double.MaxValue); + + + public AxisAlignedBox3d(bool bIgnore) { + Min = new Vector3d(Double.MaxValue, Double.MaxValue, Double.MaxValue); + Max = new Vector3d(Double.MinValue, Double.MinValue, Double.MinValue); + } + + public AxisAlignedBox3d(double xmin, double ymin, double zmin, double xmax, double ymax, double zmax) { + Min = new Vector3d(xmin, ymin, zmin); + Max = new Vector3d(xmax, ymax, zmax); + } + + public AxisAlignedBox3d(double fCubeSize) { + Min = new Vector3d(0, 0, 0); + Max = new Vector3d(fCubeSize, fCubeSize, fCubeSize); + } + + public AxisAlignedBox3d(double fWidth, double fHeight, double fDepth) { + Min = new Vector3d(0, 0, 0); + Max = new Vector3d(fWidth, fHeight, fDepth); + } + + public AxisAlignedBox3d(Vector3d vMin, Vector3d vMax) { + Min = new Vector3d(Math.Min(vMin.x, vMax.x), Math.Min(vMin.y, vMax.y), Math.Min(vMin.z, vMax.z)); + Max = new Vector3d(Math.Max(vMin.x, vMax.x), Math.Max(vMin.y, vMax.y), Math.Max(vMin.z, vMax.z)); + } + + public AxisAlignedBox3d(Vector3d vCenter, double fHalfWidth, double fHalfHeight, double fHalfDepth) { + Min = new Vector3d(vCenter.x - fHalfWidth, vCenter.y - fHalfHeight, vCenter.z - fHalfDepth); + Max = new Vector3d(vCenter.x + fHalfWidth, vCenter.y + fHalfHeight, vCenter.z + fHalfDepth); + } + public AxisAlignedBox3d(Vector3d vCenter, double fHalfSize) { + Min = new Vector3d(vCenter.x - fHalfSize, vCenter.y - fHalfSize, vCenter.z - fHalfSize); + Max = new Vector3d(vCenter.x + fHalfSize, vCenter.y + fHalfSize, vCenter.z + fHalfSize); + } + + public double Width { + get { return Max.x - Min.x; } + } + public double Height { + get { return Max.y - Min.y; } + } + public double Depth + { + get { return Max.z - Min.z; } + } + + public double Volume { + get { return Width * Height * Depth; } + } + public double DiagonalLength { + get { return (double)Math.Sqrt((Max.x - Min.x) * (Max.x - Min.x) + + (Max.y - Min.y) * (Max.y - Min.y) + (Max.z - Min.z) * (Max.z - Min.z)); } + } + public double MaxDim { + get { return Math.Max(Width, Math.Max(Height, Depth)); } + } + + public Vector3d Diagonal + { + get { return new Vector3d(Max.x - Min.x, Max.y - Min.y, Max.z-Min.z); } + } + public Vector3d Extents + { + get { return new Vector3d((Max.x - Min.x)*0.5, (Max.y - Min.y)*0.5, (Max.z - Min.z)*0.5); } + } + public Vector3d Center { + get { return new Vector3d(0.5 * (Min.x + Max.x), 0.5 * (Min.y + Max.y), 0.5 * (Min.z + Max.z)); } + } + + // TODO + ////! 0 == bottom-left, 1 = bottom-right, 2 == top-right, 3 == top-left + //public Vector3d GetCorner(int i) { + // return new Vector3d((i % 3 == 0) ? Min.x : Max.x, (i < 2) ? Min.y : Max.y); + //} + + //! value is subtracted from min and added to max + public void Expand(double fRadius) { + Min.x -= fRadius; Min.y -= fRadius; Min.z -= fRadius; + Max.x += fRadius; Max.y += fRadius; Max.z += fRadius; + } + //! value is added to min and subtracted from max + public void Contract(double fRadius) { + Min.x += fRadius; Min.y += fRadius; Min.z += fRadius; + Max.x -= fRadius; Max.y -= fRadius; Max.z -= fRadius; + } + + public void Contain(Vector3d v) { + Min.x = Math.Min(Min.x, v.x); + Min.y = Math.Min(Min.y, v.y); + Min.z = Math.Min(Min.z, v.z); + Max.x = Math.Max(Max.x, v.x); + Max.y = Math.Max(Max.y, v.y); + Max.z = Math.Max(Max.z, v.z); + } + + public void Contain(AxisAlignedBox3d box) { + Contain(box.Min); + Contain(box.Max); + } + + public AxisAlignedBox3d Intersect(AxisAlignedBox3d box) { + AxisAlignedBox3d intersect = new AxisAlignedBox3d( + Math.Max(Min.x, box.Min.x), Math.Max(Min.y, box.Min.y), Math.Max(Min.z, box.Min.z), + Math.Min(Max.x, box.Max.x), Math.Min(Max.y, box.Max.y), Math.Min(Max.z, box.Max.z)); + if (intersect.Height <= 0 || intersect.Width <= 0 || intersect.Depth <= 0) + return AxisAlignedBox3d.Empty; + else + return intersect; + } + + + + public bool Contains(Vector3d v) { + return (Min.x <= v.x) && (Min.y <= v.y) + && (Max.x >= v.x) && (Max.y >= v.y) + && (Max.z >= v.z) && (Max.z >= v.z); + } + public bool Intersects(AxisAlignedBox3d box) { + return !((box.Max.x <= Min.x) || (box.Min.x >= Max.x) + || (box.Max.y <= Min.y) || (box.Min.y >= Max.y) + || (box.Max.z <= Min.z) || (box.Min.z >= Max.z) ); + } + + + + // [TODO] we have handled corner cases, but not edge cases! + // those are 2D, so it would be like (dx > width && dy > height) + //public double Distance(Vector3d v) + //{ + // double dx = (double)Math.Abs(v.x - Center.x); + // double dy = (double)Math.Abs(v.y - Center.y); + // double dz = (double)Math.Abs(v.z - Center.z); + // double fWidth = Width * 0.5; + // double fHeight = Height * 0.5; + // double fDepth = Depth * 0.5; + // if (dx < fWidth && dy < fHeight && dz < Depth) + // return 0.0f; + // else if (dx > fWidth && dy > fHeight && dz > fDepth) + // return (double)Math.Sqrt((dx - fWidth) * (dx - fWidth) + (dy - fHeight) * (dy - fHeight) + (dz - fDepth) * (dz - fDepth)); + // else if (dx > fWidth) + // return dx - fWidth; + // else if (dy > fHeight) + // return dy - fHeight; + // else if (dz > fDepth) + // return dz - fDepth; + // return 0.0f; + //} + + + //! relative translation + public void Translate(Vector3d vTranslate) { + Min.Add(vTranslate); + Max.Add(vTranslate); + } + + public void MoveMin(Vector3d vNewMin) { + Max.x = vNewMin.x + (Max.x - Min.x); + Max.y = vNewMin.y + (Max.y - Min.y); + Max.z = vNewMin.z + (Max.z - Min.z); + Min.Set(vNewMin); + } + public void MoveMin(double fNewX, double fNewY, double fNewZ) { + Max.x = fNewX + (Max.x - Min.x); + Max.y = fNewY + (Max.y - Min.y); + Max.z = fNewZ + (Max.z - Min.z); + Min.Set(fNewX, fNewY, fNewZ); + } + + + + public override string ToString() { + return string.Format("x[{0:F8},{1:F8}] y[{2:F8},{3:F8}] z[{2:F8},{3:F8}]", Min.x, Max.x, Min.y, Max.y, Min.z, Max.z); + } + + + } +} diff --git a/math/Ray3.cs b/math/Ray3.cs new file mode 100644 index 00000000..1918f3f9 --- /dev/null +++ b/math/Ray3.cs @@ -0,0 +1,71 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +#if G3_USING_UNITY +using UnityEngine; +#endif + +namespace g3 +{ + public class Ray3d + { + public Vector3d Origin; + public Vector3d Direction; + + public Ray3d(Vector3d origin, Vector3d direction) + { + this.Origin = origin; + this.Direction = direction; + } + + // parameter is distance along ray + public Vector3d PointAt(double d) { + return Origin + d * Direction; + } + +#if G3_USING_UNITY + public static implicit operator Ray3d(UnityEngine.Ray r) + { + return new Ray3d(r.origin, r.direction); + } + public static explicit operator Ray(Ray3d r) + { + return new Ray((Vector3)r.Origin, (Vector3)r.Direction); + } +#endif + + } + + + + public class Ray3f + { + public Vector3f Origin; + public Vector3f Direction; + + public Ray3f(Vector3f origin, Vector3f direction) + { + this.Origin = origin; + this.Direction = direction; + } + + // parameter is distance along ray + public Vector3f PointAt(float d) + { + return Origin + d * Direction; + } + +#if G3_USING_UNITY + public static implicit operator Ray3f(UnityEngine.Ray r) + { + return new Ray3f(r.origin, r.direction); + } + public static implicit operator Ray(Ray3f r) + { + return new Ray(r.Origin, r.Direction); + } +#endif + } +} diff --git a/math/Segment2.cs b/math/Segment2.cs new file mode 100644 index 00000000..16c3ced4 --- /dev/null +++ b/math/Segment2.cs @@ -0,0 +1,82 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class Segment2d + { + // Center-direction-extent representation. + public Vector2d Center; + public Vector2d Direction; + public double Extent; + + public Segment2d(Vector2d p0, Vector2d p1) + { + update_from_endpoints(p0, p1); + } + public Segment2d(Vector2d center, Vector2d direction, double extent) + { + Center = center; Direction = direction; Extent = extent; + } + + public Vector2d P0 + { + get { return Center - Extent * Direction; } + set { update_from_endpoints(value, P1); } + } + public Vector2d P1 + { + get { return Center + Extent * Direction; } + set { update_from_endpoints(P0, value); } + } + + + void update_from_endpoints(Vector2d p0, Vector2d p1) + { + Center = 0.5 * (p0 + p1); + Direction = p1 - p0; + Extent = 0.5 * Direction.Normalize(); + } + } + + + + public class Segment2f + { + // Center-direction-extent representation. + public Vector2f Center; + public Vector2f Direction; + public float Extent; + + public Segment2f(Vector2f p0, Vector2f p1) + { + update_from_endpoints(p0, p1); + } + public Segment2f(Vector2f center, Vector2f direction, float extent) + { + Center = center; Direction = direction; Extent = extent; + } + + public Vector2f P0 + { + get { return Center - Extent * Direction; } + set { update_from_endpoints(value, P1); } + } + public Vector2f P1 + { + get { return Center + Extent * Direction; } + set { update_from_endpoints(P0, value); } + } + + + void update_from_endpoints(Vector2f p0, Vector2f p1) + { + Center = 0.5f * (p0 + p1); + Direction = p1 - p0; + Extent = 0.5f * Direction.Normalize(); + } + } + +} diff --git a/math/Segment3.cs b/math/Segment3.cs new file mode 100644 index 00000000..8325361c --- /dev/null +++ b/math/Segment3.cs @@ -0,0 +1,110 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class Segment3d + { + // Center-direction-extent representation. + public Vector3d Center; + public Vector3d Direction; + public double Extent; + + public Segment3d(Vector3d p0, Vector3d p1) { + update_from_endpoints(p0, p1); + } + public Segment3d(Vector3d center, Vector3d direction, double extent) { + Center = center; Direction = direction; Extent = extent; + } + + public void SetEndpoints(Vector3d p0, Vector3d p1) { + update_from_endpoints(p0, p1); + } + + + public Vector3d P0 { + get { return Center - Extent * Direction; } + set { update_from_endpoints(value, P1); } + } + public Vector3d P1 { + get { return Center + Extent * Direction; } + set { update_from_endpoints(P0, value); } + } + + // parameter is signed distance from center in direction + public Vector3d PointAt(double d) { + return Center + d * Direction; + } + + // t ranges from [0,1] over [P0,P1] + public Vector3d PointBetween(double t) { + return Center + (2 * t - 1) * Extent * Direction; + } + + + void update_from_endpoints(Vector3d p0, Vector3d p1) { + Center = 0.5 * (p0 + p1); + Direction = p1 - p0; + Extent = 0.5* Direction.Normalize(); + } + } + + + + public class Segment3f + { + // Center-direction-extent representation. + public Vector3f Center; + public Vector3f Direction; + public float Extent; + + public Segment3f(Vector3f p0, Vector3f p1) + { + update_from_endpoints(p0, p1); + } + public Segment3f(Vector3f center, Vector3f direction, float extent) + { + Center = center; Direction = direction; Extent = extent; + } + + + public void SetEndpoints(Vector3f p0, Vector3f p1) { + update_from_endpoints(p0, p1); + } + + + public Vector3f P0 + { + get { return Center - Extent * Direction; } + set { update_from_endpoints(value, P1); } + } + public Vector3f P1 + { + get { return Center + Extent * Direction; } + set { update_from_endpoints(P0, value); } + } + + // parameter is signed distance from center in direction + public Vector3f PointAt(float d) { + return Center + d * Direction; + } + + + // t ranges from [0,1] over [P0,P1] + public Vector3f PointBetween(float t) { + return Center + (2 * t - 1) * Extent * Direction; + } + + + + void update_from_endpoints(Vector3f p0, Vector3f p1) + { + Center = 0.5f * (p0 + p1); + Direction = p1 - p0; + Extent = 0.5f * Direction.Normalize(); + } + } + +} diff --git a/math/Vector2d.cs b/math/Vector2d.cs index 62e943b0..53bda54a 100644 --- a/math/Vector2d.cs +++ b/math/Vector2d.cs @@ -124,5 +124,15 @@ public override string ToString() { return string.Format("{0:F8} {1:F8}", v[0], v[1]); } + + public static implicit operator Vector2d(Vector2f v) + { + return new Vector2d(v[0], v[1]); + } + public static explicit operator Vector2f(Vector2d v) + { + return new Vector2f((float)v[0], (float)v[1]); + } + } } diff --git a/math/Vector3d.cs b/math/Vector3d.cs index 77966a78..13016e01 100644 --- a/math/Vector3d.cs +++ b/math/Vector3d.cs @@ -170,6 +170,16 @@ public virtual string ToString(string fmt) { + public static implicit operator Vector3d(Vector3f v) + { + return new Vector3d(v[0], v[1], v[2]); + } + public static explicit operator Vector3f(Vector3d v) + { + return new Vector3f((float)v[0], (float)v[1], (float)v[2]); + } + + #if G3_USING_UNITY public static implicit operator Vector3d(UnityEngine.Vector3 v) { diff --git a/queries/DistRay3Segment3.cs b/queries/DistRay3Segment3.cs new file mode 100644 index 00000000..1587ea23 --- /dev/null +++ b/queries/DistRay3Segment3.cs @@ -0,0 +1,176 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + class DistRay3Segment3 + { + Ray3d ray; + public Ray3d Ray + { + get { return ray; } + set { ray = value; DistanceSquared = -1.0; } + } + + Segment3d segment; + public Segment3d Segment + { + get { return segment; } + set { segment = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d RayClosest; + public double RayParameter; + public Vector3d SegmentClosest; + public double SegmentParameter; + + + public DistRay3Segment3(Ray3d ray, Segment3d segment) + { + this.ray = ray; this.segment = segment; + } + + // have to do this if you are changing Ray/Segment yourself and want to reuse this object + public void Reset() { + DistanceSquared = -1.0f; + } + + public double Get() { + return Math.Sqrt(GetSquared()); + } + + public double GetSquared() + { + if (DistanceSquared > 0) + return DistanceSquared; + + Vector3d diff = ray.Origin - segment.Center; + double a01 = -ray.Direction.Dot(segment.Direction); + double b0 = diff.Dot(ray.Direction); + double b1 = -diff.Dot(segment.Direction); + double c = diff.LengthSquared; + double det = Math.Abs(1 - a01 * a01); + double s0, s1, sqrDist, extDet; + + if (det >= MathUtil.ZeroTolerance) { + // The Ray and Segment are not parallel. + s0 = a01 * b1 - b0; + s1 = a01 * b0 - b1; + extDet = segment.Extent * det; + + if (s0 >= 0) { + if (s1 >= -extDet) { + if (s1 <= extDet) // region 0 + { + // Minimum at interior points of Ray and Segment. + double invDet = (1) / det; + s0 *= invDet; + s1 *= invDet; + sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) + + s1 * (a01 * s0 + s1 + (2) * b1) + c; + } else // region 1 + { + s1 = segment.Extent; + s0 = -(a01 * s1 + b0); + if (s0 > 0) { + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } else { + s0 = 0; + sqrDist = s1 * (s1 + (2) * b1) + c; + } + } + } else // region 5 + { + s1 = -segment.Extent; + s0 = -(a01 * s1 + b0); + if (s0 > 0) { + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } else { + s0 = 0; + sqrDist = s1 * (s1 + (2) * b1) + c; + } + } + } else { + if (s1 <= -extDet) // region 4 + { + s0 = -(-a01 * segment.Extent + b0); + if (s0 > 0) { + s1 = -segment.Extent; + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } else { + s0 = 0; + s1 = -b1; + if (s1 < -segment.Extent) { + s1 = -segment.Extent; + } else if (s1 > segment.Extent) { + s1 = segment.Extent; + } + sqrDist = s1 * (s1 + (2) * b1) + c; + } + } else if (s1 <= extDet) // region 3 + { + s0 = 0; + s1 = -b1; + if (s1 < -segment.Extent) { + s1 = -segment.Extent; + } else if (s1 > segment.Extent) { + s1 = segment.Extent; + } + sqrDist = s1 * (s1 + (2) * b1) + c; + } else // region 2 + { + s0 = -(a01 * segment.Extent + b0); + if (s0 > 0) { + s1 = segment.Extent; + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } else { + s0 = 0; + s1 = -b1; + if (s1 < -segment.Extent) { + s1 = -segment.Extent; + } else if (s1 > segment.Extent) { + s1 = segment.Extent; + } + sqrDist = s1 * (s1 + (2) * b1) + c; + } + } + } + } else { + // Ray and Segment are parallel. + if (a01 > 0) { + // Opposite direction vectors. + s1 = -segment.Extent; + } else { + // Same direction vectors. + s1 = segment.Extent; + } + + s0 = -(a01 * s1 + b0); + if (s0 > 0) { + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } else { + s0 = 0; + sqrDist = s1 * (s1 + (2) * b1) + c; + } + } + + RayClosest = ray.Origin + s0 * ray.Direction; + SegmentClosest = segment.Center + s1 * segment.Direction; + RayParameter = s0; + SegmentParameter = s1; + + // Account for numerical round-off errors. + if (sqrDist < 0) { + sqrDist = 0; + } + DistanceSquared = sqrDist; + return DistanceSquared; + } + + + } +} diff --git a/queries/RayIntersection.cs b/queries/RayIntersection.cs index e24a371e..4f149ddc 100644 --- a/queries/RayIntersection.cs +++ b/queries/RayIntersection.cs @@ -44,6 +44,25 @@ public static bool SphereSigned(Vector3f vOrigin, Vector3f vDirection, Vector3f + public static bool SphereSigned(Vector3d vOrigin, Vector3d vDirection, Vector3d vCenter, double fRadius, out double fRayT) + { + fRayT = 0.0; + Vector3d m = vOrigin - vCenter; + double b = m.Dot(vDirection); + double c = m.Dot(m) - fRadius * fRadius; + + // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) + if (c > 0.0f && b > 0.0f) + return false; + double discr = b * b - c; + // A negative discriminant corresponds to ray missing sphere + if (discr < 0.0) + return false; + // Ray now found to intersect sphere, compute smallest t value of intersection + fRayT = -b - Math.Sqrt(discr); + return true; + } + public static bool InfiniteCylinder(Vector3f vOrigin, Vector3f vDirection, Vector3f vCylOrigin, Vector3f vCylAxis, float fRadius, out float fRayT) {