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(); }