public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; var oldCapsule = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule; var newCapsule = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule; var oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward); var newSeg = new Segment(oldCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward); var radSum = newCapsule.Radius + newSphere.Radius; var oldDistSq = Distance.PointSegmentDistanceSq(out var oldt, oldSphere.Position, oldSeg); var newDistSq = Distance.PointSegmentDistanceSq(out var newt, newSphere.Position, newSeg); if (MathHelper.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance)) { var segPos = oldSeg.GetPoint(oldt); var delta = oldSphere.Position - segPos; var dist = (float)System.Math.Sqrt(oldDistSq); var depth = radSum - dist; if (dist > JiggleMath.Epsilon) { delta /= dist; } else { delta = Vector3.TransformNormal(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360)))); } var worldPos = segPos + (oldCapsule.Radius - 0.5f * depth) * delta; unsafe { var collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1); } } }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; var oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; var newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; var oldDist = oldBox.GetDistanceToPoint(out var oldBoxPoint, oldSphere.Position); var newDist = newBox.GetDistanceToPoint(out var newBoxPoint, newSphere.Position); var oldDepth = oldSphere.Radius - oldDist; var newDepth = newSphere.Radius - newDist; if (System.Math.Max(oldDepth, newDepth) > -collTolerance) { Vector3 dir; if (oldDist < -JiggleMath.Epsilon) { dir = oldBoxPoint - oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else if (oldDist > JiggleMath.Epsilon) { dir = oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else { dir = oldSphere.Position - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } unsafe { var collInfo = new SmallCollPointInfo(oldBoxPoint - body0Pos, oldBoxPoint - body1Pos, oldDepth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; var oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Plane; var newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Plane; var newPlaneInvTransform = newPlane.InverseTransformMatrix; var oldPlaneInvTransform = oldPlane.InverseTransformMatrix; var oldSpherePos = Vector3.Transform(oldSphere.Position, oldPlaneInvTransform); var newSpherePos = Vector3.Transform(newSphere.Position, newPlaneInvTransform); var oldDist = Distance.PointPlaneDistance(oldSpherePos, oldPlane); var newDist = Distance.PointPlaneDistance(newSpherePos, newPlane); if (System.Math.Min(newDist, oldDist) > collTolerance + newSphere.Radius) { return; } var oldDepth = oldSphere.Radius - oldDist; var worldPos = oldSphere.Position - oldSphere.Radius * oldPlane.Normal; unsafe { var collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth); collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, &collInfo, 1); } }
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldCapsule0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; var newCapsule0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; var oldSeg0 = new Segment(oldCapsule0.Position, oldCapsule0.Length * oldCapsule0.Orientation.Backward); var newSeg0 = new Segment(newCapsule0.Position, newCapsule0.Length * newCapsule0.Orientation.Backward); var oldCapsule1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule; var newCapsule1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule; var oldSeg1 = new Segment(oldCapsule1.Position, oldCapsule1.Length * oldCapsule1.Orientation.Backward); var newSeg1 = new Segment(newCapsule1.Position, newCapsule1.Length * newCapsule1.Orientation.Backward); var radSum = newCapsule0.Radius + newCapsule1.Radius; var oldDistSq = Distance.SegmentSegmentDistanceSq(out var oldt0, out var oldt1, oldSeg0, oldSeg1); var newDistSq = Distance.SegmentSegmentDistanceSq(out var newt0, out var newt1, newSeg0, newSeg1); if (System.Math.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance)) { var pos0 = oldSeg0.GetPoint(oldt0); var pos1 = oldSeg1.GetPoint(oldt1); var delta = pos0 - pos1; var dist = (float)System.Math.Sqrt(oldDistSq); var depth = radSum - dist; if (dist > JiggleMath.Epsilon) { delta /= dist; } else { delta = Vector3.TransformNormal(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360)))); } var worldPos = pos1 + (oldCapsule1.Radius - 0.5f * depth) * delta; unsafe { var collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1); } } }
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere0 = (Sphere)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0); var newSphere0 = (Sphere)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0); var oldSphere1 = (Sphere)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1); var newSphere1 = (Sphere)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1); var oldDelta = oldSphere0.Position - oldSphere1.Position; var newDelta = newSphere0.Position - oldSphere1.Position; var oldDistSq = oldDelta.LengthSquared(); var newDistSq = newDelta.LengthSquared(); var radSum = newSphere0.Radius + newSphere1.Radius; if (System.Math.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance)) { var oldDist = (float)System.Math.Sqrt(oldDistSq); var depth = radSum - oldDist; if (oldDist > JiggleMath.Epsilon) { oldDelta /= oldDist; } else { oldDelta = Vector3.TransformNormal(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360)))); } var worldPos = oldSphere1.Position + (oldSphere1.Radius - 0.5f * depth) * oldDelta; unsafe { var collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref oldDelta, &collInfo, 1); } } }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; var oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; var newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; newHeightmap.GetHeightAndNormal(out var newDist, out var normal, newSphere.Position); if (newDist < collTolerance + newSphere.Radius) { var oldDist = oldHeightmap.GetHeight(oldSphere.Position); var depth = oldSphere.Radius - oldDist; var oldPt = oldSphere.Position - oldSphere.Radius * normal; unsafe { var ptInfo = new SmallCollPointInfo(oldPt - body0Pos, oldPt - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref normal, &ptInfo, 1); } } }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; var newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; var oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward); var newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward); var radius = oldCapsule.Radius; var oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; var newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; var oldDistSq = Distance.SegmentBoxDistanceSq(out var oldSegT, out var oldBoxT0, out var oldBoxT1, out var oldBoxT2, oldSeg, oldBox); var newDistSq = Distance.SegmentBoxDistanceSq(out var newSegT, out var newBoxT0, out var newBoxT1, out var newBoxT2, newSeg, newBox); if (MathHelper.Min(oldDistSq, newDistSq) < (radius + collTolerance) * (radius + collTolerance)) { var segPos = oldSeg.GetPoint(oldSegT); var boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right + oldBoxT1 * oldBox.Orientation.Up + oldBoxT2 * oldBox.Orientation.Backward; var dist = (float)System.Math.Sqrt(oldDistSq); var depth = radius - dist; Vector3 dir; if (dist > JiggleMath.Epsilon) { dir = segPos - boxPos; JiggleMath.NormalizeSafe(ref dir); } else if ((segPos - oldBox.GetCentre()).LengthSquared() > JiggleMath.Epsilon) { dir = segPos - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } else { dir = Vector3.Transform(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360)))); } unsafe { var collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }