Skip to content

Commit

Permalink
utility functions, etc
Browse files Browse the repository at this point in the history
  • Loading branch information
@rms80
rms80 committed Apr 15, 2018
1 parent e3ccdad commit 717c98f
Show file tree
Hide file tree
Showing 11 changed files with 302 additions and 32 deletions.
8 changes: 8 additions & 0 deletions curve/CurveUtils.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Vector3d> Vertices { get { return VertexList; } }
}

Expand Down
27 changes: 22 additions & 5 deletions curve/DCurve3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -136,23 +139,37 @@ public Vector3d Start {
get { return vertices[0]; }
}
public Vector3d End {
get { return vertices.Last(); }
get { return (Closed) ? vertices[0] : vertices.Last(); }
}

public IEnumerable<Vector3d> Vertices {
get { return 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<Segment3d> 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);
}


Expand Down
2 changes: 2 additions & 0 deletions curve/ICurve.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Vector3d> Vertices { get; }
}
Expand Down
2 changes: 1 addition & 1 deletion math/Box2.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ public Vector2d ClosestPoint(Vector2d v)
}
}

return closest.x * AxisX + closest.y * AxisY;
return Center + closest.x*AxisX + closest.y*AxisY;
}


Expand Down
169 changes: 169 additions & 0 deletions math/Box3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -252,6 +259,168 @@ public void ScaleExtents(Vector3d s)
Extent *= s;
}




/// <summary>
/// Returns distance to box, or 0 if point is inside box.
/// Ported from WildMagic5 Wm5DistPoint3Box3.cpp
/// </summary>
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;
}



/// <summary>
/// Returns distance to box, or 0 if point is inside box.
/// Ported from WildMagic5 Wm5DistPoint3Box3.cpp
/// </summary>
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);
Expand Down
12 changes: 11 additions & 1 deletion math/Matrix3d.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
21 changes: 16 additions & 5 deletions math/Quaterniond.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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".
Expand Down
14 changes: 14 additions & 0 deletions math/Segment3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
Loading

0 comments on commit 717c98f

Please sign in to comment.