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