public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (CapsulePart)partA; var b = (PlanePart)partB; float ax, bx; Vector3 pa, pb, radius; Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius); Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx); Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax); if (ax - bx - a.World.Radius < Constants.Epsilon) { Vector3.Add(ref a.World.P1, ref radius, out pa); b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax); if (ax - bx - a.World.Radius < Constants.Epsilon) { Vector3.Add(ref a.World.P2, ref radius, out pa); b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (CapsulePart)partA; var b = (PlanePart)partB; float ax, bx, dx; Vector3 pa, pb, radius; Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out radius); Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx); Vector3.Dot(ref b.Plane.Normal, ref delta, out dx); dx = MathHelper.Min(dx, 0f); Vector3.Dot(ref b.Plane.Normal, ref a.World.P1, out ax); if (ax - bx - a.World.Radius + dx < Constants.Epsilon) { Vector3.Add(ref a.World.P1, ref radius, out pa); b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } Vector3.Dot(ref b.Plane.Normal, ref a.World.P2, out ax); if (ax - bx - a.World.Radius + dx < Constants.Epsilon) { Vector3.Add(ref a.World.P2, ref radius, out pa); b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } }
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { int[] faceA = a.Face(ci.FeatureA.Index); int[] faceB = b.Face(ci.FeatureB.Index); Vector3 pa, pb, n; a.World(faceA[0], out pa); a.FaceNormal(ci.FeatureA.Index, out n); Plane planeA = new Plane(pa, n); b.World(faceB[0], out pb); Plane planeB = new Plane(pb, ci.Normal); // vertices of A contained in face of B for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i], out pa); planeB.ClosestPointTo(ref pa, out pb); if (b.IsPointOnFace(ci.FeatureB.Index, ref pb, true)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } // vertices of B contained in face of A for (int i = 0; i < faceB.Length; i++) { b.World(faceB[i], out pb); planeA.ClosestPointTo(ref pb, out pa); if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } // intersections of edges from both faces Segment ea, eb; for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1); a.World(faceA[i], out ea.P2); for (int j = 0; j < faceB.Length; j++) { b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1); b.World(faceB[j], out eb.P2); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } } }
public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset) { Vector3 pa, pb, normal, v, center; float depth, r2 = a.World.Radius * a.World.Radius; bool foundAny = false; for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); var plane = new Plane(v, normal); Vector3.Add(ref a.World.Center, ref offset, out center); Vector3.Subtract(ref center, ref v, out v); Vector3.Dot(ref normal, ref v, out depth); if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon) { continue; } plane.ClosestPointTo(ref center, out pb); if (b.IsPointOnFace(i, ref pb, true)) { Vector3.Subtract(ref pb, ref center, out v); if (v.LengthSquared() - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref normal); return(true); } } else { int[] face = b.Face(i); for (int j = 0; j < face.Length; j++) { float s; Segment edge; b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1); b.World(face[j], out edge.P2); edge.ClosestPointTo(ref center, out s, out pb); Vector3.Subtract(ref center, ref pb, out normal); if (normal.LengthSquared() - r2 < Constants.Epsilon) { normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref normal); foundAny = true; } } } } return(foundAny); }
public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset) { Vector3 pa, pb, normal, v, center; float depth, r2 = a.World.Radius * a.World.Radius; bool foundAny = false; for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); var plane = new Plane(v, normal); Vector3.Add(ref a.World.Center, ref offset, out center); Vector3.Subtract(ref center, ref v, out v); Vector3.Dot(ref normal, ref v, out depth); if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon) continue; plane.ClosestPointTo(ref center, out pb); if (b.IsPointOnFace(i, ref pb, true)) { Vector3.Subtract(ref pb, ref center, out v); if (v.LengthSquared() - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref normal); return true; } } else { int[] face = b.Face(i); for (int j = 0; j < face.Length; j++) { float s; Segment edge; b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1); b.World(face[j], out edge.P2); edge.ClosestPointTo(ref center, out s, out pb); Vector3.Subtract(ref center, ref pb, out normal); if (normal.LengthSquared() - r2 < Constants.Epsilon) { normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref normal); foundAny = true; } } } } return foundAny; }
private static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, CapsulePart b, Vector3 offset) { Segment capa, capb = new Segment(b.World.P1, b.World.P2); Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); Vector3 pa, pb, normal, v; float sa, sb, r2 = a.World.Radius + b.World.Radius; r2 *= r2; // find the closest point between the two capsules Segment.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb); Vector3.Subtract(ref pa, ref pb, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) return false; if (normal.LengthSquared() < Constants.Epsilon) normal = Vector3.UnitZ; normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); // if the two capsules are nearly parallel, an additional support point provides stability if (sa == 0f || sa == 1f) { pa = sa == 0f ? capa.P2 : capa.P1; capb.ClosestPointTo(ref pa, out sa, out pb); } else if (sb == 0f || sb == 1f) { pb = sb == 0f ? capb.P2 : capb.P1; capa.ClosestPointTo(ref pb, out sb, out pa); } else return true; float dist; Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); } return true; }
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { int[] faceA = a.Face(ci.FeatureA.Index); Vector3 pa, pb, n; a.World(faceA[0], out pa); a.FaceNormal(ci.FeatureA.Index, out n); Plane planeA = new Plane(pa, n); Plane planeB = new Plane(tri.V1, tri.Normal); // vertices of A contained in face of B for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i], out pa); planeB.ClosestPointTo(ref pa, out pb); if (tri.Contains(ref pb)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } for (int i = 1; i <= 3; i++) { tri.Vertex(i, out pb); planeA.ClosestPointTo(ref pb, out pa); if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } // intersection of edges from both faces Segment ea, eb; for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1); a.World(faceA[i], out ea.P2); for (int j = 1; j <= 3; j++) { tri.Edge(j, out eb); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } } }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (SpherePart)partA; var b = (CapsulePart)partB; Segment path; path.P1 = a.World.Center; Vector3.Add(ref path.P1, ref delta, out path.P2); Capsule cap = b.World; cap.Radius += a.World.Radius; Segment capSegment = new Segment(b.World.P1, b.World.P2); float k; Vector3 pa, pb, normal; cap.Intersect(ref path, out k, out pa); if (k <= 1f) { capSegment.ClosestPointTo(ref pa, out k, out pb); Vector3.Subtract(ref pa, ref pb, out normal); normal.Normalize(); Vector3.Multiply(ref normal, b.World.Radius, out pa); Vector3.Add(ref pb, ref pa, out pb); Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref normal); } }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (SpherePart)partA; var b = (PlanePart)partB; Vector3 pa = a.World.Center, pb; float ax, bx; Vector3.Dot(ref b.Plane.Normal, ref b.Plane.P, out bx); Vector3.Dot(ref b.Plane.Normal, ref pa, out ax); if (ax - bx - a.World.Radius >= Constants.Epsilon) { Vector3.Add(ref a.World.Center, ref delta, out pa); Vector3.Dot(ref b.Plane.Normal, ref pa, out ax); if (ax - bx - a.World.Radius >= Constants.Epsilon) { return; } } b.Plane.ClosestPointTo(ref pa, out pb); Vector3.Multiply(ref b.Plane.Normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); }
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (SpherePart)partA; var b = (CapsulePart)partB; Segment cap = new Segment(b.World.P1, b.World.P2); Vector3 pa, pb, normal, v; float r2 = a.World.Radius + b.World.Radius; r2 *= r2; float sb; cap.ClosestPointTo(ref a.World.Center, out sb, out pb); Vector3.Subtract(ref a.World.Center, ref pb, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) { return; } normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref a.World.Center, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); cf.WritePoint(ref pa, ref pb, ref normal); }
public override void Process(int count) { Vector3 pa, pb, normal; float ax, bx; for (int i = 0; i < count; i++) { // skip triangles that the sphere is behind Vector3.Dot(ref Buffer[i].Normal, ref _path.P1, out ax); Vector3.Dot(ref Buffer[i].Normal, ref Buffer[i].V1, out bx); if (bx - ax > 0f) { continue; } float k; Buffer[i].ClosestPointTo(ref _path, out k, out pa, out pb); Vector3.Subtract(ref pa, ref pb, out normal); if (normal.LengthSquared() - _radiusSquared >= Constants.Epsilon) { continue; } normal = Buffer[i].Normal; Vector3.Transform(ref normal, ref _b.Transform.Orientation, out normal); Vector3.Multiply(ref normal, -_a.World.Radius, out pa); Vector3.Add(ref _a.World.Center, ref pa, out pa); Vector3.Transform(ref pb, ref _b.Transform.Combined, out pb); _cf.WritePoint(ref pa, ref pb, ref normal); } }
private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; a.World(ci.FeatureA.Index, out pa); tri.Vertex(ci.FeatureB.Index, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; a.World(ci.FeatureA.Index, out pa); var plane = new Plane(tri.V1, ci.Normal); plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea, eb, eab; int[] edgeA = a.Edge(ci.FeatureA.Index); a.World(edgeA[0], out ea.P1); a.World(edgeA[1], out ea.P2); int[] faceB = b.Face(ci.FeatureB.Index); b.World(faceB[0], out pb); var planeB = new Plane(pb, ci.Normal); planeB.ClosestPointTo(ref ea.P1, out eab.P1); planeB.ClosestPointTo(ref ea.P2, out eab.P2); int count = 0; if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P1, true)) { count++; cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal); } if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P2, true)) { count++; cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal); } for (int i = 0; i < faceB.Length && count < 2; i++) { b.World(faceB[i == 0 ? faceB.Length - 1 : i - 1], out eb.P1); b.World(faceB[i], out eb.P2); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { count++; cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } }
private static void CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; Segment eb; float sb; tri.Edge(ci.FeatureB.Index, out eb); a.World(ci.FeatureA.Index, out pa); eb.ClosestPointTo(ref pa, out sb, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; b.World(ci.FeatureB.Index, out pb); a.World(a.Face(ci.FeatureA.Index)[0], out pa); var plane = new Plane(pa, ci.Normal); plane.ClosestPointTo(ref pb, out pa); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea, eba, eb; int[] faceA = a.Face(ci.FeatureA.Index); a.World(faceA[0], out pa); Plane planeA = new Plane(pa, ci.Normal); tri.Edge(ci.FeatureB.Index, out eb); planeA.ClosestPointTo(ref eb.P1, out eba.P1); planeA.ClosestPointTo(ref eb.P2, out eba.P2); int count = 0; if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true)) { count++; cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal); } if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true)) { count++; cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal); } for (int i = 0; i < faceA.Length && count < 2; i++) { a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1); a.World(faceA[i], out ea.P2); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { count++; cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } }
private static void CalculateVertexEdgePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; Segment eb; float sb; int[] edgeB = b.Edge(ci.FeatureB.Index); b.World(edgeB[0], out eb.P1); b.World(edgeB[1], out eb.P2); a.World(ci.FeatureA.Index, out pa); eb.ClosestPointTo(ref pa, out sb, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateEdgeVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea; float sa; int[] edgeA = a.Edge(ci.FeatureA.Index); a.World(edgeA[0], out ea.P1); a.World(edgeA[1], out ea.P2); tri.Vertex(ci.FeatureB.Index, out pb); ea.ClosestPointTo(ref pb, out sa, out pa); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea, eb, eab; int[] edge = a.Edge(ci.FeatureA.Index); a.World(edge[0], out ea.P1); a.World(edge[1], out ea.P2); Plane planeB = new Plane(tri.V1, tri.Normal); planeB.ClosestPointTo(ref ea.P1, out eab.P1); planeB.ClosestPointTo(ref ea.P2, out eab.P2); int count = 0; if (tri.Contains(ref eab.P1)) { count++; cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal); } if (tri.Contains(ref eab.P2)) { count++; cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal); } for (int i = 1; i <= 3 && count < 2; i++) { tri.Edge(i, out eb); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { count++; cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } }
private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset) { Vector3 normalNeg; Vector3.Negate(ref b.Plane.Normal, out normalNeg); int vIndex = a.ExtremeVertex(ref normalNeg).Index; float ax, bx; Vector3 p = b.Plane.P; if (offset > 0f) { Vector3.Multiply(ref b.Plane.Normal, offset, out p); Vector3.Add(ref b.Plane.P, ref p, out p); } Vector3.Dot(ref b.Plane.Normal, ref p, out bx); for (int i = 0; i < a.FaceCount; i++) { var face = a.Face(i); bool skip = true; for (int j = 0; j < face.Length; j++) { if (face[j] == vIndex) { skip = false; break; } } if (skip) { continue; } for (int j = 0; j < face.Length; j++) { Vector3 pa, pb; a.World(face[j], out pa); Vector3.Dot(ref b.Plane.Normal, ref pa, out ax); if (ax - bx < Constants.Epsilon) { b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } } } }
private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset) { Vector3 normalNeg; Vector3.Negate(ref b.Plane.Normal, out normalNeg); int vIndex = a.ExtremeVertex(ref normalNeg).Index; float ax, bx; Vector3 p = b.Plane.P; if(offset > 0f) { Vector3.Multiply(ref b.Plane.Normal, offset, out p); Vector3.Add(ref b.Plane.P, ref p, out p); } Vector3.Dot(ref b.Plane.Normal, ref p, out bx); for (int i = 0; i < a.FaceCount; i++) { var face = a.Face(i); bool skip = true; for (int j = 0; j < face.Length; j++) { if (face[j] == vIndex) { skip = false; break; } } if (skip) continue; for (int j = 0; j < face.Length; j++) { Vector3 pa, pb; a.World(face[j], out pa); Vector3.Dot(ref b.Plane.Normal, ref pa, out ax); if (ax - bx < Constants.Epsilon) { b.Plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal); } } } }
public override void Process(int count) { Vector3 pa, pb, normal; float ax, bx; for (int i = 0; i < count; i++) { // record minimal depth and skip triangles that the sphere is behind Vector3.Dot(ref Buffer[i].Normal, ref _center, out ax); Vector3.Dot(ref Buffer[i].Normal, ref Buffer[i].V1, out bx); float d = bx - ax; if (d > 0f && d < Depth) { this.NearestTriangle = Buffer[i]; } Depth = Math.Min(d, Depth); if (d > 0f) { continue; } bool useTriNormal = Buffer[i].ClosestPointTo(ref _center, out pb); Vector3.Subtract(ref _center, ref pb, out normal); if (normal.LengthSquared() - _radiusSquared >= Constants.Epsilon) { continue; } if (useTriNormal) { normal = Buffer[i].Normal; } else { normal.Normalize(); } Vector3.Transform(ref normal, ref _b.Transform.Orientation, out normal); Vector3.Multiply(ref normal, -_a.World.Radius, out pa); Vector3.Add(ref _a.World.Center, ref pa, out pa); Vector3.Transform(ref pb, ref _b.Transform.Combined, out pb); _cf.WritePoint(ref pa, ref pb, ref normal); } }
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (SpherePart)partA; var b = (MeshPart)partB; var tf = OverlapFunctor; tf.Initialize(cf, a, b); b.ProcessTriangles(tf); // if the sphere is inside the mesh, push it out via the normal of least depth if (tf.Depth > 0f && tf.Depth < float.MaxValue) { Triangle tri; Vector3 pb; Triangle.Transform(ref tf.NearestTriangle, ref b.Transform, out tri); tri.Center(out pb); cf.WritePoint(ref a.World.Center, ref pb, ref tri.Normal); } }
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (SpherePart)partA; var b = (SpherePart)partB; float r2 = a.World.Radius + b.World.Radius; r2 *= r2; Vector3 pa, pb, normal; Vector3.Subtract(ref a.World.Center, ref b.World.Center, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) return; normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); Vector3.Multiply(ref normal, b.World.Radius, out pb); Vector3.Add(ref b.World.Center, ref pb, out pb); cf.WritePoint(ref pa, ref pb, ref normal); }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (SpherePart)partA; var b = (SpherePart)partB; float d2 = a.World.Radius + b.World.Radius; d2 *= d2; Vector3 v, p = b.World.Center, q = a.World.Center; Vector3.Add(ref q, ref delta, out v); Vector3.Subtract(ref v, ref q, out v); Vector3 pq; Vector3.Subtract(ref q, ref p, out pq); float ax, bx, cx; Vector3.Dot(ref v, ref v, out ax); Vector3.Dot(ref v, ref pq, out bx); Vector3.Dot(ref pq, ref pq, out cx); cx -= d2; float n = (-bx - (float)Math.Sqrt(bx * bx - ax * cx)) / ax; if (n <= 1f) { Vector3.Multiply(ref v, n, out q); Vector3.Add(ref a.World.Center, ref q, out q); Vector3 normal; Vector3.Subtract(ref q, ref p, out normal); normal.Normalize(); Vector3 pa, pb; Vector3.Multiply(ref normal, -a.World.Radius, out pa); Vector3.Add(ref a.World.Center, ref pa, out pa); Vector3.Multiply(ref normal, b.World.Radius, out pb); Vector3.Add(ref b.World.Center, ref pb, out pb); cf.WritePoint(ref pa, ref pb, ref normal); } //Segment path; //path.P1 = a.World.Center; //Vector3.Add(ref path.P1, ref delta, out path.P2); //Vector3.Subtract(ref path.P2, ref path.P1, out v); //v.Normalize(); //q = path.P1; //float scalar; //Vector3 normal, pa, pb; //path.ClosestPointTo(ref b.World.Center, out scalar, out pa); //Vector3.Subtract(ref pa, ref b.World.Center, out normal); //if (normal.LengthSquared() - r2 < Constants.Epsilon) //{ // normal.Normalize(); // Vector3.Multiply(ref normal, -a.World.Radius, out pa); // Vector3.Add(ref a.World.Center, ref pa, out pa); // Vector3.Multiply(ref normal, b.World.Radius, out pb); // Vector3.Add(ref b.World.Center, ref pb, out pb); // cf.WritePoint(ref pa, ref pb, ref normal); //} }
private static void CalculateVertexVertexPoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; a.World(ci.FeatureA.Index, out pa); b.World(ci.FeatureB.Index, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; a.World(ci.FeatureA.Index, out pa); b.World(b.Face(ci.FeatureB.Index)[0], out pb); var plane = new Plane(pb, ci.Normal); plane.ClosestPointTo(ref pa, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { int[] faceA = a.Face(ci.FeatureA.Index); int[] faceB = b.Face(ci.FeatureB.Index); Vector3 pa, pb, n; a.World(faceA[0], out pa); a.FaceNormal(ci.FeatureA.Index, out n); Plane planeA = new Plane(pa, n); b.World(faceB[0], out pb); Plane planeB = new Plane(pb, ci.Normal); // vertices of A contained in face of B for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i], out pa); planeB.ClosestPointTo(ref pa, out pb); if(b.IsPointOnFace(ci.FeatureB.Index, ref pb, true)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } // vertices of B contained in face of A for (int i = 0; i < faceB.Length; i++) { b.World(faceB[i], out pb); planeA.ClosestPointTo(ref pb, out pa); if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true)) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } // intersections of edges from both faces Segment ea, eb; for (int i = 0; i < faceA.Length; i++) { a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1); a.World(faceA[i], out ea.P2); for (int j = 0; j < faceB.Length; j++) { b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1); b.World(faceB[j], out eb.P2); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } } }
private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea, eb, eba; int[] edgeB = b.Edge(ci.FeatureB.Index); b.World(edgeB[0], out eb.P1); b.World(edgeB[1], out eb.P2); int[] faceA = a.Face(ci.FeatureA.Index); a.World(faceA[0], out pa); var planeA = new Plane(pa, ci.Normal); planeA.ClosestPointTo(ref eb.P1, out eba.P1); planeA.ClosestPointTo(ref eb.P2, out eba.P2); int count = 0; if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true)) { count++; cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal); } if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true)) { count++; cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal); } for(int i = 0; i < faceA.Length && count < 2; i++) { a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1); a.World(faceA[i], out ea.P2); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); if (sa > 0f && sa < 1f && sb > 0f && sb < 1f) { count++; cf.WritePoint(ref pa, ref pb, ref ci.Normal); } } }
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset) { Vector3 v, normal; float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius; Segment capa; Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); float curDepth, finalDepth = float.MaxValue; int faceIdx = -1; // find the face with the least penetration depth along its normal for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); Vector3.Dot(ref normal, ref v, out bx); Vector3.Dot(ref normal, ref capa.P1, out ax1); Vector3.Dot(ref normal, ref capa.P2, out ax2); bx += a.World.Radius; curDepth = Math.Max(bx - ax1, bx - ax2); if (curDepth < 0f) return false; if (curDepth < finalDepth) { faceIdx = i; finalDepth = curDepth; } } bool got1 = false, got2 = false; Vector3 pa, pb, pa1, pa2, pb1, pb2; pa1 = pa2 = pb1 = pb2 = Vector3.Zero; var face = b.Face(faceIdx); b.World(face[0], out v); b.FaceNormal(faceIdx, out normal); var plane = new Plane(v, normal); Vector3.Dot(ref normal, ref v, out bx); bx += a.World.Radius; // determine if either capsule point is inside the face pa1 = capa.P1; plane.ClosestPointTo(ref pa1, out pb1); Vector3.Dot(ref normal, ref pa1, out ax1); got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true); pa2 = capa.P2; plane.ClosestPointTo(ref pa2, out pb2); Vector3.Dot(ref normal, ref pa2, out ax1); if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true)) { if (got1) got2 = true; else { got1 = true; pa1 = pa2; pb1 = pb2; } } // if one capsule point is inside the face but one is not, try to generate a second point on an edge if (got1 ^ got2) { float sbPrev = float.NaN; int edgePrev = -1; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue; if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } } else if (!got1 && !got2) { // neither point is inside the face, so try all edges float sbPrev = float.NaN, edgePrev = float.NaN; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue; if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; } else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } // if only one edge was crossed, create a new normal instead of using the face normal if (got1 ^ got2) { Vector3.Subtract(ref pa1, ref pb1, out normal); normal.Normalize(); } } // write out points if (got1 || got2) { Vector3.Multiply(ref normal, -a.World.Radius, out v); if (got1) { Vector3.Add(ref pa1, ref v, out pa1); Vector3.Subtract(ref pa1, ref offset, out pa1); cf.WritePoint(ref pa1, ref pb1, ref normal); } if (got2) { Vector3.Add(ref pa2, ref v, out pa2); Vector3.Subtract(ref pa2, ref offset, out pa2); cf.WritePoint(ref pa2, ref pb2, ref normal); } } return got1 || got2; }
private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; tri.Vertex(ci.FeatureB.Index, out pb); a.World(a.Face(ci.FeatureA.Index)[0], out pa); var plane = new Plane(pa, ci.Normal); plane.ClosestPointTo(ref pb, out pa); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset) { Vector3 v, normal; float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius; Segment capa; Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); float curDepth, finalDepth = float.MaxValue; int faceIdx = -1; // find the face with the least penetration depth along its normal for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); Vector3.Dot(ref normal, ref v, out bx); Vector3.Dot(ref normal, ref capa.P1, out ax1); Vector3.Dot(ref normal, ref capa.P2, out ax2); bx += a.World.Radius; curDepth = Math.Max(bx - ax1, bx - ax2); if (curDepth < 0f) { return(false); } if (curDepth < finalDepth) { faceIdx = i; finalDepth = curDepth; } } bool got1 = false, got2 = false; Vector3 pa, pb, pa1, pa2, pb1, pb2; pa1 = pa2 = pb1 = pb2 = Vector3.Zero; var face = b.Face(faceIdx); b.World(face[0], out v); b.FaceNormal(faceIdx, out normal); var plane = new Plane(v, normal); Vector3.Dot(ref normal, ref v, out bx); bx += a.World.Radius; // determine if either capsule point is inside the face pa1 = capa.P1; plane.ClosestPointTo(ref pa1, out pb1); Vector3.Dot(ref normal, ref pa1, out ax1); got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true); pa2 = capa.P2; plane.ClosestPointTo(ref pa2, out pb2); Vector3.Dot(ref normal, ref pa2, out ax1); if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true)) { if (got1) { got2 = true; } else { got1 = true; pa1 = pa2; pb1 = pb2; } } // if one capsule point is inside the face but one is not, try to generate a second point on an edge if (got1 ^ got2) { float sbPrev = float.NaN; int edgePrev = -1; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) { continue; } if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } } else if (!got1 && !got2) { // neither point is inside the face, so try all edges float sbPrev = float.NaN, edgePrev = float.NaN; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) { continue; } if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; } else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } // if only one edge was crossed, create a new normal instead of using the face normal if (got1 ^ got2) { Vector3.Subtract(ref pa1, ref pb1, out normal); normal.Normalize(); } } // write out points if (got1 || got2) { Vector3.Multiply(ref normal, -a.World.Radius, out v); if (got1) { Vector3.Add(ref pa1, ref v, out pa1); Vector3.Subtract(ref pa1, ref offset, out pa1); cf.WritePoint(ref pa1, ref pb1, ref normal); } if (got2) { Vector3.Add(ref pa2, ref v, out pa2); Vector3.Subtract(ref pa2, ref offset, out pa2); cf.WritePoint(ref pa2, ref pb2, ref normal); } } return(got1 || got2); }
private static void CalculateEdgeEdgePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { Vector3 pa, pb; Segment ea, eb; int[] edgeA = a.Edge(ci.FeatureA.Index); a.World(edgeA[0], out ea.P1); a.World(edgeA[1], out ea.P2); tri.Edge(ci.FeatureB.Index, out eb); float sa, sb; Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb); cf.WritePoint(ref pa, ref pb, ref ci.Normal); }
public override void Process(int count) { Vector3 pa, pb, normal, v; float dist, d; bool useTriNormal; for (int i = 0; i < count; i++) { // check capsule endpoints int ptCount = 0; for (int j = 0; j < 2; j++) { pa = j == 0 ? _cap.P1 : _cap.P2; useTriNormal = Buffer[i].ClosestPointTo(ref pa, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - _radiusSquared < Constants.Epsilon) { if (useTriNormal) { normal = Buffer[i].Normal; } else { // compute normal from triangle edge Vector3.Subtract(ref pa, ref pb, out normal); // if point is behind triangle, negate normal Vector3.Dot(ref normal, ref Buffer[i].Normal, out d); if (d <= -Constants.Epsilon) { Vector3.Multiply(ref Buffer[i].Normal, -2f * d, out v); Vector3.Add(ref normal, ref v, out normal); } normal.Normalize(); } Vector3.Transform(ref normal, ref _b.Transform.Orientation, out normal); Vector3.Transform(ref pa, ref _b.Transform.Combined, out pa); Vector3.Transform(ref pb, ref _b.Transform.Combined, out pb); Vector3.Multiply(ref normal, -_radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Subtract(ref pa, ref _offset, out pa); _hasCollision = true; _cf.WritePoint(ref pa, ref pb, ref normal); ptCount++; } } if (ptCount > 1) { continue; } // check for capsule/edge intersection useTriNormal = Buffer[i].ClosestPointTo(ref _cap, out d, out pa, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (d >= Constants.Epsilon && d <= 1f - Constants.Epsilon && dist - _radiusSquared < Constants.Epsilon) { if (useTriNormal) { normal = Buffer[i].Normal; } else { // compute normal from triangle edge Vector3.Subtract(ref pa, ref pb, out normal); // if point is behind triangle, negate normal Vector3.Dot(ref normal, ref Buffer[i].Normal, out d); if (d <= -Constants.Epsilon) { Vector3.Multiply(ref Buffer[i].Normal, -2f * d, out v); Vector3.Add(ref normal, ref v, out normal); } normal.Normalize(); } // transform points back into world space Vector3.Transform(ref normal, ref _b.Transform.Orientation, out normal); Vector3.Transform(ref pa, ref _b.Transform.Combined, out pa); Vector3.Transform(ref pb, ref _b.Transform.Combined, out pb); Vector3.Multiply(ref normal, -_radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Subtract(ref pa, ref _offset, out pa); _cf.WritePoint(ref pa, ref pb, ref normal); _hasCollision = true; } } }
private static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, CapsulePart b, Vector3 offset) { Segment capa, capb = new Segment(b.World.P1, b.World.P2); Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); Vector3 pa, pb, normal, v; float sa, sb, r2 = a.World.Radius + b.World.Radius; r2 *= r2; // find the closest point between the two capsules Segment.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb); Vector3.Subtract(ref pa, ref pb, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) { return(false); } if (normal.LengthSquared() < Constants.Epsilon) { normal = Vector3.UnitZ; } normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); // if the two capsules are nearly parallel, an additional support point provides stability if (sa == 0f || sa == 1f) { pa = sa == 0f ? capa.P2 : capa.P1; capb.ClosestPointTo(ref pa, out sa, out pb); } else if (sb == 0f || sb == 1f) { pb = sb == 0f ? capb.P2 : capb.P1; capa.ClosestPointTo(ref pb, out sb, out pa); } else { return(true); } float dist; Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); } return(true); }