public static void GetVelocityOfPoint(ref Vector3 point, ref Vector3 center, ref Vector3 linearVelocity, ref Vector3 angularVelocity, out Vector3 velocity)
{
Vector3 offset = point - center;
Vector3x.Cross(ref angularVelocity, ref offset, out velocity);
velocity += linearVelocity;
}
public float Determinant()
{
//Current implementation of cross far from optimal without shuffles. This assumes it'll eventually be accelerated.
Vector3 cross;
Vector3x.Cross(ref Y, ref Z, out cross);
return(Vector3.Dot(X, cross));
}
private static void FindNormal(ref QuickList <int> indices, ref QuickList <Vector3> points, int triangleIndex, out Vector3 normal)
{
var a = points.Elements[indices.Elements[triangleIndex]];
Vector3 ab = points.Elements[indices.Elements[triangleIndex + 1]] - a;
Vector3 ac = points.Elements[indices.Elements[triangleIndex + 2]] - a;
Vector3x.Cross(ref ac, ref ab, out normal);
}
private static bool IsTriangleVisibleFromPoint(ref QuickList <int> indices, ref QuickList <Vector3> points, int triangleIndex, ref Vector3 point)
{
//Compute the normal of the triangle.
var a = points.Elements[indices.Elements[triangleIndex]];
Vector3 ab = points.Elements[indices.Elements[triangleIndex + 1]] - a;
Vector3 ac = points.Elements[indices.Elements[triangleIndex + 2]] - a;
Vector3 normal;
Vector3x.Cross(ref ac, ref ab, out normal);
//Assume a consistent winding. Check to see if the normal points at the point.
Vector3 offset = point - a;
float dot = Vector3.Dot(offset, normal);
return(dot >= 0);
}
public static void Invert(ref Matrix3x3 m, out Matrix3x3 inverse)
{
//Current implementation of cross far from optimal without shuffles, and even then this has some room for improvement.
//Inverts should be really rare, so it's not too concerning. Use the scalar version when possible until ryujit improves (and we improve this implementation).
Vector3 yz, zx, xy;
Vector3x.Cross(ref m.Y, ref m.Z, out yz);
Vector3x.Cross(ref m.Z, ref m.X, out zx);
Vector3x.Cross(ref m.X, ref m.Y, out xy);
var inverseDeterminant = 1f / Vector3.Dot(m.X, yz);
inverse.X = yz * inverseDeterminant;
inverse.Y = zx * inverseDeterminant;
inverse.Z = xy * inverseDeterminant;
Transpose(ref inverse, out inverse);
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hitClockwise">True if the the triangle was hit on the clockwise face, false otherwise.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, ref Vector3 a, ref Vector3 b, ref Vector3 c, out bool hitClockwise, out RayHit hit)
{
hitClockwise = false;
hit = new RayHit();
Vector3 ab = b - a;
Vector3 ac = c - a;
Vector3x.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
{
return(false); //Degenerate triangle!
}
float d = -Vector3.Dot(ray.Direction, hit.Normal);
hitClockwise = d >= 0;
Vector3 ap = ray.Position - a;
hit.T = Vector3.Dot(ap, hit.Normal);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
{
return(false);//Hit is behind origin, or too far away.
}
hit.Location = ray.Position + hit.T * ray.Direction;
// Compute barycentric coordinates
ap = hit.Location - a;
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
ABdotAB = Vector3.Dot(ab, ab);
ABdotAC = Vector3.Dot(ab, ac);
ABdotAP = Vector3.Dot(ab, ap);
ACdotAC = Vector3.Dot(ac, ac);
ACdotAP = Vector3.Dot(ac, ap);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return((u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon));
}
/// <summary>
/// Computes the quaternion rotation between two normalized vectors.
/// </summary>
/// <param name="v1">First unit-length vector.</param>
/// <param name="v2">Second unit-length vector.</param>
/// <param name="q">Quaternion representing the rotation from v1 to v2.</param>
public static void GetQuaternionBetweenNormalizedVectors(ref Vector3 v1, ref Vector3 v2, out Quaternion q)
{
float dot = Vector3.Dot(v1, v2);
//For non-normal vectors, the multiplying the axes length squared would be necessary:
//float w = dot + (float)Math.Sqrt(v1.LengthSquared() * v2.LengthSquared());
if (dot < -0.9999f) //parallel, opposing direction
{
//If this occurs, the rotation required is ~180 degrees.
//The problem is that we could choose any perpendicular axis for the rotation. It's not uniquely defined.
//The solution is to pick an arbitrary perpendicular axis.
//Project onto the plane which has the lowest component magnitude.
//On that 2d plane, perform a 90 degree rotation.
float absX = Math.Abs(v1.X);
float absY = Math.Abs(v1.Y);
float absZ = Math.Abs(v1.Z);
if (absX < absY && absX < absZ)
{
q = new Quaternion(0, -v1.Z, v1.Y, 0);
}
else if (absY < absZ)
{
q = new Quaternion(-v1.Z, 0, v1.X, 0);
}
else
{
q = new Quaternion(-v1.Y, v1.X, 0, 0);
}
}
else
{
Vector3 axis;
Vector3x.Cross(ref v1, ref v2, out axis);
q = new Quaternion(axis.X, axis.Y, axis.Z, dot + 1);
}
q.Normalize();
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="sidedness">Sidedness of the triangle to test.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
{
hit = new RayHit();
Vector3 ab = b - a;
Vector3 ac = c - a;
Vector3x.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
{
return(false); //Degenerate triangle!
}
float d = -Vector3.Dot(ray.Direction, hit.Normal);
switch (sidedness)
{
case TriangleSidedness.DoubleSided:
if (d <= 0) //Pointing the wrong way. Flip the normal.
{
hit.Normal = -hit.Normal;
d = -d;
}
break;
case TriangleSidedness.Clockwise:
if (d <= 0) //Pointing the wrong way. Can't hit.
{
return(false);
}
break;
case TriangleSidedness.Counterclockwise:
if (d >= 0) //Pointing the wrong way. Can't hit.
{
return(false);
}
hit.Normal = -hit.Normal;
d = -d;
break;
}
Vector3 ap = ray.Position - a;
hit.T = Vector3.Dot(ap, hit.Normal);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
{
return(false);//Hit is behind origin, or too far away.
}
hit.Location = ray.Position + hit.T * ray.Direction;
// Compute barycentric coordinates
ap = hit.Location - a;
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
ABdotAB = Vector3.Dot(ab, ab);
ABdotAC = Vector3.Dot(ab, ac);
ABdotAP = Vector3.Dot(ab, ap);
ACdotAC = Vector3.Dot(ac, ac);
ACdotAP = Vector3.Dot(ac, ap);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return((u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon));
}
private static void ComputeInitialTetrahedron(ref QuickList <Vector3> points, ref QuickList <int> outsidePointCandidates, ref QuickList <int> triangleIndices, out Vector3 centroid)
{
//Find four points on the hull.
//We'll start with using the x axis to identify two points on the hull.
int a, b, c, d;
Vector3 direction;
//Find the extreme points along the x axis.
float minimumX = float.MaxValue, maximumX = -float.MaxValue;
int minimumXIndex = 0, maximumXIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
var v = points.Elements[i];
if (v.X > maximumX)
{
maximumX = v.X;
maximumXIndex = i;
}
else if (v.X < minimumX)
{
minimumX = v.X;
minimumXIndex = i;
}
}
a = minimumXIndex;
b = maximumXIndex;
//Check for redundancies..
if (a == b)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Now, use a second axis perpendicular to the two points we found.
Vector3 ab = points.Elements[b] - points.Elements[a];
Vector3x.Cross(ref ab, ref Toolbox.UpVector, out direction);
if (direction.LengthSquared() < Toolbox.Epsilon)
{
Vector3x.Cross(ref ab, ref Toolbox.RightVector, out direction);
}
float minimumDot, maximumDot;
int minimumIndex, maximumIndex;
GetExtremePoints(ref direction, ref points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the axis.
float dot = Vector3.Dot(direction, points.Elements[a]);
//Use the point further from the axis.
if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
c = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
c = maximumIndex;
}
//Check for redundancies..
if (a == c || b == c)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Use a third axis perpendicular to the plane defined by the three unique points a, b, and c.
Vector3 ac = points.Elements[c] - points.Elements[a];
Vector3x.Cross(ref ab, ref ac, out direction);
GetExtremePoints(ref direction, ref points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the plane.
dot = Vector3.Dot(direction, points.Elements[a]);
//Use the point further from the plane.
if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
d = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
d = maximumIndex;
}
//Check for redundancies..
if (a == d || b == d || c == d)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Add the triangles.
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(d);
triangleIndices.Add(a);
triangleIndices.Add(c);
triangleIndices.Add(d);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(d);
//The centroid is guaranteed to be within the convex hull. It will be used to verify the windings of triangles throughout the hull process.
centroid = (points.Elements[a] + points.Elements[b] + points.Elements[c] + points.Elements[d]) * 0.25f;
for (int i = 0; i < triangleIndices.Count; i += 3)
{
var vA = points.Elements[triangleIndices.Elements[i]];
var vB = points.Elements[triangleIndices.Elements[i + 1]];
var vC = points.Elements[triangleIndices.Elements[i + 2]];
//Check the signed volume of a parallelepiped with the edges of this triangle and the centroid.
Vector3 cross;
ab = vB - vA;
ac = vC - vA;
Vector3x.Cross(ref ac, ref ab, out cross);
Vector3 offset = vA - centroid;
float volume = Vector3.Dot(offset, cross);
//This volume/cross product could also be used to check for degeneracy, but we already tested for that.
if (Math.Abs(volume) < Toolbox.BigEpsilon)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
if (volume < 0)
{
//If the signed volume is negative, that means the triangle's winding is opposite of what we want.
//Flip it around!
var temp = triangleIndices.Elements[i];
triangleIndices.Elements[i] = triangleIndices.Elements[i + 1];
triangleIndices.Elements[i + 1] = temp;
}
}
//Points which belong to the tetrahedra are guaranteed to be 'in' the convex hull. Do not allow them to be considered.
var tetrahedronIndices = new QuickList <int>(BufferPools <int> .Locking);
tetrahedronIndices.Add(a);
tetrahedronIndices.Add(b);
tetrahedronIndices.Add(c);
tetrahedronIndices.Add(d);
//Sort the indices to allow a linear time loop.
Array.Sort(tetrahedronIndices.Elements, 0, 4);
int tetrahedronIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
if (tetrahedronIndex < 4 && i == tetrahedronIndices[tetrahedronIndex])
{
//Don't add a tetrahedron index. Now that we've found this index, though, move on to the next one.
++tetrahedronIndex;
}
else
{
outsidePointCandidates.Add(i);
}
}
tetrahedronIndices.Dispose();
}
