From 717c98fba3a4c6d8c76a81da9205a34eecc78bff Mon Sep 17 00:00:00 2001 From: Ryan Schmidt Date: Sun, 15 Apr 2018 14:33:26 -0400 Subject: [PATCH] utility functions, etc --- curve/CurveUtils.cs | 8 ++ curve/DCurve3.cs | 27 ++++-- curve/ICurve.cs | 2 + math/Box2.cs | 2 +- math/Box3.cs | 169 +++++++++++++++++++++++++++++++++++++ math/Matrix3d.cs | 12 ++- math/Quaterniond.cs | 21 +++-- math/Segment3.cs | 14 +++ math/Vector3d.cs | 68 ++++++++++++--- queries/RayIntersection.cs | 8 +- spatial/PointAABBTree3.cs | 3 +- 11 files changed, 302 insertions(+), 32 deletions(-) diff --git a/curve/CurveUtils.cs b/curve/CurveUtils.cs index 87897553..663b6fbd 100644 --- a/curve/CurveUtils.cs +++ b/curve/CurveUtils.cs @@ -165,7 +165,15 @@ public class IWrappedCurve3d : ISampledCurve3d public bool Closed { get; set; } public int VertexCount { get { return (VertexList == null) ? 0 : VertexList.Count; } } + public int SegmentCount { get { return Closed ? VertexCount : VertexCount - 1; } } + public Vector3d GetVertex(int i) { return VertexList[i]; } + public Segment3d GetSegment(int iSegment) { + return (Closed) ? new Segment3d(VertexList[iSegment], VertexList[(iSegment + 1) % VertexList.Count]) + : new Segment3d(VertexList[iSegment], VertexList[iSegment + 1]); + } + + public IEnumerable Vertices { get { return VertexList; } } } diff --git a/curve/DCurve3.cs b/curve/DCurve3.cs index d46c4349..4378a8e5 100644 --- a/curve/DCurve3.cs +++ b/curve/DCurve3.cs @@ -75,6 +75,9 @@ public void AppendVertex(Vector3d v) { public int VertexCount { get { return vertices.Count; } } + public int SegmentCount { + get { return Closed ? vertices.Count : vertices.Count - 1; } + } public Vector3d GetVertex(int i) { return vertices[i]; @@ -136,7 +139,7 @@ public Vector3d Start { get { return vertices[0]; } } public Vector3d End { - get { return vertices.Last(); } + get { return (Closed) ? vertices[0] : vertices.Last(); } } public IEnumerable Vertices { @@ -144,15 +147,29 @@ public IEnumerable Vertices { } - public Segment3d Segment(int iSegment) + public Segment3d GetSegment(int iSegment) { - return new Segment3d(vertices[iSegment], vertices[iSegment + 1]); + return (Closed) ? new Segment3d(vertices[iSegment], vertices[(iSegment+1)%vertices.Count]) + : new Segment3d(vertices[iSegment], vertices[iSegment + 1]); } public IEnumerable SegmentItr() { - for (int i = 0; i < vertices.Count - 1; ++i) - yield return new Segment3d(vertices[i], vertices[i + 1]); + if (Closed) { + int NV = vertices.Count - 1; + for (int i = 0; i < NV; ++i) + yield return new Segment3d(vertices[i], vertices[i + 1]); + } else { + int NV = vertices.Count; + for (int i = 0; i < vertices.Count; ++i) + yield return new Segment3d(vertices[i], vertices[(i + 1)%NV]); + } + } + + public Vector3d PointAt(int iSegment, double fSegT) + { + Segment3d seg = new Segment3d(vertices[iSegment], vertices[(iSegment + 1) % vertices.Count]); + return seg.PointAt(fSegT); } diff --git a/curve/ICurve.cs b/curve/ICurve.cs index 8e59be2b..eaf106ea 100644 --- a/curve/ICurve.cs +++ b/curve/ICurve.cs @@ -31,9 +31,11 @@ public interface IParametricCurve3d public interface ISampledCurve3d { int VertexCount { get; } + int SegmentCount { get; } bool Closed { get; } Vector3d GetVertex(int i); + Segment3d GetSegment(int i); IEnumerable Vertices { get; } } diff --git a/math/Box2.cs b/math/Box2.cs index fdb95a31..08b59779 100644 --- a/math/Box2.cs +++ b/math/Box2.cs @@ -197,7 +197,7 @@ public Vector2d ClosestPoint(Vector2d v) } } - return closest.x * AxisX + closest.y * AxisY; + return Center + closest.x*AxisX + closest.y*AxisY; } diff --git a/math/Box3.cs b/math/Box3.cs index 80fb62d2..f0ab9ad3 100644 --- a/math/Box3.cs +++ b/math/Box3.cs @@ -53,6 +53,13 @@ public Box3d(Frame3f frame, Vector3d extent) AxisZ = frame.Z; Extent = extent; } + public Box3d(Segment3d seg) + { + Center = seg.Center; + AxisZ = seg.Direction; + Vector3d.MakePerpVectors(ref AxisZ, out AxisX, out AxisY); + Extent = new Vector3d(0, 0, seg.Extent); + } public static readonly Box3d Empty = new Box3d(Vector3d.Zero); public static readonly Box3d UnitZeroCentered = new Box3d(Vector3d.Zero, 0.5 * Vector3d.One); @@ -252,6 +259,168 @@ public void ScaleExtents(Vector3d s) Extent *= s; } + + + + /// + /// Returns distance to box, or 0 if point is inside box. + /// Ported from WildMagic5 Wm5DistPoint3Box3.cpp + /// + public double DistanceSquared(Vector3d v) + { + // Work in the box's coordinate system. + v -= this.Center; + + // Compute squared distance and closest point on box. + double sqrDistance = 0; + double delta; + Vector3d closest = new Vector3d(); + int i; + for (i = 0; i < 3; ++i) { + closest[i] = Axis(i).Dot(ref v); + if (closest[i] < -Extent[i]) { + delta = closest[i] + Extent[i]; + sqrDistance += delta * delta; + closest[i] = -Extent[i]; + } else if (closest[i] > Extent[i]) { + delta = closest[i] - Extent[i]; + sqrDistance += delta * delta; + closest[i] = Extent[i]; + } + } + + return sqrDistance; + } + + + + /// + /// Returns distance to box, or 0 if point is inside box. + /// Ported from WildMagic5 Wm5DistPoint3Box3.cpp + /// + public Vector3d ClosestPoint(Vector3d v) + { + // Work in the box's coordinate system. + v -= this.Center; + + // Compute squared distance and closest point on box. + double sqrDistance = 0; + double delta; + Vector3d closest = new Vector3d(); + for (int i = 0; i < 3; ++i) { + closest[i] = Axis(i).Dot(ref v); + double extent = Extent[i]; + if (closest[i] < -extent) { + delta = closest[i] + extent; + sqrDistance += delta * delta; + closest[i] = -extent; + } else if (closest[i] > extent) { + delta = closest[i] - extent; + sqrDistance += delta * delta; + closest[i] = extent; + } + } + + return Center + closest.x*AxisX + closest.y*AxisY + closest.z*AxisZ; + } + + + + + + // ported from WildMagic5 Wm5ContBox3.cpp::MergeBoxes + public static Box3d Merge(ref Box3d box0, ref Box3d box1) + { + // Construct a box that contains the input boxes. + Box3d box = new Box3d(); + + // The first guess at the box center. This value will be updated later + // after the input box vertices are projected onto axes determined by an + // average of box axes. + box.Center = 0.5 * (box0.Center + box1.Center); + + // A box's axes, when viewed as the columns of a matrix, form a rotation + // matrix. The input box axes are converted to quaternions. The average + // quaternion is computed, then normalized to unit length. The result is + // the slerp of the two input quaternions with t-value of 1/2. The result + // is converted back to a rotation matrix and its columns are selected as + // the merged box axes. + Quaterniond q0 = new Quaterniond(), q1 = new Quaterniond(); + Matrix3d rot0 = new Matrix3d(ref box0.AxisX, ref box0.AxisY, ref box0.AxisZ, false); + q0.SetFromRotationMatrix(ref rot0); + Matrix3d rot1 = new Matrix3d(ref box1.AxisX, ref box1.AxisY, ref box1.AxisZ, false); + q1.SetFromRotationMatrix(ref rot1); + if (q0.Dot(q1) < 0) { + q1 = -q1; + } + + Quaterniond q = q0 + q1; + double invLength = 1.0 / Math.Sqrt(q.Dot(q)); + q = q * invLength; + Matrix3d q_mat = q.ToRotationMatrix(); + box.AxisX = q_mat.Column(0); box.AxisY = q_mat.Column(1); box.AxisZ = q_mat.Column(2); //q.ToRotationMatrix(box.Axis); + + // Project the input box vertices onto the merged-box axes. Each axis + // D[i] containing the current center C has a minimum projected value + // min[i] and a maximum projected value max[i]. The corresponding end + // points on the axes are C+min[i]*D[i] and C+max[i]*D[i]. The point C + // is not necessarily the midpoint for any of the intervals. The actual + // box center will be adjusted from C to a point C' that is the midpoint + // of each interval, + // C' = C + sum_{i=0}^2 0.5*(min[i]+max[i])*D[i] + // The box extents are + // e[i] = 0.5*(max[i]-min[i]) + + int i, j; + double dot; + Vector3d[] vertex = new Vector3d[8]; + Vector3d pmin = Vector3d.Zero; + Vector3d pmax = Vector3d.Zero; + + box0.ComputeVertices(vertex); + for (i = 0; i < 8; ++i) { + Vector3d diff = vertex[i] - box.Center; + for (j = 0; j < 3; ++j) { + dot = box.Axis(j).Dot(ref diff); + if (dot > pmax[j]) { + pmax[j] = dot; + } else if (dot < pmin[j]) { + pmin[j] = dot; + } + } + } + + box1.ComputeVertices(vertex); + for (i = 0; i < 8; ++i) { + Vector3d diff = vertex[i] - box.Center; + for (j = 0; j < 3; ++j) { + dot = box.Axis(j).Dot(ref diff); + if (dot > pmax[j]) { + pmax[j] = dot; + } else if (dot < pmin[j]) { + pmin[j] = dot; + } + } + } + + // [min,max] is the axis-aligned box in the coordinate system of the + // merged box axes. Update the current box center to be the center of + // the new box. Compute the extents based on the new center. + for (j = 0; j < 3; ++j) { + box.Center += (0.5*(pmax[j] + pmin[j])) * box.Axis(j); + box.Extent[j] = 0.5*(pmax[j] - pmin[j]); + } + + return box; + } + + + + + + + + public static implicit operator Box3d(Box3f v) { return new Box3d(v.Center, v.AxisX, v.AxisY, v.AxisZ, v.Extent); diff --git a/math/Matrix3d.cs b/math/Matrix3d.cs index 7f6f3993..7fc052f2 100644 --- a/math/Matrix3d.cs +++ b/math/Matrix3d.cs @@ -68,7 +68,17 @@ public Matrix3d(Vector3d v1, Vector3d v2, Vector3d v3, bool bRows) Row2 = new Vector3d(v1.z, v2.z, v3.z); } } - public Matrix3d(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) { + public Matrix3d(ref Vector3d v1, ref Vector3d v2, ref Vector3d v3, bool bRows) + { + if (bRows) { + Row0 = v1; Row1 = v2; Row2 = v3; + } else { + Row0 = new Vector3d(v1.x, v2.x, v3.x); + Row1 = new Vector3d(v1.y, v2.y, v3.y); + Row2 = new Vector3d(v1.z, v2.z, v3.z); + } + } + public Matrix3d(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) { Row0 = new Vector3d(m00, m01, m02); Row1 = new Vector3d(m10, m11, m12); Row2 = new Vector3d(m20, m21, m22); diff --git a/math/Quaterniond.cs b/math/Quaterniond.cs index d608338a..205192f0 100644 --- a/math/Quaterniond.cs +++ b/math/Quaterniond.cs @@ -74,7 +74,9 @@ public double Dot(Quaterniond q2) { } - + public static Quaterniond operator -(Quaterniond q2) { + return new Quaterniond(-q2.x, -q2.y, -q2.z, -q2.w); + } public static Quaterniond operator*(Quaterniond a, Quaterniond b) { double w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; @@ -83,12 +85,19 @@ public double Dot(Quaterniond q2) { double z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x; return new Quaterniond(x, y, z, w); } - - + public static Quaterniond operator *(Quaterniond q1, double d) { + return new Quaterniond(d * q1.x, d * q1.y, d * q1.z, d * q1.w); + } + public static Quaterniond operator *(double d, Quaterniond q1) { + return new Quaterniond(d * q1.x, d * q1.y, d * q1.z, d * q1.w); + } public static Quaterniond operator -(Quaterniond q1, Quaterniond q2) { return new Quaterniond(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); } + public static Quaterniond operator +(Quaterniond q1, Quaterniond q2) { + return new Quaterniond(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); + } public static Vector3d operator *(Quaterniond q, Vector3d v) { //return q.ToRotationMatrix() * v; @@ -256,8 +265,10 @@ public static Quaterniond Slerp(Quaterniond p, Quaterniond q, double t) { } - - public void SetFromRotationMatrix(Matrix3d rot) + public void SetFromRotationMatrix(Matrix3d rot) { + SetFromRotationMatrix(ref rot); + } + public void SetFromRotationMatrix(ref Matrix3d rot) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". diff --git a/math/Segment3.cs b/math/Segment3.cs index f96947ab..4583ef90 100644 --- a/math/Segment3.cs +++ b/math/Segment3.cs @@ -61,6 +61,20 @@ public double DistanceSquared(Vector3d p) Vector3d proj = Center + t * Direction; return (proj - p).LengthSquared; } + public double DistanceSquared(Vector3d p, out double t) + { + t = (p - Center).Dot(Direction); + if (t >= Extent) { + t = Extent; + return P1.DistanceSquared(p); + } else if (t <= -Extent) { + t = -Extent; + return P0.DistanceSquared(p); + } + Vector3d proj = Center + t * Direction; + return (proj - p).LengthSquared; + } + public Vector3d NearestPoint(Vector3d p) { diff --git a/math/Vector3d.cs b/math/Vector3d.cs index 0c2ca2da..6884a450 100644 --- a/math/Vector3d.cs +++ b/math/Vector3d.cs @@ -356,9 +356,12 @@ public static explicit operator Vector3(Vector3d v) // complicated functions go down here... - // [RMS] this is from WildMagic5, but I added returning the minLength value - // from GTEngine, because I use this in place of GTEngine's Orthonormalize in - // ComputeOrthogonalComplement below + /// + /// Gram-Schmidt orthonormalization of the input vectors. + /// [RMS] this is from WildMagic5, but I added returning the minLength value + /// from GTEngine, because I use this in place of GTEngine's Orthonormalize in + /// ComputeOrthogonalComplement below + /// public static double Orthonormalize(ref Vector3d u, ref Vector3d v, ref Vector3d w) { // If the input vectors are v0, v1, and v2, then the Gram-Schmidt @@ -393,9 +396,11 @@ public static double Orthonormalize(ref Vector3d u, ref Vector3d v, ref Vector3d } - // Input W must be a unit-length vector. The output vectors {U,V} are - // unit length and mutually perpendicular, and {U,V,W} is an orthonormal basis. - // ported from WildMagic5 + /// + /// Input W must be a unit-length vector. The output vectors {U,V} are + /// unit length and mutually perpendicular, and {U,V,W} is an orthonormal basis. + /// ported from WildMagic5 + /// public static void GenerateComplementBasis(ref Vector3d u, ref Vector3d v, Vector3d w) { double invLength; @@ -421,14 +426,16 @@ public static void GenerateComplementBasis(ref Vector3d u, ref Vector3d v, Vecto } } - // this function is from GTEngine - // Compute a right-handed orthonormal basis for the orthogonal complement - // of the input vectors. The function returns the smallest length of the - // unnormalized vectors computed during the process. If this value is nearly - // zero, it is possible that the inputs are linearly dependent (within - // numerical round-off errors). On input, numInputs must be 1 or 2 and - // v0 through v(numInputs-1) must be initialized. On output, the - // vectors v0 through v2 form an orthonormal set. + /// + /// this function is ported from GTEngine. + /// Compute a right-handed orthonormal basis for the orthogonal complement + /// of the input vectors. The function returns the smallest length of the + /// unnormalized vectors computed during the process. If this value is nearly + /// zero, it is possible that the inputs are linearly dependent (within + /// numerical round-off errors). On input, numInputs must be 1 or 2 and + /// v0 through v(numInputs-1) must be initialized. On output, the + /// vectors v0 through v2 form an orthonormal set. + /// public static double ComputeOrthogonalComplement(int numInputs, Vector3d v0, ref Vector3d v1, ref Vector3d v2 /*, bool robust = false*/) { if (numInputs == 1) { @@ -451,5 +458,38 @@ public static double ComputeOrthogonalComplement(int numInputs, Vector3d v0, ref return 0; } + + + /// + /// Returns two vectors perpendicular to n, as efficiently as possible. + /// Duff et all method, from https://graphics.pixar.com/library/OrthonormalB/paper.pdf + /// + public static void MakePerpVectors(ref Vector3d n, out Vector3d b1, out Vector3d b2) + { + if (n.z < 0.0) { + double a = 1.0 / (1.0 - n.z); + double b = n.x * n.y * a; + //b1 = Vec3f(1.0f - n.x * n.x * a, -b, n.x); + //b2 = Vec3f(b, n.y * n.y * a - 1.0f, -n.y); + b1.x = 1.0f - n.x * n.x * a; + b1.y = -b; + b1.z = n.x; + b2.x = b; + b2.y = n.y * n.y * a - 1.0f; + b2.z = -n.y; + } else { + double a = 1.0 / (1.0 + n.z); + double b = -n.x * n.y * a; + //b1 = Vec3f(1.0 - n.x * n.x * a, b, -n.x); + //b2 = Vec3f(b, 1.0 - n.y * n.y * a, -n.y); + b1.x = 1.0 - n.x * n.x * a; + b1.y = b; + b1.z = -n.x; + b2.x = b; + b2.y = 1.0 - n.y * n.y * a; + b2.z = -n.y; + } + } + } } diff --git a/queries/RayIntersection.cs b/queries/RayIntersection.cs index a3d27cd6..1c60a980 100644 --- a/queries/RayIntersection.cs +++ b/queries/RayIntersection.cs @@ -15,12 +15,12 @@ private RayIntersection() // basic ray-sphere intersection public static bool Sphere(Vector3f vOrigin, Vector3f vDirection, Vector3f vCenter, float fRadius, out float fRayT) { - bool bHit = SphereSigned(vOrigin, vDirection, vCenter, fRadius, out fRayT); + bool bHit = SphereSigned(ref vOrigin, ref vDirection, ref vCenter, fRadius, out fRayT); fRayT = Math.Abs(fRayT); return bHit; } - public static bool SphereSigned(Vector3f vOrigin, Vector3f vDirection, Vector3f vCenter, float fRadius, out float fRayT) + public static bool SphereSigned(ref Vector3f vOrigin, ref Vector3f vDirection, ref Vector3f vCenter, float fRadius, out float fRayT) { fRayT = 0.0f; Vector3f m = vOrigin - vCenter; @@ -44,11 +44,11 @@ public static bool SphereSigned(Vector3f vOrigin, Vector3f vDirection, Vector3f - public static bool SphereSigned(Vector3d vOrigin, Vector3d vDirection, Vector3d vCenter, double fRadius, out double fRayT) + public static bool SphereSigned(ref Vector3d vOrigin, ref Vector3d vDirection, ref Vector3d vCenter, double fRadius, out double fRayT) { fRayT = 0.0; Vector3d m = vOrigin - vCenter; - double b = m.Dot(vDirection); + double b = m.Dot(ref 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) diff --git a/spatial/PointAABBTree3.cs b/spatial/PointAABBTree3.cs index 8b3edb3e..93b38841 100644 --- a/spatial/PointAABBTree3.cs +++ b/spatial/PointAABBTree3.cs @@ -7,8 +7,7 @@ namespace g3 { /// - /// Hierarchical Axis-Aligned-Bounding-Box tree for a DMesh3 mesh. - /// This class supports a variety of spatial queries, listed below. + /// Hierarchical Axis-Aligned-Bounding-Box tree for an IPointSet /// /// /// TODO: no timestamp support right now...