/// <summary> /// Indicates if a segment intersects a triangle /// </summary> /// <param name="seg"></param> /// <param name="triangle"></param> /// <returns>bool</returns> public static bool SegmentTriangleOverlap(Segment seg, Triangle triangle) { // the parameters - if hit then they get copied into the args float u, v, t; Vector3 e1 = triangle.Edge0; Vector3 e2 = triangle.Edge1; Vector3 p = Vector3.Cross(seg.Delta, e2); float a = Vector3.Dot(e1, p); if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon) return false; float f = 1.0f / a; Vector3 s = seg.Origin - triangle.Origin; u = f * Vector3.Dot(s, p); if (u < 0.0f || u > 1.0f) return false; Vector3 q = Vector3.Cross(s, e1); v = f * Vector3.Dot(seg.Delta, q); if (v < 0.0f || (u + v) > 1.0f) return false; t = f * Vector3.Dot(e2, q); if (t < 0.0f || t > 1.0f) return false; return true; }
/// <summary> /// SegmentTriangleDistanceSq /// </summary> /// <param name="segT"></param> /// <param name="triT0"></param> /// <param name="triT1"></param> /// <param name="seg"></param> /// <param name="triangle"></param> /// <returns>float</returns> public static float SegmentTriangleDistanceSq(out float segT, out float triT0, out float triT1, Segment seg, Triangle triangle) { // compare segment to all three edges of the triangle float distSq = float.MaxValue; if (Intersection.SegmentTriangleIntersection(out segT, out triT0, out triT1, seg, triangle)) { segT = triT0 = triT1 = 0.0f; return 0.0f; } float s, t, u; float distEdgeSq; distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge0)); if (distEdgeSq < distSq) { distSq = distEdgeSq; segT = s; triT0 = t; triT1 = 0.0f; } distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge1)); if (distEdgeSq < distSq) { distSq = distEdgeSq; segT = s; triT0 = 0.0f; triT1 = t; } distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin + triangle.Edge0, triangle.Edge2)); if (distEdgeSq < distSq) { distSq = distEdgeSq; segT = s; triT0 = 1.0f - t; triT1 = t; } // compare segment end points to triangle interior float startTriSq = PointTriangleDistanceSq(out t, out u, seg.Origin, triangle); if (startTriSq < distSq) { distSq = startTriSq; segT = 0.0f; triT0 = t; triT1 = u; } float endTriSq = PointTriangleDistanceSq(out t, out u, seg.GetEnd(), triangle); if (endTriSq < distSq) { distSq = endTriSq; segT = 1.0f; triT0 = t; triT1 = u; } return distSq; }
internal static void CollDetectSphereStaticMeshSweep(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { // really use a swept test - or overlap? Vector3 delta = newSphere.Center - oldSphere.Center; if (delta.LengthSquared() < (0.25f * newSphere.Radius * newSphere.Radius)) { CollDetectSphereStaticMeshOverlap(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor); } else { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + oldSphere.Radius; float sphereToR2 = sphereTolR * sphereTolR; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(oldSphere, ref bb); BoundingBoxHelper.AddSphere(newSphere, ref bb); // get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix); unsafe { #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed( int* potentialTriangles = potTriArray) { #endif int numCollPts = 0; int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { // first test the old sphere for being on the wrong side IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); float distToCentreOld = meshTriangle.Plane.DotCoordinate(oldSphereCen); if (distToCentreOld <= 0.0f) continue; // now test the new sphere for being clear float distToCentreNew = meshTriangle.Plane.DotCoordinate(newSphereCen); if (distToCentreNew > sphereTolR) continue; int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); // If the old sphere is intersecting, just use that result float s, t; float d2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); if (d2 < sphereToR2) { float dist = (float)System.Math.Sqrt(d2); float depth = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > float.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth); } collNormal += collisionN; } else if (distToCentreNew < distToCentreOld) { // old sphere is not intersecting - do a sweep, but only if the sphere is moving into the // triangle Vector3 pt, N; // CHECK THIS float depth; if (Intersection.SweptSphereTriangleIntersection(out pt, out N, out depth, oldSphere, newSphere, triangle, distToCentreOld, distToCentreNew, Intersection.EdgesToTest.EdgeAll, Intersection.CornersToTest.CornerAll)) { // collision point etc must be relative to the old position because that's //where the impulses are applied float dist = (float)System.Math.Sqrt(d2); float depth2 = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > JiggleMath.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt2 = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt2 - body0Pos, pt2 - body1Pos, depth); } collNormal += collisionN; } } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } #if USE_STACKALLOC } } #else FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); } #endif } }
public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + newSphere.Radius; float sphereTolR2 = sphereTolR * sphereTolR; unsafe { #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed( int* potentialTriangles = potTriArray) { #endif int numCollPts = 0; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(newSphere, ref bb); int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); // Deano : get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); float distToCentre = meshTriangle.Plane.DotCoordinate(newSphereCen); if (distToCentre <= 0.0f) continue; if (distToCentre >= sphereTolR) continue; int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); float s, t; float newD2 = Distance.PointTriangleDistanceSq(out s, out t, newSphereCen, triangle); if (newD2 < sphereTolR2) { // have overlap - but actually report the old intersection float oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); float dist = (float)System.Math.Sqrt((float)oldD2); float depth = oldSphere.Radius - dist; Vector3 triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref triPointSTNorm); Vector3 collisionN = (dist > float.Epsilon) ? triPointSTNorm : triangle.Normal; // since impulse get applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth); } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } #if USE_STACKALLOC } } #else FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); } #endif } }
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth, BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle, float oldCentreDistToPlane, float newCentreDistToPlane, EdgesToTest edgesToTest, CornersToTest cornersToTest) { int i; var trianglePlane = triangle.Plane; N = Vector3.Zero; if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane)) { return(false); } var v0 = triangle.GetPoint(0); var v1 = triangle.GetPoint(1); var v2 = triangle.GetPoint(2); var e0 = v1 - v0; var e1 = v2 - v1; var e2 = v0 - v2; var allInside = true; var outDir0 = Vector3.Cross(e0, trianglePlane.Normal); if (Vector3.Dot(pt - v0, outDir0) > 0.0f) { allInside = false; } var outDir1 = Vector3.Cross(e1, trianglePlane.Normal); if (Vector3.Dot(pt - v1, outDir1) > 0.0f) { allInside = false; } var outDir2 = Vector3.Cross(e2, trianglePlane.Normal); if (Vector3.Dot(pt - v2, outDir2) > 0.0f) { allInside = false; } if (allInside) { N = trianglePlane.Normal; return(true); } var bestT = float.MaxValue; var Ks = newSphere.Center - oldSphere.Center; var kss = Vector3.Dot(Ks, Ks); var radius = newSphere.Radius; var radiusSq = radius * radius; for (i = 0; i < 3; ++i) { var mask = 1 << i; if (!((mask != 0) & ((int)edgesToTest != 0))) { continue; } Vector3 Ke; Vector3 vp; switch (i) { case 0: Ke = e0; vp = v0; break; case 1: Ke = e1; vp = v1; break; case 2: default: Ke = e2; vp = v2; break; } var Kg = vp - oldSphere.Center; var kee = Vector3.Dot(Ke, Ke); if (System.Math.Abs(kee) < JiggleMath.Epsilon) { continue; } var kes = Vector3.Dot(Ke, Ks); var kgs = Vector3.Dot(Kg, Ks); var keg = Vector3.Dot(Ke, Kg); var kgg = Vector3.Dot(Kg, Kg); var a = kee * kss - kes * kes; if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } var b = 2.0f * (keg * kes - kee * kgs); var c = kee * (kgg - radiusSq) - keg * keg; var blah = b * b - 4.0f * a * c; if (blah < 0.0f) { continue; } var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } var Ct = oldSphere.Center + t * Ks; var d = Vector3.Dot(Ct - vp, Ke) / kee; if (d < 0.0f || d > 1.0f) { continue; } bestT = t; pt = vp + d * Ke; N = Ct - pt; JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) { return(true); } bestT = float.MaxValue; for (i = 0; i < 3; ++i) { var mask = 1 << i; if (!((mask != 0) & (cornersToTest != 0))) { continue; } Vector3 vp; switch (i) { case 0: vp = v0; break; case 1: vp = v1; break; case 2: default: vp = v2; break; } var Kg = vp - oldSphere.Center; var kgs = Vector3.Dot(Kg, Ks); var kgg = Vector3.Dot(Kg, Kg); var a = kss; if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } var b = -2.0f * kgs; var c = kgg - radiusSq; var blah = b * b - 4.0f * a * c; if (blah < 0.0f) { continue; } var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } bestT = t; var Ct = oldSphere.Center + t * Ks; N = Ct - vp; JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) { return(true); } return(false); }
/// <summary> /// CollDetectCapsuleStaticMeshOverlap /// </summary> /// <param name="oldCapsule"></param> /// <param name="newCapsule"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float capsuleTolR = collTolerance + newCapsule.Radius; float capsuleTolR2 = capsuleTolR * capsuleTolR; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddCapsule(newCapsule, ref bb); unsafe { #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed (int* potentialTriangles = potTriArray) { #endif int numCollPts = 0; int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); Vector3 capsuleStart = newCapsule.Position; Vector3 capsuleEnd = newCapsule.GetEnd(); Matrix4 meshInvTransform = mesh.InverseTransformMatrix; Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform); Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); // we do the plane test using the capsule in mesh space float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart); float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd); // BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR. if ((distToStart > capsuleTolR && distToEnd > capsuleTolR) || (distToStart < -capsuleTolR && distToEnd < -capsuleTolR)) continue; // we now transform the triangle into world space (we could keep leave the mesh alone // but at this point 3 vector transforms is probably not a major slow down) int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Vector3 triVec0; Vector3 triVec1; Vector3 triVec2; mesh.GetVertex(i0, out triVec0); mesh.GetVertex(i1, out triVec1); mesh.GetVertex(i2, out triVec2); // Deano move tri into world space Matrix4 transformMatrix = mesh.TransformMatrix; Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0); Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1); Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2); Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2); Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart); float tS, tT0, tT1; float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle); if (d2 < capsuleTolR2) { Vector3 oldCapsuleStart = oldCapsule.Position; Vector3 oldCapsuleEnd = oldCapsule.GetEnd(); Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart); d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle); // report result from old position float dist = (float)System.Math.Sqrt(d2); float depth = oldCapsule.Radius - dist; Vector3 pt = triangle.GetPoint(tT0, tT1); Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) : meshTriangle.Plane.Normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Reused existing collPts. collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } #if USE_STACKALLOC } } #else } FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); #endif } }
private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox, ref IndexedTriangle triangle, TriangleMesh mesh, ref CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Matrix dirs0 = newBox.Orientation; #region REFERENCE: Triangle tri = new Triangle(mesh.GetVertex(triangle.GetVertexIndex(0)),mesh.GetVertex(triangle.GetVertexIndex(1)),mesh.GetVertex(triangle.GetVertexIndex(2))); Vector3 triVec0; Vector3 triVec1; Vector3 triVec2; mesh.GetVertex(triangle.GetVertexIndex(0), out triVec0); mesh.GetVertex(triangle.GetVertexIndex(1), out triVec1); mesh.GetVertex(triangle.GetVertexIndex(2), out triVec2); // Deano move tri into world space Matrix transformMatrix = mesh.TransformMatrix; Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0); Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1); Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2); Triangle tri = new Triangle(ref triVec0,ref triVec1,ref triVec2); #endregion #region REFERENCE Vector3 triEdge0 = (tri.GetPoint(1) - tri.GetPoint(0)); Vector3 pt0; Vector3 pt1; tri.GetPoint(0, out pt0); tri.GetPoint(1, out pt1); Vector3 triEdge0; Vector3.Subtract(ref pt1, ref pt0, out triEdge0); #endregion #region REFERENCE Vector3 triEdge1 = (tri.GetPoint(2) - tri.GetPoint(1)); Vector3 pt2; tri.GetPoint(2, out pt2); Vector3 triEdge1; Vector3.Subtract(ref pt2, ref pt1, out triEdge1); #endregion #region REFERENCE Vector3 triEdge2 = (tri.GetPoint(0) - tri.GetPoint(2)); Vector3 triEdge2; Vector3.Subtract(ref pt0, ref pt2, out triEdge2); #endregion triEdge0.Normalize(); triEdge1.Normalize(); triEdge2.Normalize(); Vector3 triNormal = triangle.Plane.Normal; // the 15 potential separating axes const int numAxes = 13; Vector3[] axes = new Vector3[numAxes]; axes[0] = triNormal; axes[1] = dirs0.Right; axes[2] = dirs0.Up; axes[3] = dirs0.Backward; Vector3.Cross(ref axes[1], ref triEdge0, out axes[4]); Vector3.Cross(ref axes[1], ref triEdge1, out axes[5]); Vector3.Cross(ref axes[1], ref triEdge2, out axes[6]); Vector3.Cross(ref axes[2], ref triEdge0, out axes[7]); Vector3.Cross(ref axes[2], ref triEdge1, out axes[8]); Vector3.Cross(ref axes[2], ref triEdge2, out axes[9]); Vector3.Cross(ref axes[3], ref triEdge0, out axes[10]); Vector3.Cross(ref axes[3], ref triEdge1, out axes[11]); Vector3.Cross(ref axes[3], ref triEdge2, out axes[12]); // the overlap depths along each axis float[] overlapDepths = new float[numAxes]; // see if the boxes are separate along any axis, and if not keep a // record of the depths along each axis int i; for (i = 0; i < numAxes; ++i) { overlapDepths[i] = 1.0f; if (Disjoint(out overlapDepths[i], axes[i], newBox, tri, collTolerance)) return false; } // The box overlap, find the separation depth closest to 0. float minDepth = float.MaxValue; int minAxis = -1; for (i = 0; i < numAxes; ++i) { // If we can't normalise the axis, skip it float l2 = axes[i].LengthSquared(); if (l2 < JiggleMath.Epsilon) continue; // Normalise the separation axis and the depth float invl = 1.0f / (float)System.Math.Sqrt(l2); axes[i] *= invl; overlapDepths[i] *= invl; // If this axis is the minimum, select it if (overlapDepths[i] < minDepth) { minDepth = overlapDepths[i]; minAxis = i; } } if (minAxis == -1) return false; // Make sure the axis is facing towards the 0th box. // if not, invert it Vector3 D = newBox.GetCentre() - tri.Centre; Vector3 N = axes[minAxis]; float depth = overlapDepths[minAxis]; if (Vector3.Dot(D, N) < 0.0f) N *= -1; Vector3 boxOldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 boxNewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero; Vector3 meshPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; List<Vector3> pts = new List<Vector3>(); //pts.Clear(); const float combinationDist = 0.05f; GetBoxTriangleIntersectionPoints(pts, newBox, tri, depth + combinationDist); // adjust the depth #region REFERENCE: Vector3 delta = boxNewPos - boxOldPos; Vector3 delta; Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta); #endregion #region REFERENCE: float oldDepth = depth + Vector3.Dot(delta, N); float oldDepth; Vector3.Dot(ref delta, ref N, out oldDepth); oldDepth += depth; #endregion unsafe { // report collisions int numPts = pts.Count; #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) #endif { if (numPts > 0) { if (numPts >= MaxLocalStackSCPI) { numPts = MaxLocalStackSCPI - 1; } // adjust positions for (i = 0; i < numPts; ++i) { collPts[i] = new SmallCollPointInfo(pts[i] - boxNewPos, pts[i] - meshPos, oldDepth); } collisionFunctor.CollisionNotify(ref info, ref N, collPts, numPts); #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif return true; } else { #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif return false; } } } }
/// <summary> /// DoOverlapBoxTriangleTest /// </summary> /// <param name="oldBox"></param> /// <param name="newBox"></param> /// <param name="triangle"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> /// <returns>bool</returns> private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox, ref IndexedTriangle triangle, TriangleMesh mesh, ref CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Matrix4 dirs0 = newBox.Orientation; #region REFERENCE: Triangle tri = new Triangle(mesh.GetVertex(triangle.GetVertexIndex(0)),mesh.GetVertex(triangle.GetVertexIndex(1)),mesh.GetVertex(triangle.GetVertexIndex(2))); Vector3 triVec0; Vector3 triVec1; Vector3 triVec2; mesh.GetVertex(triangle.GetVertexIndex(0), out triVec0); mesh.GetVertex(triangle.GetVertexIndex(1), out triVec1); mesh.GetVertex(triangle.GetVertexIndex(2), out triVec2); // Deano move tri into world space Matrix4 transformMatrix = mesh.TransformMatrix; Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0); Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1); Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2); Triangle tri = new Triangle(ref triVec0,ref triVec1,ref triVec2); #endregion #region REFERENCE Vector3 triEdge0 = (tri.GetPoint(1) - tri.GetPoint(0)); Vector3 pt0; Vector3 pt1; tri.GetPoint(0, out pt0); tri.GetPoint(1, out pt1); Vector3 triEdge0; Vector3.Subtract(ref pt1, ref pt0, out triEdge0); #endregion #region REFERENCE Vector3 triEdge1 = (tri.GetPoint(2) - tri.GetPoint(1)); Vector3 pt2; tri.GetPoint(2, out pt2); Vector3 triEdge1; Vector3.Subtract(ref pt2, ref pt1, out triEdge1); #endregion #region REFERENCE Vector3 triEdge2 = (tri.GetPoint(0) - tri.GetPoint(2)); Vector3 triEdge2; Vector3.Subtract(ref pt0, ref pt2, out triEdge2); #endregion triEdge0.Normalize(); triEdge1.Normalize(); triEdge2.Normalize(); // BEN-OPTIMISATION: Replaced loops with code that requires no looping. // The new code is faster, has less allocations and math especially // since the method returns as soon as it finds a non-overlapping axis, // i.e. Before irreleveat allocations occur. #region "Old (less efficient) code" /*Vector3 triNormal = triangle.Plane.Normal; // the 15 potential separating axes const int numAxes = 13; Vector3[] axes = new Vector3[numAxes]; axes[0] = triNormal; axes[1] = dirs0.Right; axes[2] = dirs0.Up; axes[3] = dirs0.Backward; Vector3.Cross(ref axes[1], ref triEdge0, out axes[4]); Vector3.Cross(ref axes[1], ref triEdge1, out axes[5]); Vector3.Cross(ref axes[1], ref triEdge2, out axes[6]); Vector3.Cross(ref axes[2], ref triEdge0, out axes[7]); Vector3.Cross(ref axes[2], ref triEdge1, out axes[8]); Vector3.Cross(ref axes[2], ref triEdge2, out axes[9]); Vector3.Cross(ref axes[3], ref triEdge0, out axes[10]); Vector3.Cross(ref axes[3], ref triEdge1, out axes[11]); Vector3.Cross(ref axes[3], ref triEdge2, out axes[12]); // the overlap depths along each axis float[] overlapDepths = new float[numAxes]; // see if the boxes are separate along any axis, and if not keep a // record of the depths along each axis int i; for (i = 0; i < numAxes; ++i) { overlapDepths[i] = 1.0f; if (Disjoint(out overlapDepths[i], axes[i], newBox, tri, collTolerance)) return false; } // The box overlap, find the separation depth closest to 0. float minDepth = float.MaxValue; int minAxis = -1; for (i = 0; i < numAxes; ++i) { // If we can't normalise the axis, skip it float l2 = axes[i].LengthSquared; if (l2 < JiggleMath.Epsilon) continue; // Normalise the separation axis and the depth float invl = 1.0f / (float)System.Math.Sqrt(l2); axes[i] *= invl; overlapDepths[i] *= invl; // If this axis is the minimum, select it if (overlapDepths[i] < minDepth) { minDepth = overlapDepths[i]; minAxis = i; } } if (minAxis == -1) return false; // Make sure the axis is facing towards the 0th box. // if not, invert it Vector3 D = newBox.GetCentre() - tri.Centre; Vector3 N = axes[minAxis]; float depth = overlapDepths[minAxis];*/ #endregion #region "Optimised code" Vector3 triNormal = triangle.Plane.Normal; Vector3 right = dirs0.Right(); Vector3 up = dirs0.Up(); Vector3 backward = dirs0.Backward(); float testDepth; if (Disjoint(out testDepth, ref triNormal, newBox, ref tri, collTolerance)) return (false); float depth = testDepth; Vector3 N = triNormal; if (Disjoint(out testDepth, ref right, newBox, ref tri, collTolerance)) return (false); if (testDepth < depth) { depth = testDepth; N = right; } if (Disjoint(out testDepth, ref up, newBox, ref tri, collTolerance)) return (false); if (testDepth < depth) { depth = testDepth; N = up; } if (Disjoint(out testDepth, ref backward, newBox, ref tri, collTolerance)) return (false); if (testDepth < depth) { depth = testDepth; N = backward; } Vector3 axis; Vector3.Cross(ref right, ref triEdge0, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref right, ref triEdge1, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref right, ref triEdge2, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref up, ref triEdge0, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref up, ref triEdge1, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref up, ref triEdge2, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref backward, ref triEdge0, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref backward, ref triEdge1, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } Vector3.Cross(ref backward, ref triEdge2, out axis); if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance)) return (false); testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z); if (testDepth < depth) { depth = testDepth; N = axis; } /*if (N == Vector3.Zero) return (false);*/ Vector3 D = newBox.GetCentre() - tri.Centre; N.Normalize(); int i; #endregion if (Vector3.Dot(D, N) < 0.0f) N *= -1; Vector3 boxOldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 boxNewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero; Vector3 meshPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; List<Vector3> pts = new List<Vector3>(); //pts.Clear(); const float combinationDist = 0.05f; GetBoxTriangleIntersectionPoints(pts, newBox, tri, depth + combinationDist); // adjust the depth #region REFERENCE: Vector3 delta = boxNewPos - boxOldPos; Vector3 delta; Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta); #endregion #region REFERENCE: float oldDepth = depth + Vector3.Dot(delta, N); float oldDepth; Vector3.Dot(ref delta, ref N, out oldDepth); oldDepth += depth; #endregion unsafe { // report collisions int numPts = pts.Count; #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) #endif { if (numPts > 0) { if (numPts >= MaxLocalStackSCPI) { numPts = MaxLocalStackSCPI - 1; } // adjust positions for (i = 0; i < numPts; ++i) { // BEN-OPTIMISATION: Reused existing SmallCollPointInfo and inlined vector substraction. collPts[i].R0.X = pts[i].X - boxNewPos.X; collPts[i].R0.Y = pts[i].Y - boxNewPos.Y; collPts[i].R0.Z = pts[i].Z - boxNewPos.Z; collPts[i].R1.X = pts[i].X - meshPos.X; collPts[i].R1.Y = pts[i].Y - meshPos.Y; collPts[i].R1.Z = pts[i].Z - meshPos.Z; collPts[i].InitialPenetration = oldDepth; } collisionFunctor.CollisionNotify(ref info, ref N, collPts, numPts); #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif return true; } else { #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif return false; } } } }
private void SetNormalOfTriangleAtIndices(int a, int b, int c) { Vector3 vA = verts[a].Position; Vector3 vB = verts[b].Position; Vector3 vC = verts[c].Position; Triangle t = new Triangle(vA, vC, vB); Vector3 n = t.Normal; verts[a].Normal += n; verts[b].Normal += n; verts[c].Normal += n; verts[a].Normal.Normalize(); verts[b].Normal.Normalize(); verts[c].Normal.Normalize(); }
/// <summary> /// Disjoint Returns true if disjoint. Returns false if intersecting, /// and sets the overlap depth, d scaled by the axis length /// </summary> /// <param name="d"></param> /// <param name="axis"></param> /// <param name="box"></param> /// <param name="triangle"></param> /// <param name="collTolerance"></param> /// <returns></returns> private static bool Disjoint(out float d, Vector3 axis, Box box, Triangle triangle, float collTolerance) { float min0, max0, min1, max1; box.GetSpan(out min0, out max0, axis); triangle.GetSpan(out min1, out max1, axis); if (min0 > (max1 + collTolerance + JiggleMath.Epsilon) || min1 > (max0 + collTolerance + JiggleMath.Epsilon)) { d = 0.0f; return true; } if ((max0 > max1) && (min1 > min0)) { // triangle is inside - choose the min dist to move it out d = System.Math.Min(max0 - min1, max1 - min0); } else if ((max1 > max0) && (min0 > min1)) { // box is inside - choose the min dist to move it out d = System.Math.Min(max1 - min0, max0 - min1); } else { // objects overlap d = (max0 < max1) ? max0 : max1; d -= (min0 > min1) ? min0 : min1; } return false; }
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth, BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle, float oldCentreDistToPlane, float newCentreDistToPlane, EdgesToTest edgesToTest, CornersToTest cornersToTest) { int i; Microsoft.Xna.Framework.Plane trianglePlane = triangle.Plane; N = Vector3.Zero; // Check against plane if (!SweptSpherePlaneIntersection(out pt,out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane)) return false; Vector3 v0 = triangle.GetPoint(0); Vector3 v1 = triangle.GetPoint(1); Vector3 v2 = triangle.GetPoint(2); Vector3 e0 = v1 - v0; Vector3 e1 = v2 - v1; Vector3 e2 = v0 - v2; // If the point is inside the triangle, this is a hit bool allInside = true; Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal); if (Vector3.Dot(pt - v0, outDir0) > 0.0f) { allInside = false; } Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal); if (Vector3.Dot(pt - v1, outDir1) > 0.0f) { allInside = false; } Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal); if (Vector3.Dot(pt - v2, outDir2) > 0.0f) { allInside = false; } // Quick result? if (allInside) { N = trianglePlane.Normal; return true; } // Now check against the edges float bestT = float.MaxValue; Vector3 Ks = newSphere.Center - oldSphere.Center; float kss = Vector3.Dot(Ks, Ks); float radius = newSphere.Radius; float radiusSq = radius * radius; for (i = 0; i < 3; ++i) { int mask = 1 << i; if (!((mask !=0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS continue; Vector3 Ke; Vector3 vp; switch (i) { case 0: Ke = e0; vp = v0; break; case 1: Ke = e1; vp = v1; break; case 2: default: Ke = e2; vp = v2; break; } Vector3 Kg = vp - oldSphere.Center; float kee = Vector3.Dot(Ke, Ke); if (System.Math.Abs(kee) < JiggleMath.Epsilon) continue; float kes = Vector3.Dot(Ke, Ks); float kgs = Vector3.Dot(Kg, Ks); float keg = Vector3.Dot(Ke, Kg); float kgg = Vector3.Dot(Kg, Kg); // a * t^2 + b * t + c = 0 float a = kee * kss - (kes * kes); if (System.Math.Abs(a) < JiggleMath.Epsilon) continue; float b = 2.0f * (keg * kes - kee * kgs); float c = kee * (kgg - radiusSq) - keg * keg; float blah = b*b - 4.0f * a * c; if (blah < 0.0f) continue; // solve for t - take minimum float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) continue; if (t > bestT) continue; // now check where it hit on the edge Vector3 Ct = oldSphere.Center + t * Ks; float d = Vector3.Dot((Ct - vp), Ke) / kee; if (d < 0.0f || d > 1.0f) continue; // wahay - got hit. Already checked that t < bestT bestT = t; pt = vp + d * Ke; N = (Ct - pt);// .GetNormalisedSafe(); JiggleMath.NormalizeSafe(ref N); // depth is already calculated } if (bestT <= 1.0f) return true; // check the corners bestT = float.MaxValue; for (i = 0; i < 3; ++i) { int mask = 1 << i; if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS continue; Vector3 vp; switch (i) { case 0: vp = v0; break; case 1: vp = v1; break; case 2: default: vp = v2; break; } Vector3 Kg = vp - oldSphere.Center; float kgs = Vector3.Dot(Kg, Ks); float kgg = Vector3.Dot(Kg, Kg); float a = kss; if (System.Math.Abs(a) < JiggleMath.Epsilon) continue; float b = -2.0f * kgs; float c = kgg - radiusSq; float blah = (b * b) - 4.0f * a * c; if (blah < 0.0f) continue; // solve for t - take minimum float t = (-b - (float) System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) continue; if (t > bestT) continue; bestT = t; Vector3 Ct = oldSphere.Center + t * Ks; N = (Ct - vp);//.GetNormalisedSafe(); JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) return true; return false; }
public static bool SegmentTriangleIntersection(out float tS, out float tT0, out float tT1, Segment seg, Triangle triangle) { /// the parameters - if hit then they get copied into the args float u, v, t; tS = 0; tT0 = 0; tT1 = 0; Vector3 e1 = triangle.Edge0; Vector3 e2 = triangle.Edge1; Vector3 p = Vector3.Cross(seg.Delta, e2); float a = Vector3.Dot(e1, p); if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon) return false; float f = 1.0f / a; Vector3 s = seg.Origin - triangle.Origin; u = f * Vector3.Dot(s, p); if (u < 0.0f || u > 1.0f) return false; Vector3 q = Vector3.Cross(s, e1); v = f * Vector3.Dot(seg.Delta, q); if (v < 0.0f || (u + v) > 1.0f) return false; t = f * Vector3.Dot(e2, q); if (t < 0.0f || t > 1.0f) return false; tS = t; tT0 = u; tT1 = v; //if (tS != 0) tS = t; //if (tT0 != 0) tT0 = u; //if (tT1 != 0) tT1 = v; return true; }
public static bool SegmentTriangleIntersection(out float tS, out float tT0, out float tT1, Segment seg, Triangle triangle) { float u, v, t; tS = 0; tT0 = 0; tT1 = 0; var e1 = triangle.Edge0; var e2 = triangle.Edge1; var p = Vector3.Cross(seg.Delta, e2); var a = Vector3.Dot(e1, p); if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon) { return(false); } var f = 1.0f / a; var s = seg.Origin - triangle.Origin; u = f * Vector3.Dot(s, p); if (u < 0.0f || u > 1.0f) { return(false); } var q = Vector3.Cross(s, e1); v = f * Vector3.Dot(seg.Delta, q); if (v < 0.0f || u + v > 1.0f) { return(false); } t = f * Vector3.Dot(e2, q); if (t < 0.0f || t > 1.0f) { return(false); } tS = t; tT0 = u; tT1 = v; return(true); }
/// <summary> /// PointTriangleDistanceSq /// </summary> /// <param name="pfSParam"></param> /// <param name="pfTParam"></param> /// <param name="rkPoint"></param> /// <param name="rkTri"></param> /// <returns>float</returns> public static float PointTriangleDistanceSq(out float pfSParam, out float pfTParam, Vector3 rkPoint, Triangle rkTri) { Vector3 kDiff = rkTri.Origin - rkPoint; float fA00 = rkTri.Edge0.LengthSquared(); float fA01 = Vector3.Dot(rkTri.Edge0, rkTri.Edge1); float fA11 = rkTri.Edge1.LengthSquared(); float fB0 = Vector3.Dot(kDiff, rkTri.Edge0); float fB1 = Vector3.Dot(kDiff, rkTri.Edge1); float fC = kDiff.LengthSquared(); float fDet = System.Math.Abs(fA00 * fA11 - fA01 * fA01); float fS = fA01 * fB1 - fA11 * fB0; float fT = fA01 * fB0 - fA00 * fB1; float fSqrDist; if (fS + fT <= fDet) { if (fS < 0.0f) { if (fT < 0.0f) // region 4 { if (fB0 < 0.0f) { fT = 0.0f; if (-fB0 >= fA00) { fS = 1.0f; fSqrDist = fA00 + 2.0f * fB0 + fC; } else { fS = -fB0 / fA00; fSqrDist = fB0 * fS + fC; } } else { fS = 0.0f; if (fB1 >= 0.0f) { fT = 0.0f; fSqrDist = fC; } else if (-fB1 >= fA11) { fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } } else // region 3 { fS = 0.0f; if (fB1 >= 0.0f) { fT = 0.0f; fSqrDist = fC; } else if (-fB1 >= fA11) { fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } } else if (fT < 0.0f) // region 5 { fT = 0.0f; if (fB0 >= 0.0f) { fS = 0.0f; fSqrDist = fC; } else if (-fB0 >= fA00) { fS = 1.0f; fSqrDist = fA00 + 2.0f * fB0 + fC; } else { fS = -fB0 / fA00; fSqrDist = fB0 * fS + fC; } } else // region 0 { // minimum at interior point float fInvDet = 1.0f / fDet; fS *= fInvDet; fT *= fInvDet; fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) + fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC; } } else { float fTmp0, fTmp1, fNumer, fDenom; if (fS < 0.0f) // region 2 { fTmp0 = fA01 + fB0; fTmp1 = fA11 + fB1; if (fTmp1 > fTmp0) { fNumer = fTmp1 - fTmp0; fDenom = fA00 - 2.0f * fA01 + fA11; if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDist = fA00 + 2.0f * fB0 + fC; } else { fS = fNumer / fDenom; fT = 1.0f - fS; fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) + fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC; } } else { fS = 0.0f; if (fTmp1 <= 0.0f) { fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else if (fB1 >= 0.0f) { fT = 0.0f; fSqrDist = fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } } else if (fT < 0.0f) // region 6 { fTmp0 = fA01 + fB1; fTmp1 = fA00 + fB0; if (fTmp1 > fTmp0) { fNumer = fTmp1 - fTmp0; fDenom = fA00 - 2.0f * fA01 + fA11; if (fNumer >= fDenom) { fT = 1.0f; fS = 0.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fT = fNumer / fDenom; fS = 1.0f - fT; fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) + fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC; } } else { fT = 0.0f; if (fTmp1 <= 0.0f) { fS = 1.0f; fSqrDist = fA00 + 2.0f * fB0 + fC; } else if (fB0 >= 0.0f) { fS = 0.0f; fSqrDist = fC; } else { fS = -fB0 / fA00; fSqrDist = fB0 * fS + fC; } } } else // region 1 { fNumer = fA11 + fB1 - fA01 - fB0; if (fNumer <= 0.0f) { fS = 0.0f; fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fDenom = fA00 - 2.0f * fA01 + fA11; if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDist = fA00 + 2.0f * fB0 + fC; } else { fS = fNumer / fDenom; fT = 1.0f - fS; fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) + fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC; } } } } pfSParam = fS; pfTParam = fT; return System.Math.Abs(fSqrDist); }
/// <summary> /// GetBoxTriangleIntersectionPoints /// Pushes intersection points onto the back of pts. Returns the /// number of points found. /// Points that are close together (compared to /// combinationDistance) get combined /// </summary> /// <param name="pts"></param> /// <param name="box"></param> /// <param name="triangle"></param> /// <param name="combinationDistance"></param> /// <returns></returns> private static int GetBoxTriangleIntersectionPoints(List<Vector3> pts, Box box, Triangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangle Box.Edge[] edges; box.GetEdges(out edges); Vector3[] boxPts; box.GetCornerPoints(out boxPts); float tS; float tv1, tv2; int iEdge; for (iEdge = 0; iEdge < 12; ++iEdge) { Box.Edge edge = edges[iEdge]; Segment seg = new Segment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (Intersection.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { AddPoint(pts, seg.GetPoint(tS), combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangle with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); Segment s1 = new Segment(pt0, pt1 - pt0); Segment s2 = new Segment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) AddPoint(pts, pos, combinationDistance * combinationDistance); if (box.SegmentIntersect(out tS, out pos, out n, s2)) AddPoint(pts, pos, combinationDistance * combinationDistance); } return pts.Count; }
// BEN-OPTIMISATION: ref axis and ref triangle, BEN-CLEANUP: Renamed min and max variables. /// <summary> /// Disjoint Returns true if disjoint. Returns false if intersecting, /// and sets the overlap depth, d scaled by the axis length /// </summary> /// <param name="d"></param> /// <param name="axis"></param> /// <param name="box"></param> /// <param name="triangle"></param> /// <param name="collTolerance"></param> /// <returns>bool</returns> private static bool Disjoint(out float d, ref Vector3 axis, Box box, ref Triangle triangle, float collTolerance) { float minBox, maxBox, minTri, maxTri; // BEN-OPTIMISATION: ref axis box.GetSpan(out minBox, out maxBox, ref axis); triangle.GetSpan(out minTri, out maxTri, ref axis); if (minBox > (maxTri + collTolerance + JiggleMath.Epsilon) || minTri > (maxBox + collTolerance + JiggleMath.Epsilon)) { d = 0.0f; return (true); } if ((maxBox > maxTri) && (minTri > minBox)) { // triangle is inside - choose the min dist to move it out d = System.Math.Min(maxBox - minTri, maxTri - minBox); } else if ((maxTri > maxBox) && (minBox > minTri)) { // box is inside - choose the min dist to move it out d = System.Math.Min(maxTri - minBox, maxBox - minTri); } else { // objects overlap d = (maxBox < maxTri) ? maxBox : maxTri; d -= (minBox > minTri) ? minBox : minTri; } return (false); }
/// <summary> /// SegmentIntersect /// </summary> /// <param name="frac"></param> /// <param name="pos"></param> /// <param name="normal"></param> /// <param name="seg"></param> /// <returns>bool</returns> public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { // move segment into octree space seg.Origin = Vector3.Transform(seg.Origin, invTransform); seg.Delta = Vector3.TransformNormal(seg.Delta, invTransform); BoundingBox segBox = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSegment(seg, ref segBox); unsafe { #if USE_STACKALLOC int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { #else int[] potTriArray = DetectFunctor.IntStackAlloc(); fixed (int* potentialTriangles = potTriArray) { #endif int numTriangles = GetTrianglesIntersectingtAABox(potentialTriangles, DetectFunctor.MaxLocalStackTris, ref segBox); float tv1, tv2; pos = Vector3.Zero; normal = Vector3.Zero; float bestFrac = float.MaxValue; for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = GetTriangle(potentialTriangles[iTriangle]); float thisFrac; Triangle tri = new Triangle(GetVertex(meshTriangle.GetVertexIndex(0)), GetVertex(meshTriangle.GetVertexIndex(1)), GetVertex(meshTriangle.GetVertexIndex(2))); if (Intersection.SegmentTriangleIntersection(out thisFrac, out tv1, out tv2, seg, tri)) { if (thisFrac < bestFrac) { bestFrac = thisFrac; // re-project pos = Vector3.Transform(seg.GetPoint(thisFrac), transformMatrix); normal = Vector3.TransformNormal(meshTriangle.Plane.Normal, transformMatrix); } } } frac = bestFrac; if (bestFrac < float.MaxValue) { DetectFunctor.FreeStackAlloc(potTriArray); return true; } else { DetectFunctor.FreeStackAlloc(potTriArray); return false; } #if USE_STACKALLOC } #else } #endif } }
/// <summary> /// GetBoxTriangleIntersectionPoints /// Pushes intersection points onto the back of pts. Returns the /// number of points found. /// Points that are close together (compared to /// combinationDistance) get combined /// </summary> /// <param name="pts"></param> /// <param name="box"></param> /// <param name="triangle"></param> /// <param name="combinationDistance"></param> /// <returns>int</returns> private static int GetBoxTriangleIntersectionPoints(List<Vector3> pts, Box box, Triangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangle Box.Edge[] edges; box.GetEdges(out edges); Vector3[] boxPts; box.GetCornerPoints(out boxPts); float tS; float tv1, tv2; // BEN-OPTIMISATION: Allocating just one Vector3 to be reused. Vector3 point = new Vector3(); int iEdge; for (iEdge = 0; iEdge < 12; ++iEdge) { Box.Edge edge = edges[iEdge]; Segment seg = new Segment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (Intersection.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { // BEN-OPTIMISATION: Reusing the existing point variable instead allocating new ones. // This also allows point to be based by reference. seg.GetPoint(ref point, tS); AddPoint(pts, ref point, combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangle with the box for (iEdge = 0; iEdge < 3; ++iEdge) { #region "BEN-OPTIMISATION: Remove excess allocations and pass variables by reference." // ORIGINAL CODE: /*Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); Segment s1 = new Segment(pt0, pt1 - pt0); Segment s2 = new Segment(pt1, pt0 - pt1);*/ // OPTIMISED CODE: Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); Vector3 difference1; Vector3 difference2; Vector3.Subtract(ref pt1, ref pt0, out difference1); Vector3.Subtract(ref pt0, ref pt1, out difference2); Segment s1 = new Segment(ref pt0, ref difference1); Segment s2 = new Segment(ref pt1, ref difference2); #endregion if (box.SegmentIntersect(out tS, out pos, out n, s1)) AddPoint(pts, ref pos, combinationDistance * combinationDistance); if (box.SegmentIntersect(out tS, out pos, out n, s2)) AddPoint(pts, ref pos, combinationDistance * combinationDistance); } return pts.Count; }
void LoadMS3D(string filename) { using (FileStream fs = new FileStream(filename, FileMode.Open)) { using (BinaryReader br = new BinaryReader(fs, System.Text.Encoding.Default)) { br.ReadChars(10); br.ReadInt32(); vertexCount = br.ReadUInt16(); ModelVertex[] vertices = new ModelVertex[vertexCount]; Vector3 minVert = Vector3.One * float.PositiveInfinity; Vector3 maxVert = Vector3.One * float.NegativeInfinity; for (int i = 0; i < vertexCount; i++) { br.ReadByte(); vertices[i].Position = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); minVert = Vector3.Min(minVert, vertices[i].Position); maxVert = Vector3.Max(maxVert, vertices[i].Position); vertices[i].BoneIndex = (int)br.ReadChar(); if (vertices[i].BoneIndex >= 255) vertices[i].BoneIndex = 0; vertices[i].Weight = 1; br.ReadByte(); } ushort triangleCount = br.ReadUInt16(); Triangle[] triList = new Triangle[triangleCount]; for (int i = 0; i < triangleCount; i++) { br.ReadUInt16(); //flag //Indices ushort v0 = br.ReadUInt16(); ushort v1 = br.ReadUInt16(); ushort v2 = br.ReadUInt16(); triList[i].vertex0 = v0; triList[i].vertex1 = v1; triList[i].vertex2 = v2; //Vertex 0 Normal vertices[v0].Normal += new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); //Vertex 1 Normal vertices[v1].Normal += new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); //Vertex 2 Normal vertices[v2].Normal += new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); //U vertices[v0].TexCoord.X = br.ReadSingle(); vertices[v1].TexCoord.X = br.ReadSingle(); vertices[v2].TexCoord.X = br.ReadSingle(); //V vertices[v0].TexCoord.Y = br.ReadSingle(); vertices[v1].TexCoord.Y = br.ReadSingle(); vertices[v2].TexCoord.Y = br.ReadSingle(); vertices[v0].Weight++; vertices[v1].Weight++; vertices[v2].Weight++; //Smoothing br.ReadByte(); //Group index br.ReadByte(); } for (int i = 0; i < vertexCount; i++) { vertices[i].Normal /= vertices[i].Weight; vertices[i].Weight = 1; } for (int i = 0; i < triangleCount; i++) { vertices[triList[i].vertex0].AddTangent(vertices[triList[i].vertex1], vertices[triList[i].vertex2]); vertices[triList[i].vertex1].AddTangent(vertices[triList[i].vertex0].Tangent, vertices[triList[i].vertex0].Tangent2); vertices[triList[i].vertex2].AddTangent(vertices[triList[i].vertex0].Tangent, vertices[triList[i].vertex0].Tangent2); //vertices[triList[i].vertex1].AddTangent(vertices[triList[i].vertex0], vertices[triList[i].vertex2]); //vertices[triList[i].vertex2].AddTangent(vertices[triList[i].vertex0], vertices[triList[i].vertex1]); } VertexPNTTI[] verts = new VertexPNTTI[vertexCount]; for (int i = 0; i < vertexCount; i++) { Vector3 N = vertices[i].Normal; N.Normalize(); Vector3 tangent = vertices[i].Tangent / vertices[i].Weight; tangent = (tangent - N * Vector3.Dot(N, tangent)); tangent.Normalize(); vertices[i].Tangent = tangent; vertices[i].Tangent2 /= vertices[i].Weight; vertices[i].Weight = 1; float binormSign = (Vector3.Dot(Vector3.Cross(N, tangent), vertices[i].Tangent2) < 0.0f) ? -1.0f : 1.0f; verts[i] = new VertexPNTTI(vertices[i].Position, vertices[i].Normal, vertices[i].TexCoord, vertices[i].Tangent, vertices[i].BoneIndex, binormSign); } vertexBuffer = new VertexBuffer(GFX.Device, vertexCount * VertexPNTTI.SizeInBytes, BufferUsage.None); vertexBuffer.SetData<VertexPNTTI>(verts); ushort groupCount = br.ReadUInt16(); parts = new ModelPart[groupCount]; int[] matIndices = new int[groupCount]; for (int i = 0; i < groupCount; i++) { br.ReadByte(); parts[i] = new ModelPart(); parts[i].name = new string(br.ReadChars(32)); //Group Name parts[i].name = parts[i].name.Replace("\0",string.Empty); ushort numTriangles = br.ReadUInt16(); //numTriangles parts[i].renderElement = new RenderElement(); parts[i].renderElement.PrimitiveCount = numTriangles; parts[i].renderElement.StartVertex = 0; parts[i].renderElement.VertexBuffer = vertexBuffer; parts[i].renderElement.VertexCount = vertexCount; parts[i].renderElement.VertexStride = VertexPNTTI.SizeInBytes; parts[i].renderElement.VertexDec = GFXVertexDeclarations.PNTTIDec; parts[i].bounds.Max = Vector3.Zero; parts[i].bounds.Min = Vector3.Zero; bool useIntIndices = (numTriangles >= ushort.MaxValue); IndexElementSize size = (useIntIndices) ? IndexElementSize.ThirtyTwoBits : IndexElementSize.SixteenBits; int stride = (useIntIndices) ? sizeof(uint) : sizeof(ushort); parts[i].renderElement.IndexBuffer = new IndexBuffer(GFX.Device, stride * numTriangles * 3, BufferUsage.None, size); List<ushort> ushortIndices = new List<ushort>(); List<uint> uintIndices = new List<uint>(); for (int l = 0; l < numTriangles; l++) { ushort t = br.ReadUInt16(); //triangle index if (useIntIndices) { uintIndices.Add((uint)triList[t].vertex2); uintIndices.Add((uint)triList[t].vertex1); uintIndices.Add((uint)triList[t].vertex0); } else { ushortIndices.Add((ushort)triList[t].vertex2); ushortIndices.Add((ushort)triList[t].vertex1); ushortIndices.Add((ushort)triList[t].vertex0); } parts[i].bounds.Max = Vector3.Max(parts[i].bounds.Max, vertices[triList[t].vertex0].Position); parts[i].bounds.Max = Vector3.Max(parts[i].bounds.Max, vertices[triList[t].vertex1].Position); parts[i].bounds.Max = Vector3.Max(parts[i].bounds.Max, vertices[triList[t].vertex2].Position); parts[i].bounds.Min = Vector3.Min(parts[i].bounds.Min, vertices[triList[t].vertex0].Position); parts[i].bounds.Min = Vector3.Min(parts[i].bounds.Min, vertices[triList[t].vertex1].Position); parts[i].bounds.Min = Vector3.Min(parts[i].bounds.Min, vertices[triList[t].vertex2].Position); } if (useIntIndices) parts[i].renderElement.IndexBuffer.SetData<uint>(uintIndices.ToArray()); else parts[i].renderElement.IndexBuffer.SetData<ushort>(ushortIndices.ToArray()); matIndices[i] = (int)br.ReadChar(); //Material index } meshBounds = new BoundingBox(parts[0].bounds.Min, parts[0].bounds.Max); for (int i = 1; i < parts.Length; i++) { meshBounds.Min = Vector3.Min(parts[i].bounds.Min, meshBounds.Min); meshBounds.Max = Vector3.Max(parts[i].bounds.Max, meshBounds.Max); } ushort MaterialCount = br.ReadUInt16(); string[] materialNames = new string[MaterialCount]; for (int i = 0; i < MaterialCount; i++) { materialNames[i] = new string(br.ReadChars(32)); materialNames[i] = materialNames[i].Replace("\0", string.Empty); for (int l = 0; l < 4; l++) br.ReadSingle(); for (int l = 0; l < 4; l++) br.ReadSingle(); for (int l = 0; l < 4; l++) br.ReadSingle(); for (int l = 0; l < 4; l++) br.ReadSingle(); br.ReadSingle(); br.ReadSingle(); br.ReadChar(); br.ReadChars(128); br.ReadChars(128); } CheckMissingMaterials(filename, matIndices, materialNames); for (int i = 0; i < groupCount; i++) { int matIndex = matIndices[i]; if (matIndex < 255) parts[i].material = ResourceManager.Inst.GetMaterial(materialNames[matIndex]); if (parts[i].material == null) parts[i].material = ResourceManager.Inst.GetMaterial("NULL"); } float fps = br.ReadSingle();//FPS br.ReadSingle();//current time int frameCount = br.ReadInt32(); //Total frames ushort boneCount = br.ReadUInt16(); nodes = new AnimationNode[boneCount]; if (boneCount > 0) { List<string> nodeParentNames = new List<string>(); for (int i = 0; i < boneCount; i++) { br.ReadByte(); //flag string name = new string(br.ReadChars(32)).Replace("\0", ""); string parentName = new string(br.ReadChars(32)).Replace("\0", ""); nodeParentNames.Add(parentName); Vector3 rotation = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); Vector3 position = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); nodes[i] = new AnimationNode(name, position, rotation); namesToNodes.Add(name, nodes[i]); ushort keyRotCount = br.ReadUInt16(); //Key frame rot count ushort keyPosCount = br.ReadUInt16(); //Key frame pos count //nodes[i].rotationFrames = new ModelBoneAnimationFrame[keyRotCount]; //nodes[i].translationFrames = new ModelBoneAnimationFrame[keyPosCount]; /* for (int j = 0; j < keyRotCount; j++) { nodes[i].rotationFrames[j].time = br.ReadSingle() * fps; //time nodes[i].rotationFrames[j].Displacement = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); nodes[i].rotationFrames[j].boneName = name; } for (int j = 0; j < keyPosCount; j++) { nodes[i].translationFrames[j].time = br.ReadSingle() * fps; //time nodes[i].translationFrames[j].Displacement = new Vector3(br.ReadSingle(), br.ReadSingle(), br.ReadSingle()); nodes[i].translationFrames[j].boneName = name; }*/ int count = (keyRotCount + keyPosCount) * 4; for (int j = 0; j < count; j++) br.ReadSingle(); } List<AnimationNode> rootNodeList = new List<AnimationNode>(); for (int i = 0; i < nodes.Length; i++) { if (namesToNodes.ContainsKey(nodeParentNames[i])) { AnimationNode node = namesToNodes[nodeParentNames[i]]; node.children.Add(nodes[i]); } else rootNodeList.Add(nodes[i]); } rootNodes = rootNodeList.ToArray(); for (int i = 0; i < rootNodes.Length; i++) { Matrix identityMat = Matrix.Identity; rootNodes[i].ApplyTransform(ref identityMat); } Matrix[] inverseMats = new Matrix[nodes.Length]; Matrix[] invRotMats = new Matrix[nodes.Length]; for (int i = 0; i < inverseMats.Length; i++) { inverseMats[i] = Matrix.Invert(nodes[i].Transform); invRotMats[i] = MathUtils.Invert3x3(nodes[i].Transform); } if (nodesAreAnimated) { for (int i = 0; i < verts.Length; i++) { verts[i].Position = Vector3.Transform(verts[i].Position, inverseMats[(int)verts[i].Index]); verts[i].Normal = Vector3.TransformNormal(verts[i].Normal, inverseMats[(int)verts[i].Index]); verts[i].Tangent = Vector3.TransformNormal(verts[i].Tangent, inverseMats[(int)verts[i].Index]); } } vertexBuffer.SetData<VertexPNTTI>(verts); for (int i = 0; i < nodes.Length; i++) { Vector3 Rotation = nodes[i].Rotation; Matrix transform = Matrix.CreateRotationX(Rotation.X) * Matrix.CreateRotationY(Rotation.Y) * Matrix.CreateRotationZ(Rotation.Z); transform.Translation = nodes[i].Translation; nodes[i].Transform = transform; } } } } }