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