public MatrixF Translate(CoordinateF translation)
{
return(new MatrixF(Values[0], Values[1], Values[2], Values[3],
Values[4], Values[5], Values[6], Values[7],
Values[8], Values[9], Values[10], Values[11],
Values[12] + translation.X, Values[13] + translation.Y, Values[14] + translation.Z, Values[15]));
}
public CoordinateF ClosestPoint(CoordinateF point)
{
// http://paulbourke.net/geometry/pointline/
var delta = End - Start;
var den = delta.LengthSquared();
if (den == 0)
{
return(Start); // Start and End are the same
}
var numPoint = (point - Start).ComponentMultiply(delta);
var num = numPoint.X + numPoint.Y + numPoint.Z;
var u = num / den;
if (u < 0)
{
return(Start); // Point is before the segment start
}
if (u > 1)
{
return(End); // Point is after the segment end
}
return(Start + u * delta);
}
public bool EquivalentTo(CoordinateF test, float delta = 0.0001f)
{
var xd = Math.Abs(X - test.X);
var yd = Math.Abs(Y - test.Y);
var zd = Math.Abs(Z - test.Z);
return((xd < delta) && (yd < delta) && (zd < delta));
}
public CoordinateF Cross(CoordinateF that)
{
var xv = (Y * that.Z) - (Z * that.Y);
var yv = (Z * that.X) - (X * that.Z);
var zv = (X * that.Y) - (Y * that.X);
return(new CoordinateF(xv, yv, zv));
}
public CoordinateF Rotate(CoordinateF coord)
{
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
var q = new QuaternionF(coord.Normalise(), 0);
var temp = q * Conjugate();
return((this * temp).Vector);
}
public Coordinate(CoordinateF other)
{
_x = (decimal)other.X;
_y = (decimal)other.Y;
_z = (decimal)other.Z;
_dx = other.X;
_dy = other.Y;
_dy = other.Z;
}
public static MatrixF Scale(CoordinateF scale)
{
var m = Identity;
m.Values[0] = scale.X;
m.Values[5] = scale.Y;
m.Values[10] = scale.Z;
return(m);
}
public static MatrixF Translation(CoordinateF translation)
{
var m = Identity;
m.Values[12] = translation.X;
m.Values[13] = translation.Y;
m.Values[14] = translation.Z;
return(m);
}
public PlaneF(Plane p)
{
Normal = new CoordinateF(p.Normal);
DistanceFromOrigin = (float)p.DistanceFromOrigin;
PointOnPlane = new CoordinateF(p.PointOnPlane);
A = Normal.X;
B = Normal.Y;
C = Normal.Z;
D = -DistanceFromOrigin;
}
public PlaneF(CoordinateF norm, CoordinateF pointOnPlane)
{
Normal = norm.Normalise();
DistanceFromOrigin = Normal.Dot(pointOnPlane);
PointOnPlane = pointOnPlane;
A = Normal.X;
B = Normal.Y;
C = Normal.Z;
D = -DistanceFromOrigin;
}
public PlaneF(CoordinateF norm, float distanceFromOrigin)
{
Normal = norm.Normalise();
DistanceFromOrigin = distanceFromOrigin;
PointOnPlane = Normal * DistanceFromOrigin;
A = Normal.X;
B = Normal.Y;
C = Normal.Z;
D = -DistanceFromOrigin;
}
public bool Equals(CoordinateF other)
{
if (ReferenceEquals(null, other))
{
return(false);
}
if (ReferenceEquals(this, other))
{
return(true);
}
return(EquivalentTo(other));
}
public static MatrixF Rotation(CoordinateF axis, float angle)
{
var cos = (float)Math.Cos(-angle);
var sin = (float)Math.Sin(-angle);
var t = 1f - cos;
axis = axis.Normalise();
return(new MatrixF(t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y, 0,
t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X, 0,
t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos, 0,
0, 0, 0, 1));
}
public PlaneF(CoordinateF p1, CoordinateF p2, CoordinateF p3)
{
var ab = p2 - p1;
var ac = p3 - p1;
Normal = ac.Cross(ab).Normalise();
DistanceFromOrigin = Normal.Dot(p1);
PointOnPlane = p1;
A = Normal.X;
B = Normal.Y;
C = Normal.Z;
D = -DistanceFromOrigin;
}
public static QuaternionF EulerAngles(CoordinateF angles)
{
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternionF/index.htm
angles = angles / 2;
var sy = (float)Math.Sin(angles.Z);
var sp = (float)Math.Sin(angles.Y);
var sr = (float)Math.Sin(angles.X);
var cy = (float)Math.Cos(angles.Z);
var cp = (float)Math.Cos(angles.Y);
var cr = (float)Math.Cos(angles.X);
return(new QuaternionF(sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy));
}
public CoordinateF ComponentDivide(CoordinateF c)
{
if (Math.Abs(c.X - 0) < 0.0001)
{
c.X = 1;
}
if (Math.Abs(c.Y - 0) < 0.0001)
{
c.Y = 1;
}
if (Math.Abs(c.Z - 0) < 0.0001)
{
c.Z = 1;
}
return(new CoordinateF(X / c.X, Y / c.Y, Z / c.Z));
}
/// <summary>Finds if the given point is above, below, or on the plane.</summary>
/// <param name="co">The coordinate to test</param>
/// <param name="epsilon">Tolerance value</param>
/// <returns>
/// value == -1 if coordinate is below the plane<br />
/// value == 1 if coordinate is above the plane<br />
/// value == 0 if coordinate is on the plane.
/// </returns>
public int OnPlane(CoordinateF co, float epsilon = 0.5f)
{
//eval (s = Ax + By + Cz + D) at point (x,y,z)
//if s > 0 then point is "above" the plane (same side as normal)
//if s < 0 then it lies on the opposite side
//if s = 0 then the point (x,y,z) lies on the plane
var res = EvalAtPoint(co);
if (Math.Abs(res) < epsilon)
{
return(0);
}
if (res < 0)
{
return(-1);
}
return(1);
}
public BoxF(IEnumerable <BoxF> boxes)
{
if (!boxes.Any())
{
throw new Exception("Cannot create a bounding box out of zero other boxes.");
}
var min = new CoordinateF(float.MaxValue, float.MaxValue, float.MaxValue);
var max = new CoordinateF(float.MinValue, float.MinValue, float.MinValue);
foreach (var box in boxes)
{
min.X = Math.Min(box.Start.X, min.X);
min.Y = Math.Min(box.Start.Y, min.Y);
min.Z = Math.Min(box.Start.Z, min.Z);
max.X = Math.Max(box.End.X, max.X);
max.Y = Math.Max(box.End.Y, max.Y);
max.Z = Math.Max(box.End.Z, max.Z);
}
Start = min;
End = max;
Center = (Start + End) / 2;
}
public CoordinateF[][] GetBoxFaces()
{
var topLeftBack = new CoordinateF(Start.X, End.Y, End.Z);
var topRightBack = End.Clone();
var topLeftFront = new CoordinateF(Start.X, Start.Y, End.Z);
var topRightFront = new CoordinateF(End.X, Start.Y, End.Z);
var bottomLeftBack = new CoordinateF(Start.X, End.Y, Start.Z);
var bottomRightBack = new CoordinateF(End.X, End.Y, Start.Z);
var bottomLeftFront = Start.Clone();
var bottomRightFront = new CoordinateF(End.X, Start.Y, Start.Z);
return(new[]
{
new[] { topLeftFront, topRightFront, bottomRightFront, bottomLeftFront },
new[] { topRightBack, topLeftBack, bottomLeftBack, bottomRightBack },
new[] { topLeftBack, topLeftFront, bottomLeftFront, bottomLeftBack },
new[] { topRightFront, topRightBack, bottomRightBack, bottomRightFront },
new[] { topLeftBack, topRightBack, topRightFront, topLeftFront },
new[] { bottomLeftFront, bottomRightFront, bottomRightBack, bottomLeftBack }
});
}
public BoxF(IEnumerable <CoordinateF> coordinates)
{
if (!coordinates.Any())
{
throw new Exception("Cannot create a bounding box out of zero coordinates.");
}
var min = new CoordinateF(float.MaxValue, float.MaxValue, float.MaxValue);
var max = new CoordinateF(float.MinValue, float.MinValue, float.MinValue);
foreach (var vertex in coordinates)
{
min.X = Math.Min(vertex.X, min.X);
min.Y = Math.Min(vertex.Y, min.Y);
min.Z = Math.Min(vertex.Z, min.Z);
max.X = Math.Max(vertex.X, max.X);
max.Y = Math.Max(vertex.Y, max.Y);
max.Z = Math.Max(vertex.Z, max.Z);
}
Start = min;
End = max;
Center = (Start + End) / 2;
}
public IEnumerable <LineF> GetBoxLines()
{
var topLeftBack = new CoordinateF(Start.X, End.Y, End.Z);
var topRightBack = End.Clone();
var topLeftFront = new CoordinateF(Start.X, Start.Y, End.Z);
var topRightFront = new CoordinateF(End.X, Start.Y, End.Z);
var bottomLeftBack = new CoordinateF(Start.X, End.Y, Start.Z);
var bottomRightBack = new CoordinateF(End.X, End.Y, Start.Z);
var bottomLeftFront = Start.Clone();
var bottomRightFront = new CoordinateF(End.X, Start.Y, Start.Z);
yield return(new LineF(topLeftBack, topRightBack));
yield return(new LineF(topLeftFront, topRightFront));
yield return(new LineF(topLeftBack, topLeftFront));
yield return(new LineF(topRightBack, topRightFront));
yield return(new LineF(topLeftBack, bottomLeftBack));
yield return(new LineF(topLeftFront, bottomLeftFront));
yield return(new LineF(topRightBack, bottomRightBack));
yield return(new LineF(topRightFront, bottomRightFront));
yield return(new LineF(bottomLeftBack, bottomRightBack));
yield return(new LineF(bottomLeftFront, bottomRightFront));
yield return(new LineF(bottomLeftBack, bottomLeftFront));
yield return(new LineF(bottomRightBack, bottomRightFront));
}
public QuaternionF(CoordinateF vector, float scalar)
{
Vector = vector;
Scalar = scalar;
}
public CoordinateF ComponentMultiply(CoordinateF c)
{
return(new CoordinateF(X * c.X, Y * c.Y, Z * c.Z));
}
public float Dot(CoordinateF c)
{
return((X * c.X) + (Y * c.Y) + (Z * c.Z));
}
/// <summary>
/// Project a point into the space of this plane. I.e. Get the point closest
/// to the provided point that is on this plane.
/// </summary>
/// <param name="point">The point to project</param>
/// <returns>The point projected onto this plane</returns>
public CoordinateF Project(CoordinateF point)
{
// http://www.gamedev.net/topic/262196-projecting-vector-onto-a-plane/
// Projected = Point - ((Point - PointOnPlane) . Normal) * Normal
return(point - ((point - PointOnPlane).Dot(Normal)) * Normal);
}
public float EvalAtPoint(CoordinateF co)
{
return(A * co.X + B * co.Y + C * co.Z + D);
}
public static QuaternionF AxisAngle(CoordinateF axis, float angle)
{
return(Math.Abs(axis.VectorMagnitude()) < 0.0001
? Identity
: new QuaternionF(axis.Normalise() * (float)Math.Sin(angle / 2), (float)Math.Cos(angle / 2)).Normalise());
}
/// <summary>
/// Returns true if the given coordinate is inside this box.
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public bool CoordinateIsInside(CoordinateF c)
{
return(c.X >= Start.X && c.Y >= Start.Y && c.Z >= Start.Z &&
c.X <= End.X && c.Y <= End.Y && c.Z <= End.Z);
}
public QuaternionF(float x, float y, float z, float w)
{
Vector = new CoordinateF(x, y, z);
Scalar = w;
}
public BoxF(CoordinateF start, CoordinateF end)
{
Start = start;
End = end;
Center = (Start + End) / 2;
}
