private unsafe void Init(CollDetectInfo info, Vector3 dirToBody0, SmallCollPointInfo *pointInfos, int numPointInfos) { this.SkinInfo = info; this.dirToBody0 = dirToBody0; int ID0 = info.Skin0.GetMaterialID(info.IndexPrim0); int ID1 = info.Skin1.GetMaterialID(info.IndexPrim1); MaterialTable matTable = info.Skin0.CollisionSystem.MaterialTable; if (ID0 == (int)MaterialTable.MaterialID.UserDefined || (int)ID1 == (int)MaterialTable.MaterialID.UserDefined) { MaterialProperties prop0, prop1; if (ID0 == (int)MaterialTable.MaterialID.UserDefined) { prop0 = info.Skin0.GetMaterialProperties(info.IndexPrim0); } else { prop0 = matTable.GetMaterialProperties(ID0); } if (ID1 == (int)MaterialTable.MaterialID.UserDefined) { prop1 = info.Skin1.GetMaterialProperties(info.IndexPrim1); } else { prop1 = matTable.GetMaterialProperties(ID1); } MatPairProperties.Restitution = prop0.Elasticity * prop1.Elasticity; MatPairProperties.StaticFriction = prop0.StaticRoughness * prop1.StaticRoughness; MatPairProperties.DynamicFriction = prop0.DynamicRoughness * prop1.DynamicRoughness; } else { MatPairProperties = matTable.GetPairProperties(ID0, ID1); } numPointInfos = (numPointInfos > MaxCollisionPoints) ? MaxCollisionPoints : numPointInfos; NumCollPts = 0; for (int i = 0; i < numPointInfos; ++i) { if (freePtInfos.Count == 0) { freePtInfos.Push(new CollPointInfo()); } this.PointInfo[NumCollPts] = freePtInfos.Pop(); this.PointInfo[NumCollPts++].Init(ref pointInfos[i]); } }
/// <summary> /// CollisionInfos will be given out from a pool. If more than /// MaxCollisionPoints are passed in, the input positions will /// be silently truncated! /// </summary> /// <param name="info"></param> /// <param name="dirToBody0"></param> /// <param name="pointInfos"></param> /// <param name="numPointInfos"></param> /// <returns></returns> public static unsafe CollisionInfo GetCollisionInfo(CollDetectInfo info, Vector3 dirToBody0, SmallCollPointInfo *pointInfos, int numCollPts) { if (freeInfos.Count == 0) { freeInfos.Push(new CollisionInfo()); } CollisionInfo collInfo = freeInfos.Pop();//[freeInfos.Count - 1]; collInfo.Init(info, dirToBody0, pointInfos, numCollPts); //freeInfos.RemoveAt(freeInfos.Count - 1); return(collInfo); }
/// <summary> /// Skins are passed back because there maybe more than one skin /// per body, and the user can always get the body from the skin. /// </summary> /// <param name="collDetectInfo"></param> /// <param name="dirToBody0"></param> /// <param name="pointInfos"></param> public unsafe abstract void CollisionNotify(ref CollDetectInfo collDetectInfo, ref Vector3 dirToBody0, SmallCollPointInfo *pointInfos, int numCollPts);
public override unsafe void CollisionNotify(ref CollDetectInfo collDetectInfo, ref Vector3 dirToBody0, SmallCollPointInfo *pointInfos, int numCollPts) { CollisionInfo info; var skin0 = collDetectInfo.Skin0; var skin1 = collDetectInfo.Skin1; if (skin0?.Owner != null) { var generateContactPoints = skin0.OnCollisionEvent(skin0, skin1); if (skin1 != null) { generateContactPoints &= skin1.OnCollisionEvent(skin1, skin0); } if (generateContactPoints) { info = CollisionInfo.GetCollisionInfo(collDetectInfo, dirToBody0, pointInfos, numCollPts); colls.Add(info); skin0.Collisions.Add(info); if (skin1?.Owner != null) { skin1.Collisions.Add(info); } } } else if (skin1?.Owner != null) { var generateContactPoints = skin1.OnCollisionEvent(skin1, skin0); if (skin0 != null) { generateContactPoints &= skin0.OnCollisionEvent(skin0, skin1); } if (generateContactPoints) { info = CollisionInfo.GetCollisionInfo(collDetectInfo, -dirToBody0, pointInfos, numCollPts); colls.Add(info); skin1.Collisions.Add(info); if (skin0?.Owner != null) { skin0.Collisions.Add(info); } } } else { System.Diagnostics.Debug.WriteLine("Collision detected with both skin bodies null."); } }
/// <summary> /// Detect BoxPlane Collisions. /// </summary> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box; Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box; JPlane oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as JPlane; JPlane newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as JPlane; Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix; Vector3 newBoxCen = Vector3.Transform(newBox.GetCentre(), newPlaneInvTransform); // quick check float centreDist = Distance.PointPlaneDistance(newBoxCen, newPlane); if (centreDist > collTolerance + newBox.GetBoundingRadiusAroundCentre()) { return; } Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix; Vector3[] newPts; newBox.GetCornerPoints(out newPts); Vector3[] oldPts; oldBox.GetCornerPoints(out oldPts); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; for (int i = 0; i < 8; ++i) { Vector3.Transform(ref oldPts[i], ref oldPlaneInvTransform, out oldTransPts[i]); Vector3.Transform(ref newPts[i], ref newPlaneInvTransform, out newPts[i]); float oldDepth = -Distance.PointPlaneDistance(ref oldTransPts[i], oldPlane); float newDepth = -Distance.PointPlaneDistance(ref newPts[i], newPlane); if (MathHelper.Max(oldDepth, newDepth) > -collTolerance) { if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses instead of reallocating. collPts[numCollPts].R0 = oldPts[i] - body0Pos; collPts[numCollPts].R1 = oldPts[i] - body1Pos; collPts[numCollPts++].InitialPenetration = oldDepth; } } } if (numCollPts > 0) { collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts); } } #if !USE_STACKALLOC 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 qqfx 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 pt1, out triEdge0); #endregion #region qqfx 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 qqfx 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> /// Detect BoxBox Collisions. /// </summary> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Box box0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box; Box box1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; Box oldBox0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box; Box oldBox1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; Matrix dirs0 = box0.Orientation; Matrix dirs1 = box1.Orientation; seperatingAxes[0] = dirs0.Right; seperatingAxes[1] = dirs0.Up; seperatingAxes[2] = dirs0.Backward; seperatingAxes[3] = dirs1.Right; seperatingAxes[4] = dirs1.Up; seperatingAxes[5] = dirs1.Backward; Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]); Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]); Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]); Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]); Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]); Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]); Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]); Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]); Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]); // 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 < 15; ++i) { // If we can't normalise the axis, skip it float l2 = seperatingAxes[i].LengthSquared(); if (l2 < JiggleMath.Epsilon) { continue; } overlapDepth[i] = float.MaxValue; if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, collTolerance)) { return; } } // The box overlap, find the seperation depth closest to 0. float minDepth = float.MaxValue; int minAxis = -1; for (i = 0; i < 15; ++i) { // If we can't normalise the axis, skip it float l2 = seperatingAxes[i].LengthSquared(); if (l2 < JiggleMath.Epsilon) { continue; } // Normalise the separation axis and depth float invl = 1.0f / (float)System.Math.Sqrt(l2); seperatingAxes[i] *= invl; overlapDepth[i] *= invl; // If this axis is the minmum, select it if (overlapDepth[i] < minDepth) { minDepth = overlapDepth[i]; minAxis = i; } } if (minAxis == -1) { return; } // Make sure the axis is facing towards the 0th box. // if not, invert it Vector3 D = box1.GetCentre() - box0.GetCentre(); Vector3 N = seperatingAxes[minAxis]; float depth = overlapDepth[minAxis]; if (Vector3.Dot(D, N) > 0.0f) { N *= -1.0f; } float minA = MathHelper.Min(box0.SideLengths.X, MathHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z)); float minB = MathHelper.Min(box1.SideLengths.X, MathHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z)); float combinationDist = 0.05f * MathHelper.Min(minA, minB); // the contact points bool contactPointsFromOld = true; contactPts.Clear(); if (depth > -JiggleMath.Epsilon) { GetBoxBoxIntersectionPoints(contactPts, oldBox0, oldBox1, combinationDist, collTolerance); } int numPts = contactPts.Count; if (numPts == 0) { contactPointsFromOld = false; GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, collTolerance); } numPts = contactPts.Count; Vector3 body0OldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1OldPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; Vector3 body0NewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero; Vector3 body1NewPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.Position : Vector3.Zero; #region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos; Vector3 bodyDelta; Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta); Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta); Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta); #endregion #region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N); float bodyDeltaLen; Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen); #endregion float oldDepth = depth + bodyDeltaLen; unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; Vector3 SATPoint; switch (minAxis) { // Box0 face, Box1 corner collision case 0: case 1: case 2: { // Get the lowest point on the box1 along box1 normal GetSupportPoint(out SATPoint, box1, -N); break; } // We have a Box2 corner/Box1 face collision case 3: case 4: case 5: { // Find with vertex on the triangle collided GetSupportPoint(out SATPoint, box0, N); break; } // We have an edge/edge collision case 6: case 7: case 8: case 9: case 10: case 11: case 12: case 13: case 14: { { // Retrieve which edges collided. i = minAxis - 6; int ia = i / 3; int ib = i - ia * 3; // find two P0, P1 point on both edges. Vector3 P0, P1; GetSupportPoint(out P0, box0, N); GetSupportPoint(out P1, box1, -N); // Find the edge intersection. // plane along N and F, and passing through PB Vector3 box0Orient, box1Orient; JiggleUnsafe.Get(ref box0.transform.Orientation, ia, out box0Orient); JiggleUnsafe.Get(ref box1.transform.Orientation, ib, out box1Orient); #region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]); Vector3 planeNormal; Vector3.Cross(ref N, ref box1Orient, out planeNormal); #endregion #region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1); float planeD; Vector3.Dot(ref planeNormal, ref P1, out planeD); #endregion // find the intersection t, where Pintersection = P0 + t*box edge dir #region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal); float div; Vector3.Dot(ref box0Orient, ref planeNormal, out div); #endregion // plane and ray colinear, skip the intersection. if (System.Math.Abs(div) < JiggleMath.Epsilon) { return; } float t = (planeD - Vector3.Dot(P0, planeNormal)) / div; // point on edge of box0 #region REFERENCE: P0 += box0Orient * t; P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0); #endregion #region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N); Vector3.Multiply(ref N, 0.5f * depth, out SATPoint); Vector3.Add(ref SATPoint, ref P0, out SATPoint); #endregion } break; } default: throw new Exception("Impossible switch"); } // distribute the depth according to the distance to the SAT point if (numPts > 0) { float minDist = float.MaxValue; float maxDist = float.MinValue; for (i = 0; i < numPts; ++i) { float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint); if (dist < minDist) { minDist = dist; } if (dist > maxDist) { maxDist = dist; } } if (maxDist < minDist + JiggleMath.Epsilon) { maxDist = minDist + JiggleMath.Epsilon; } // got some intersection points for (i = 0; i < numPts; ++i) { float minDepthScale = 0.0f; float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint); float depthDiv = System.Math.Max(JiggleMath.Epsilon, maxDist - minDist); float depthScale = (dist - minDist) / depthDiv; depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth; if (contactPointsFromOld) { if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(contactPts[i].Pos - body0OldPos, contactPts[i].Pos - body1OldPos, depth); } } else { if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(contactPts[i].Pos - body0NewPos, contactPts[i].Pos - body1NewPos, depth); } } } } else { #region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth)); //collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth)); Vector3 cp0; Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0); Vector3 cp1; Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1); if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(ref cp0, ref cp1, oldDepth); } #endregion } // report Collisions collisionFunctor.CollisionNotify(ref info, ref N, collPts, numCollPts); } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }
/// <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(); Matrix 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 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 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 } }
/// <summary> /// CollDetectSphereStaticMeshOverlap /// </summary> /// <param name="oldSphere"></param> /// <param name="newSphere"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> 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); // BEN-BUG-FIX: Replaced 0.0f with -sphereTolR. if (distToCentre < -sphereTolR || 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) { // BEN-OPTIMISATION: Reuse 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 } }
/// <summary> /// CollDetectSphereStaticMeshSweep /// </summary> /// <param name="oldSphere"></param> /// <param name="newSphere"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> 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) { // BEN-OPTIMISATION: Reuse existing collPts. collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = 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) { // BEN-OPTIMISATION: Reuse existing collPts. collPts[numCollPts].R0 = pt2 - body0Pos; collPts[numCollPts].R1 = pt2 - 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 } }
/// <summary> /// CollisionNotify gets called by the CollisionSystem each time a /// Collision is detected. /// </summary> /// <param name="collDetectInfo"></param> /// <param name="dirToBody0"></param> /// <param name="pointInfos"></param> /// <param name="numCollPts"></param> public override unsafe void CollisionNotify(ref CollDetectInfo collDetectInfo, ref Vector3 dirToBody0, SmallCollPointInfo *pointInfos, int numCollPts) { CollisionInfo info; // shortcuts to save typing it over and over CollisionSkin skin0 = collDetectInfo.Skin0; CollisionSkin skin1 = collDetectInfo.Skin1; // if more than one point, add another that is in the middle - collision if ((skin0 != null) && (skin0.Owner != null)) { // if either skin say don't generate contact points, then we don't bool generateContactPoints = skin0.OnCollisionEvent(skin0, skin1); if (skin1 != null) { generateContactPoints &= skin1.OnCollisionEvent(skin1, skin0); } if (generateContactPoints) { info = CollisionInfo.GetCollisionInfo(collDetectInfo, dirToBody0, pointInfos, numCollPts); colls.Add(info); skin0.Collisions.Add(info); if ((skin1 != null) && (skin1.Owner != null)) { skin1.Collisions.Add(info); } } } else if ((skin1 != null) && (skin1.Owner != null)) { // if either skin say don't generate contact points, then we don't bool generateContactPoints = skin1.OnCollisionEvent(skin1, skin0); if (skin0 != null) { generateContactPoints &= skin0.OnCollisionEvent(skin0, skin1); } if (generateContactPoints) { info = CollisionInfo.GetCollisionInfo(collDetectInfo, -dirToBody0, pointInfos, numCollPts); colls.Add(info); skin1.Collisions.Add(info); if ((skin0 != null) && (skin0.Owner != null)) { skin0.Collisions.Add(info); } } } else { System.Diagnostics.Debug.WriteLine("Collision detected with both skin bodies null."); } }
/// <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); } } } }
/// <summary> /// /// </summary> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0); Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0); JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1); JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1); Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix; Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix; unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; // the start { Vector3 oldCapsuleStartPos = Vector3.Transform(oldCapsule.Position, oldPlaneInvTransform); Vector3 newCapsuleStartPos = Vector3.Transform(newCapsule.Position, newPlaneInvTransform); float oldDist = Distance.PointPlaneDistance(oldCapsuleStartPos, oldPlane); float newDist = Distance.PointPlaneDistance(newCapsuleStartPos, newPlane); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position8(s) Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * oldPlane.Normal; collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth); } } // the end { Vector3 oldCapsuleEndPos = Vector3.Transform(oldCapsule.GetEnd(), oldPlaneInvTransform); Vector3 newCapsuleEndPos = Vector3.Transform(newCapsule.GetEnd(), newPlaneInvTransform); float oldDist = Distance.PointPlaneDistance(oldCapsuleEndPos, oldPlane); float newDist = Distance.PointPlaneDistance(newCapsuleEndPos, newPlane); if (System.Math.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldCapsule.GetEnd() - oldCapsule.Radius * oldPlane.Normal; collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth); } if (numCollPts > 0) { collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts); } } } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; Vector3 averageNormal = Vector3.Zero; // the start { float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position); newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPts[numCollPts].R0 = worldPos - body0Pos; collPts[numCollPts].R1 = worldPos - body1Pos; collPts[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } // the end { Vector3 oldEnd = oldCapsule.GetEnd(); Vector3 newEnd = newCapsule.GetEnd(); float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd); newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldEnd - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPts[numCollPts].R0 = worldPos - body0Pos; collPts[numCollPts].R1 = worldPos - body1Pos; collPts[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref averageNormal); collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts); } } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }
/// <summary> /// Detect BoxHeightmap Collisions. /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box; Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box; Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; Vector3[] oldPts, newPts; oldBox.GetCornerPoints(out oldPts); newBox.GetCornerPoints(out newPts); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; Vector3 collNormal = Vector3.Zero; for (int i = 0; i < 8; ++i) { Vector3 newPt = newPts[i]; float newDist; Vector3 normal; newHeightmap.GetHeightAndNormal(out newDist, out normal, newPt); if (newDist < collTolerance) { Vector3 oldPt = oldPts[i]; float oldDist = oldHeightmap.GetHeight(oldPt); #region REFERENCE: collPts.Add(new CollPointInfo(oldPt - body0Pos, oldPt - body1Pos, -oldDist)); Vector3 pt0; Vector3 pt1; Vector3.Subtract(ref oldPt, ref body0Pos, out pt0); Vector3.Subtract(ref oldPt, ref body1Pos, out pt1); if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPts[numCollPts].R0 = pt0; collPts[numCollPts].R1 = pt1; collPts[numCollPts++].InitialPenetration = -oldDist; } #endregion #region REFERENCE: collNormal += normal; Vector3.Add(ref collNormal, ref normal, out collNormal); #endregion } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }