public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (CapsulePart)partA; var b = (CapsulePart)partB; DoOverlapTest(cf, a, b, Vector3.Zero); }
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 = (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 = (PolyhedronPart)partB; DoOverlapTest(cf, a, b, Vector3.Zero); }
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 Execute(CollisionFunctor cf) { while (_axisX.Count > _rows) { SetCapacity(_rows * _growFactor); } _tasks.AddTask(SortAndSweepX); _tasks.AddTask(SortAndSweepY); _tasks.AddTask(SortAndSweepZ); _tasks.Execute(); _newItems = 0; for (int i = 0; i < _rows; i++) { for (int j = 0; j < _columns; j++) { uint block = _matrixX[i, j] & _matrixY[i, j] & _matrixZ[i, j]; if (block > 0) { for (int k = 0; k < sizeof(uint); k++) { if (((1 << k) & block) > 0) { _tasks.AddTask(cf.WritePair, _list[i], _list[j * sizeof(uint) + k]); } } } } } _tasks.Execute(); }
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (CapsulePart)partA; var b = (PolyhedronPart)partB; DoOverlapTest(cf, a, b, Vector3.Zero); }
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 OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (PolyhedronPart)partA; var b = (PlanePart)partB; PolyhedronPlane.DoOverlapTest(cf, a, b, 0f); }
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 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 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 override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (CapsulePart)partA; var b = (MeshPart)partB; var tf = OverlapFunctor; tf.Initialize(cf, a, b, Vector3.Zero); b.ProcessTriangles(tf); }
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 override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (PolyhedronPart)partA; var b = (MeshPart)partB; var tf = Functor; tf.Initialize(cf, a, b, delta); b.ProcessTriangles(tf); }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (SpherePart)partA; var b = (MeshPart)partB; var tf = SweptFunctor; tf.Initialize(cf, a, b, delta); b.ProcessTriangles(tf); }
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); }
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 void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { if (ci.FeatureA.Type == PolyhedronFeatureType.None || ci.FeatureB.Type == PolyhedronFeatureType.None) { System.Diagnostics.Debug.WriteLine("Unhandled collision case!"); } // calculate contact manifold if (ci.FeatureB.Type == PolyhedronFeatureType.Vertex) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexVertexPoint(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeVertexPoint(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceVertexPoint(cf, a, b, ref ci); } } else if (ci.FeatureB.Type == PolyhedronFeatureType.Edge) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexEdgePoint(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeEdgePoints(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceEdgePoints(cf, a, b, ref ci); } } else if (ci.FeatureB.Type == PolyhedronFeatureType.Face) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexFacePoint(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeFacePoints(cf, a, b, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceFacePoints(cf, a, b, ref ci); } } }
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 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 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); } } } }
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); }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (CapsulePart)partA; var b = (CapsulePart)partB; Vector3 step, offset = Vector3.Zero; int steps = (int)(delta.Length() / a.World.Radius * 0.9f); Vector3.Divide(ref delta, steps, out step); while (steps-- >= 0 && !DoOverlapTest(cf, a, b, offset)) { Vector3.Add(ref offset, ref step, out offset); } }
public void Initialize(CollisionFunctor cf, PolyhedronPart a, MeshPart b, Vector3 delta) { _cf = cf; _a = a; _b = b; _delta = delta; _useSweptTest = _delta != Vector3.Zero; Depth = float.MaxValue; a.Center(out _center); // transform bounding box to body space b.BoundingBox(out BoundingBox); AlignedBox.Transform(ref BoundingBox, ref b.TransformInverse, out BoundingBox); }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (PolyhedronPart)partA; var b = (PlanePart)partB; var plane = b.Plane; float dx; Vector3.Dot(ref plane.Normal, ref delta, out dx); if (dx > 0f) { dx = 0f; } PolyhedronPlane.DoOverlapTest(cf, a, b, -dx); }
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (CapsulePart)partA; var b = (PolyhedronPart)partB; Vector3 step, offset = Vector3.Zero; int steps = (int)(delta.Length() / a.World.Radius * 0.9f); Vector3.Divide(ref delta, steps, out step); while (steps-- >= 0 && !DoOverlapTest(cf, a, b, offset)) { Vector3.Add(ref offset, ref step, out offset); } }
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b) { _cf = cf; _a = a; _b = b; _radius = a.World.Radius * b.TransformInverse.Scale; _radiusSquared = _radius * _radius; Depth = float.MaxValue; Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _center); BoundingBox.Minimum = BoundingBox.Maximum = _center; var radius = new Vector3(_radius); Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum); Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum); }
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 CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci) { if (ci.FeatureB.Type == TriangleFeatureType.Vertex) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexVertexPoint(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeVertexPoint(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceVertexPoint(cf, a, b, ref tri, ref ci); } } else if (ci.FeatureB.Type == TriangleFeatureType.Edge) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexEdgePoint(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeEdgePoints(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceEdgePoints(cf, a, b, ref tri, ref ci); } } else if (ci.FeatureB.Type == TriangleFeatureType.Face) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) { CalculateVertexFacePoint(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) { CalculateEdgeFacePoints(cf, a, b, ref tri, ref ci); } else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) { CalculateFaceFacePoints(cf, a, b, ref tri, ref ci); } } }
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 SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta) { var a = (CapsulePart)partA; var b = (MeshPart)partB; Vector3 step, offset = Vector3.Zero; int steps = (int)(delta.Length() / a.World.Radius * 0.9f); Vector3.Divide(ref delta, steps, out step); var tf = OverlapFunctor; while (steps-- >= 0) { tf.Initialize(cf, a, b, offset); b.ProcessTriangles(tf); if (tf.HasCollision) break; Vector3.Add(ref offset, ref step, out offset); } }
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 void Initialize(CollisionFunctor cf, CapsulePart a, MeshPart b, Vector3 offset) { _cf = cf; _b = b; _radius = a.World.Radius * b.TransformInverse.Scale; _radiusSquared = _radius * _radius; _offset = offset; _hasCollision = false; Vector3.Add(ref a.World.P1, ref _offset, out _cap.P1); Vector3.Add(ref a.World.P2, ref _offset, out _cap.P2); // calculate points and bounding box in body space var radius = new Vector3(_radius); Vector3.Transform(ref _cap.P1, ref b.TransformInverse.Combined, out _cap.P1); Vector3.Transform(ref _cap.P2, ref b.TransformInverse.Combined, out _cap.P2); AlignedBox.Fit(ref _cap.P1, ref _cap.P2, out BoundingBox); Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum); Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum); }
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); } } }
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b, Vector3 delta) { _cf = cf; _a = a; _b = b; _radius = a.World.Radius * b.TransformInverse.Scale; _radiusSquared = _radius * _radius; Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _path.P1); Vector3.Transform(ref delta, ref b.TransformInverse.Orientation, out delta); Vector3.Multiply(ref delta, b.TransformInverse.Scale, out delta); Vector3.Add(ref _path.P1, ref delta, out _path.P2); AlignedBox.Fit(ref _path.P1, ref _path.P2, out BoundingBox); var radius = new Vector3(_radius); Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum); Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum); }
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); }
/// <summary> /// When overridden in a derived class, executes the broad phase collision detection and provides /// results to the functor. /// </summary> /// <param name="cf">The functor that will receive the results of the collision detection.</param> public abstract void Execute(CollisionFunctor cf);
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; }
/// <summary> /// When overridden in a derived class, performs a swept (or simulated-swept) test between two parts. /// </summary> /// <param name="cf">The functor to which all collisions are reported.</param> /// <param name="partA">The first part to test.</param> /// <param name="partB">The second part to test.</param> /// <param name="delta">The direction and magnitude of movement of partA, relative to partB.</param> public abstract void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta);
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); } } }
private static void CalculateContactPoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci) { if (ci.FeatureA.Type == PolyhedronFeatureType.None || ci.FeatureB.Type == PolyhedronFeatureType.None) //System.Diagnostics.Debug.WriteLine("Unhandled collision case!"); // calculate contact manifold if (ci.FeatureB.Type == PolyhedronFeatureType.Vertex) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) CalculateVertexVertexPoint(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) CalculateEdgeVertexPoint(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) CalculateFaceVertexPoint(cf, a, b, ref ci); } else if (ci.FeatureB.Type == PolyhedronFeatureType.Edge) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) CalculateVertexEdgePoint(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) CalculateEdgeEdgePoints(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) CalculateFaceEdgePoints(cf, a, b, ref ci); } else if (ci.FeatureB.Type == PolyhedronFeatureType.Face) { if (ci.FeatureA.Type == PolyhedronFeatureType.Vertex) CalculateVertexFacePoint(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Edge) CalculateEdgeFacePoints(cf, a, b, ref ci); else if (ci.FeatureA.Type == PolyhedronFeatureType.Face) CalculateFaceFacePoints(cf, a, b, ref ci); } }
public override void OverlapTest(CollisionFunctor cf, Part partA, Part partB) { var a = (PolyhedronPart)partA; var b = (PolyhedronPart)partB; CollisionInfo cur = new CollisionInfo(), final = new CollisionInfo { Depth = float.PositiveInfinity }; Vector3 v; // axes: face normals of A for (int i = 0; i < a.FaceCount; i++) { a.FaceNormal(i, out cur.NormalNeg); Vector3.Negate(ref cur.NormalNeg, out cur.Normal); a.World(a.Face(i)[0], out v); cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f); Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X); cur.FeatureB = b.ExtremeVertex(ref cur.Normal); cur.Depth = cur.FeatureB.X - cur.FeatureA.X; if (cur.Depth <= -Constants.Epsilon) return; else if (cur.Depth < final.Depth) final = cur; } // axes: face normals of B for (int i = 0; i < b.FaceCount; i++) { b.FaceNormal(i, out cur.Normal); Vector3.Negate(ref cur.Normal, out cur.NormalNeg); b.World(b.Face(i)[0], out v); cur.FeatureB = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f); Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureB.X); cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg); cur.FeatureA.X = -cur.FeatureA.X; cur.Depth = cur.FeatureB.X - cur.FeatureA.X; if (cur.Depth <= -Constants.Epsilon) return; else if (cur.Depth < final.Depth) final = cur; } // axes: crossed edges from A and B Vector3 centerA, centerB; a.Center(out centerA); b.Center(out centerB); for (int i = 0; i < a.EdgeVectorCount; i++) { for (int j = 0; j < b.EdgeVectorCount; j++) { Vector3 eva, evb; a.EdgeVector(i, out eva); b.EdgeVector(j, out evb); Vector3.Cross(ref eva, ref evb, out cur.Normal); if (cur.Normal.LengthSquared() < Constants.Epsilon) continue; cur.Normal.Normalize(); float ca, cb; Vector3.Dot(ref cur.Normal, ref centerA, out ca); Vector3.Dot(ref cur.Normal, ref centerB, out cb); if (ca < cb) { cur.NormalNeg = cur.Normal; Vector3.Negate(ref cur.NormalNeg, out cur.Normal); } else Vector3.Negate(ref cur.Normal, out cur.NormalNeg); // ignore this axis if it's close to the one we already have float d; Vector3.Dot(ref cur.Normal, ref final.Normal, out d); if (Math.Abs(1f - d) < Constants.Epsilon) continue; cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg); cur.FeatureA.X = -cur.FeatureA.X; cur.FeatureB = b.ExtremeVertex(ref cur.Normal); cur.Depth = cur.FeatureB.X - cur.FeatureA.X; if (cur.Depth <= -Constants.Epsilon) return; else if (final.Depth - cur.Depth >= Constants.Epsilon) { final = cur; } } } if (final.FeatureA.Type == PolyhedronFeatureType.Vertex) { final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, -final.FeatureB.X); final.FeatureA.X = -final.FeatureA.X; } if (final.FeatureB.Type == PolyhedronFeatureType.Vertex) { final.FeatureB = b.ExtremeFeature(ref final.Normal, final.FeatureA.X); } CalculateContactPoints(cf, a, b, ref final); }
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); //} }