public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
CapsuleShape capsule0 = (CapsuleShape)body0.getCollisionShape();
CapsuleShape capsule1 = (CapsuleShape)body1.getCollisionShape();
VIntTransform transform0 = body0.getWorldTransform();
VIntTransform transform1 = body1.getWorldTransform();
VInt3 p00 = transform0.TransformPoint(capsule0.getUpAxis() * capsule0.getHalfHeight());
VInt3 p01 = transform0.TransformPoint(capsule0.getUpAxis() * -capsule0.getHalfHeight());
VInt3 p10 = transform1.TransformPoint(capsule1.getUpAxis() * capsule1.getHalfHeight());
VInt3 p11 = transform1.TransformPoint(capsule1.getUpAxis() * -capsule1.getHalfHeight());
VInt3 x, y;
VFixedPoint dist2 = Distance.SegmentSegmentDist2(p00, p01 - p00, p10, p11 - p10, out x, out y);
VFixedPoint dist = FMath.Sqrt(dist2);
VFixedPoint penetration = dist - (capsule0.getRadius() + capsule1.getRadius());
if (penetration > Globals.getContactBreakingThreshold())
{
return;
}
VInt3 diff = (x - y) / dist;
VInt3 posWorldOnA = x - diff * capsule0.getRadius();
VInt3 posWorldOnB = y + diff * capsule1.getRadius();
ManifoldPoint contractPoint = new ManifoldPoint(posWorldOnA, posWorldOnB, diff, penetration);
resultOut.addManifoldPoint(contractPoint);
}
public static bool sweepCapsuleCapsule(CapsuleShape lss0, VIntTransform transform0, VInt3 toPos, CapsuleShape lss1, VIntTransform transform1, ref VFixedPoint dist, ref VInt3 hitNormal)
{
VInt3 FromPos = transform0.position;
VFixedPoint radiusSun = lss0.getRadius() + lss1.getRadius();
VFixedPoint length = (toPos - FromPos).magnitude;
VInt3 dir = (toPos - FromPos) / length;
VInt3 lss0p0 = transform0.TransformPoint(lss0.getUpAxis() * lss0.getHalfHeight()), lss0p1 = transform0.TransformPoint(lss0.getUpAxis() * -lss0.getHalfHeight());
VInt3 lss1p0 = transform1.TransformPoint(lss1.getUpAxis() * lss1.getHalfHeight()), lss1p1 = transform1.TransformPoint(lss1.getUpAxis() * -lss1.getHalfHeight());
bool initialOverlapStatus = false;
VFixedPoint tmp = VFixedPoint.Zero;
if (lss0.getHalfHeight() < Globals.EPS)
{
initialOverlapStatus = Distance.distancePointSegmentSquared(lss1p0, lss1p1, lss0p0, ref tmp) < radiusSun * radiusSun;
}
else if (lss1.getHalfHeight() < Globals.EPS)
{
initialOverlapStatus = Distance.distancePointSegmentSquared(lss0p0, lss0p1, lss1p0, ref tmp) < radiusSun * radiusSun;
}
else
{
VInt3 x, y;
initialOverlapStatus = Distance.SegmentSegmentDist2(lss0p0, lss0p1 - lss0p0, lss1p0, lss1p1 - lss1p0, out x, out y) < radiusSun * radiusSun;
}
if (initialOverlapStatus)
{
dist = VFixedPoint.Zero;
hitNormal = (FromPos - toPos).Normalize();
return(true);
}
// 1. Extrude lss0 by lss1's length
// 2. Inflate extruded shape by lss1's radius
// 3. Raycast against resulting quad
VInt3 D = (lss0p1 - lss0p0) * VFixedPoint.Half;
VInt3 p0 = lss1p0 - D, p1 = lss1p1 - D, p0b = lss1p0 + D, p1b = lss1p1 + D;
VInt3 normal = VInt3.Cross(p1b - p0b, p1 - p0b); normal = normal.Normalize();
dist = VFixedPoint.One; bool status = false;
VInt3 pa, pb, pc;
if (VInt3.Dot(normal, dir) >= VFixedPoint.Zero)
{
pc = p0 - normal * radiusSun;
pa = p1 - normal * radiusSun;
pb = p1b - normal * radiusSun;
}
else
{
pc = p0 + normal * radiusSun;
pa = p1 + normal * radiusSun;
pb = p1b + normal * radiusSun;
}
VFixedPoint t = VFixedPoint.Zero, u = VFixedPoint.Zero, v = VFixedPoint.Zero;
if (rayQuad(transform1.position, dir, pa, pb, pc, ref t, ref u, ref v, true) && t >= VFixedPoint.Zero && t < length)
{
dist = t / length;
status = true;
}
if (!status)
{
VInt3[] caps = new VInt3[]
{
p0, p1, p1, p1b, p1b, p0b, p0b, p0
};
VInt3 tmpNormal = VInt3.zero;
for (int i = 0; i < 4; i++)
{
VFixedPoint s = VFixedPoint.Zero;
if (CapsuleRaytestAlgorithm.raycastCapsule(FromPos, toPos, caps[i * 2], caps[i * 2 + 1], radiusSun, ref tmpNormal, ref s))
{
if (s > VFixedPoint.Zero && s < dist)
{
dist = s;
status = true;
}
}
}
}
if (status)
{
VInt3 x, y;
Distance.SegmentSegmentDist2(lss0p0 + dir * length * dist, lss0p1 - lss0p0, lss1p0, lss1p1 - lss1p0, out x, out y);
hitNormal = (x - y).Normalize();
}
return(status);
}