public FMatrix3(VIntTransform trans) : this(
trans.right.x, trans.up.x, trans.forward.x,
trans.right.y, trans.up.y, trans.forward.y,
trans.right.z, trans.up.z, trans.forward.z
)
{
}
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 void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
bool needSwap = body0.getCollisionShape() is CapsuleShape;
CollisionObject sphereObject = needSwap ? body1 : body0;
CollisionObject capsuleObject = needSwap ? body0 : body1;
SphereShape sphere = (SphereShape)sphereObject.getCollisionShape();
CapsuleShape capsule = (CapsuleShape)capsuleObject.getCollisionShape();
VInt3 spherePos = sphereObject.getWorldTransform().position;
VIntTransform capsuleTransform = capsuleObject.getWorldTransform();
VInt3 p0 = capsuleTransform.TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight());
VInt3 p1 = capsuleTransform.TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight());
VFixedPoint param = VFixedPoint.Zero;
VFixedPoint dist2 = Distance.distancePointSegmentSquared(p0, p1, spherePos, ref param);
VFixedPoint dist = FMath.Sqrt(dist2);
VFixedPoint penetration = dist - (sphere.getRadius() + capsule.getRadius());
if (penetration > Globals.getContactBreakingThreshold())
{
return;
}
VInt3 lineWorldPos = p0 * (VFixedPoint.One - param) + p1 * param;
VInt3 diff = (spherePos - lineWorldPos) / dist;
VInt3 sphereWorldPos = spherePos - diff * sphere.getRadius();
VInt3 capsuleWorldPos = lineWorldPos + diff * capsule.getRadius();
ManifoldPoint contractPoint = new ManifoldPoint(needSwap ? capsuleWorldPos : sphereWorldPos, needSwap ? sphereWorldPos: capsuleWorldPos, diff * (needSwap ? -1 : 1), penetration);
resultOut.addManifoldPoint(contractPoint);
}
public void warp(VInt3 origin)
{
VIntTransform transform = me.getWorldTransform();
transform.position = origin;
me.setWorldTransform(transform);
}
public static VFixedPoint distancePointBoxSquared(VInt3 point, VIntTransform boxTransform, VInt3 boxExtent, ref VInt3 boxParam)
{
VInt3 closest = boxTransform.InverseTransformPoint(point);
VFixedPoint sqrDistance = VFixedPoint.Zero;
for (int ax = 0; ax < 3; ax++)
{
if (closest[ax] < -boxExtent[ax])
{
VFixedPoint delta = closest[ax] + boxExtent[ax];
sqrDistance += delta * delta;
closest[ax] = -boxExtent[ax];
}
else if (closest[ax] > boxExtent[ax])
{
VFixedPoint delta = closest[ax] - boxExtent[ax];
sqrDistance += delta * delta;
closest[ax] = boxExtent[ax];
}
}
boxParam = closest;
return(sqrDistance);
}
void playerStep(CollisionWorld collisionWorld, VFixedPoint dt)
{
wasOnGround = onGround();
verticalVelocity -= gravity * dt;
if (verticalVelocity > VFixedPoint.Zero && verticalVelocity > jumpSpeed)
{
verticalVelocity = jumpSpeed;
}
if (verticalVelocity < VFixedPoint.Zero && verticalVelocity.Abs() > fallSpeed.Abs())
{
verticalVelocity = -fallSpeed.Abs();
}
verticalOffset = verticalVelocity * dt;
VIntTransform transform = me.getWorldTransform();
//climb up slope
stepUp(collisionWorld);
VFixedPoint dtMoving = dt;
VInt3 move = walkDirection * dtMoving;
stepForwardAndStrafe(collisionWorld, move);
//slide down slope
stepDown(collisionWorld, dt);
transform.position = currentPosition;
me.setWorldTransform(transform);
}
static bool expandSegment(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
VInt3 q0 = aBuf[0] - bBuf[0];
VInt3 q1 = aBuf[1] - bBuf[1];
VInt3 v = q1 - q0;
VInt3 absV = v.Abs();
VFixedPoint x = absV.x, y = absV.y, z = absV.z;
VInt3 axis = VInt3.right;
if (x > y && z > y)
{
axis = VInt3.up;
}
else if (x > z)
{
axis = VInt3.forward;
}
VInt3 n = VInt3.Cross(axis, v).Normalize();
VInt3 q2;
doSupport(a, b, transformA, transformB, n, out aBuf[2], out bBuf[2], out q2);
return(expandTriangle(a, b, transformA, transformB, ref numVerts, lowerBound, upperBound));
}
static bool expandPoint(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
VInt3 x = VInt3.right;
VInt3 q0;
doSupport(a, b, transformA, transformB, x, out aBuf[1], out bBuf[1], out q0);
return(expandSegment(a, b, transformA, transformB, ref numVerts, lowerBound, upperBound));
}
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax)
{
VInt3 center = t.position;
VFixedPoint radiusWithMargin = radius + margin + getMargin();
VInt3 extent = new VInt3(radiusWithMargin, radiusWithMargin, radiusWithMargin);
aabbMin = center - extent;
aabbMax = center + extent;
}
public static void calculateVelocity(VIntTransform transform0, VIntTransform transform1, VFixedPoint timeStep, ref VInt3 linVel, ref VInt3 angVel)
{
linVel = (transform1.position - transform0.position) / timeStep;
VIntQuaternion rotation = VIntQuaternion.FromToRotation(transform0.forward, transform1.forward);
angVel.x = rotation.x;
angVel.y = rotation.y;
angVel.z = rotation.z;
angVel = angVel.Normalize() * rotation.w / timeStep;
}
public VIntTransform Transform(VIntTransform trans)
{
VInt3 position = InverseTransformPoint(trans.position);
VInt3 right = InverseTransformDirection(trans.right);
VInt3 up = InverseTransformDirection(trans.up);
VInt3 forward = InverseTransformDirection(trans.forward);
VIntTransform result = new VIntTransform(position, right, up, forward);
return(result);
}
public static bool getSphereDistance(BoxShape boxShape, VIntTransform m44T, VInt3 sphereCenter, VFixedPoint radius, out VInt3 normal, out VFixedPoint penetrationDepth)
{
VInt3 boxHalfExtent = boxShape.getHalfExtent();
VFixedPoint boxMargin = boxShape.getMargin();
penetrationDepth = VFixedPoint.Zero;
// convert the sphere position to the box's local space
VInt3 sphereRelPos = m44T.InverseTransformPoint(sphereCenter);
// Determine the closest point to the sphere center in the box
VInt3 closestPoint = sphereRelPos;
closestPoint.x = (FMath.Min(boxHalfExtent.x, closestPoint.x));
closestPoint.x = (FMath.Max(-boxHalfExtent.x, closestPoint.x));
closestPoint.y = (FMath.Min(boxHalfExtent.y, closestPoint.y));
closestPoint.y = (FMath.Max(-boxHalfExtent.y, closestPoint.y));
closestPoint.z = (FMath.Min(boxHalfExtent.z, closestPoint.z));
closestPoint.z = (FMath.Max(-boxHalfExtent.z, closestPoint.z));
VFixedPoint intersectionDist = radius + boxMargin;
VFixedPoint contactDist = intersectionDist;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
VFixedPoint dist2 = normal.sqrMagnitude;
if (dist2 > contactDist * contactDist)
{
return(false);
}
VFixedPoint distance;
//special case if the sphere center is inside the box
if (dist2 <= Globals.EPS)
{
distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, ref closestPoint, out normal);
}
else //compute the penetration details
{
distance = normal.magnitude;
normal /= distance;
}
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
VInt3 tmp = m44T.TransformDirection(normal);
normal = tmp;
return(true);
}
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, VInt3 walkMove)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
targetPosition = currentPosition + walkMove;
VFixedPoint fraction = VFixedPoint.One;
int maxIter = 10;
while (fraction > VFixedPoint.Create(0.01f) && maxIter-- > 0)
{
start.position = currentPosition;
end.position = targetPosition;
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = results[0].fraction;
VInt3 hitNormalWorld = results[0].normal;
for (int i = 1; i < results.Count; i++)
{
VFixedPoint afraction = results[i].fraction;
if (afraction <= closestHitFraction)
{
closestHitFraction = afraction;
hitNormalWorld = results[i].normal;
}
}
fraction -= closestHitFraction;
updateTargetPositionBasedOnCollision(hitNormalWorld);
VInt3 currentDir = targetPosition - currentPosition;
VFixedPoint distance2 = currentDir.sqrMagnitude;
if (distance2 > Globals.EPS2)
{
currentDir = currentDir.Normalize();
if (VInt3.Dot(currentDir, normalizedDirection) <= VFixedPoint.Zero)
{
break;
}
}
else
{
break;
}
}
else
{
currentPosition = targetPosition;
}
}
}
static bool expandTriangle(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
numVerts = 3;
Facet f0 = addFacet(0, 1, 2, lowerBound, upperBound);
Facet f1 = addFacet(1, 0, 2, lowerBound, upperBound);
f0.link(0, f1, 0);
f0.link(1, f1, 2);
f0.link(2, f1, 1);
return(true);
}
public static void rayTestSingle(VInt3 fromPos, VInt3 toPos, CollisionObject collisionObject, RayResultCallback resultCallback)
{
VIntTransform collisionObjectTransform = collisionObject.getWorldTransform();
VFixedPoint t = VFixedPoint.Zero;
VInt3 normal = VInt3.zero;
VInt3 aabbMin, aabbMax;
collisionObject.getCollisionShape().getAabb(VIntTransform.Identity, out aabbMin, out aabbMax);
if (rayTestBox(fromPos, toPos, aabbMax, collisionObject.getWorldTransform(), ref t, ref normal))
{
resultCallback.addSingleResult(collisionObject, normal, t);
}
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
bool needSwap = body0.getCollisionShape() is CapsuleShape;
CollisionObject boxObject = needSwap ? body1 : body0;
CollisionObject capsuleObject = needSwap ? body0 : body1;
BoxShape boxShape = (BoxShape)boxObject.getCollisionShape();
CapsuleShape capsuleShape = (CapsuleShape)capsuleObject.getCollisionShape();
VIntTransform boxTransform = boxObject.getWorldTransform();
VIntTransform capsuleTransform = capsuleObject.getWorldTransform();
VInt3 p0 = capsuleTransform.TransformPoint(capsuleShape.getUpAxis() * capsuleShape.getHalfHeight()), p1 = capsuleTransform.TransformPoint(capsuleShape.getUpAxis() * -capsuleShape.getHalfHeight());
VFixedPoint lParam = VFixedPoint.Zero; VInt3 closestPointBoxLS = VInt3.zero;
VFixedPoint distSq = SegmentBoxDistance.distanceSegmentBoxSquared(p0, p1, boxShape.getHalfExtent(), boxTransform, ref lParam, ref closestPointBoxLS);
VInt3 closestPointBoxWS = boxTransform.TransformPoint(closestPointBoxLS);
VInt3 closestPointLineWS = p0 * (VFixedPoint.One - lParam) + p1 * lParam;
VFixedPoint dist = FMath.Sqrt(distSq) - capsuleShape.getRadius();
if (dist > VFixedPoint.Zero)
{
return;
}
if ((closestPointBoxWS - closestPointLineWS).sqrMagnitude > Globals.EPS2)
{
VInt3 normalOnBoxWS = (closestPointLineWS - closestPointBoxWS).Normalize();
ManifoldPoint contactPoint = new ManifoldPoint(needSwap ? closestPointLineWS - normalOnBoxWS * capsuleShape.getRadius() : closestPointBoxWS,
!needSwap ? closestPointLineWS - normalOnBoxWS * capsuleShape.getRadius() : closestPointBoxWS,
normalOnBoxWS * (needSwap ? 1 : -1), dist);
resultOut.addManifoldPoint(contactPoint);
}
else //box and line are intersected
{
//EPA
LineShape coreShape = new LineShape(capsuleShape);
VInt3 pa = VInt3.zero, pb = VInt3.zero, normal = VInt3.zero;
VFixedPoint depth = VFixedPoint.Zero;
PxGJKStatus result = EpaSolver.calcPenDepth(coreShape, boxShape, capsuleTransform, boxTransform, ref pa, ref pb, ref normal, ref depth);
if (result == PxGJKStatus.EPA_CONTACT)
{
ManifoldPoint contactPoint = new ManifoldPoint(needSwap ? pa - normal * capsuleShape.getRadius() : pb, !needSwap ? pa - normal * capsuleShape.getRadius() : pb, needSwap ? normal : -normal, depth - capsuleShape.getRadius());
resultOut.addManifoldPoint(contactPoint);
}
}
}
public static void transformAabb(VInt3 halfExtents, VFixedPoint margin, VIntTransform trans, out VInt3 aabbMinOut, out VInt3 aabbMaxOut)
{
VInt3 halfExtentsWithMargin = halfExtents;
halfExtentsWithMargin.x += margin;
halfExtentsWithMargin.y += margin;
halfExtentsWithMargin.z += margin;
VInt3 center = trans.position;
VInt3[] basis = trans.getTransposeBasis();
VInt3 extent = new VInt3(VInt3.Dot(basis[0].Abs(), halfExtents), VInt3.Dot(basis[1].Abs(), halfExtents), VInt3.Dot(basis[2].Abs(), halfExtents));
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax)
{
VInt3 halfExtents = p0.Abs();
VInt3[] basis = t.getTransposeBasis();
VInt3 center = t.position;
VInt3 extent = new VInt3();
extent.x = VInt3.Dot(halfExtents, basis[0].Abs());
extent.y = VInt3.Dot(halfExtents, basis[1].Abs());
extent.z = VInt3.Dot(halfExtents, basis[2].Abs());
aabbMin = center - extent;
aabbMax = center + extent;
}
//sphere move to collide capsule
static bool sweepSphereCapsule(SphereShape sphere, VIntTransform sphereTransform, VInt3 end,
CapsuleShape capsule, VIntTransform capsuleTransform, ref VInt3 normal, ref VFixedPoint t)
{
VInt3 move = end - sphereTransform.position;
VFixedPoint radiusSum = sphere.getRadius() + capsule.getRadius();
VInt3 capsuleP0 = capsuleTransform.TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight());
VInt3 capsuleP1 = capsuleTransform.TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight());
VInt3 spherePosition = sphereTransform.position;
VFixedPoint tmp = VFixedPoint.Zero;
if (Distance.distancePointSegmentSquared(capsuleP0, capsuleP1, spherePosition, ref tmp) < radiusSum * radiusSum)
{
t = VFixedPoint.Zero;
normal = -move.Normalize();
return(true);
}
VFixedPoint u0 = VFixedPoint.Zero;
VInt3 tmpNormal = VInt3.zero;
if (capsuleP0 == capsuleP1)
{
VInt3 ToPos = spherePosition + move;
if (SphereSphereSweepAlgorithm.sphereSphereSweep(sphere.getRadius(), spherePosition, ToPos, capsule.getRadius(), capsuleTransform.position, ref u0, ref tmp, ref tmpNormal))
{
t = u0;
normal = tmpNormal;
return(true);
}
else
{
return(false);
}
}
else if (CapsuleRaytestAlgorithm.raycastCapsule(spherePosition, end, capsuleP0, capsuleP1, radiusSum, ref normal, ref u0))
{
t = u0;
VFixedPoint param = VFixedPoint.Zero;
VInt3 movedSphereCenter = spherePosition + (end - spherePosition) * u0;
Distance.distancePointSegmentSquared(capsuleP0, capsuleP1, movedSphereCenter, ref param);
normal = movedSphereCenter - (capsuleP0 * (VFixedPoint.One - param) + capsuleP1 * param);
normal = normal.Normalize();
return(true);
}
return(false);
}
protected void stepDown(CollisionWorld collisionWorld, VFixedPoint dt)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
VFixedPoint additionalDownStep = wasOnGround ? stepHeight : VFixedPoint.Zero;
VInt3 stepDrop = upAxisDirection[upAxis] * (currentStepOffset + additionalDownStep);
VFixedPoint downVelocity = (additionalDownStep == VFixedPoint.Zero && verticalVelocity < VFixedPoint.Zero ? -verticalVelocity : VFixedPoint.Zero) * dt;
VInt3 gravityDrop = upAxisDirection[upAxis] * downVelocity;
targetPosition -= stepDrop;
targetPosition -= gravityDrop;
start.position = currentPosition; end.position = targetPosition;
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = VFixedPoint.One;
for (int i = 0; i < results.Count; i++)
{
VFixedPoint fraction = results[i].fraction;
if (VInt3.Dot(results[i].normal, upAxisDirection[upAxis]) < maxSlopeCosine)
{
continue;
}
if (fraction < closestHitFraction)
{
closestHitFraction = fraction;
}
}
// we dropped a fraction of the height -> hit floor
currentPosition = currentPosition * (VFixedPoint.One - closestHitFraction) + targetPosition * closestHitFraction;
verticalVelocity = VFixedPoint.Zero;
verticalOffset = VFixedPoint.Zero;
}
else
{
// we dropped the full height
currentPosition = targetPosition;
}
}
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax)
{
VInt3 halfExtents = VInt3.zero;
halfExtents[upAxis] = getHalfHeight();
VInt3[] basis = t.getTransposeBasis();
VInt3 center = t.position;
VInt3 extent = new VInt3();
extent.x = VInt3.Dot(halfExtents, basis[0].Abs());
extent.y = VInt3.Dot(halfExtents, basis[1].Abs());
extent.z = VInt3.Dot(halfExtents, basis[2].Abs());
aabbMin = center - extent - VInt3.one * getRadius();
aabbMax = center + extent + VInt3.one * getRadius();
}
static int extrudeBox(VInt3 aabbMax, VIntTransform world, VInt3 extrusionDir, Triangle[] tris, VInt3 dir)
{
computeBoxPoints(aabbMax, -aabbMax);
for (int i = 0; i < 12; i++)
{
int VRef0 = Indices[i * 3 + 0];
int VRef1 = Indices[i * 3 + 1];
int VRef2 = Indices[i * 3 + 2];
Triangle atriangle = boxTris[i];
atriangle.verts[0] = world.TransformPoint(boxP[VRef0]);
atriangle.verts[1] = world.TransformPoint(boxP[VRef1]);
atriangle.verts[2] = world.TransformPoint(boxP[VRef2]);
}
return(extrudeMesh(boxTris, extrusionDir, dir, tris));
}
public static void objectQuerySingle(CollisionObject castObject, VInt3 ToPos, CollisionObject collisionObject, List <CastResult> results, VFixedPoint allowedPenetration)
{
SphereShape testShape = (SphereShape)castObject.getCollisionShape();
SphereShape sphereShape = (SphereShape)collisionObject.getCollisionShape();
VIntTransform collisionObjectTransform = collisionObject.getWorldTransform();
VFixedPoint d = VFixedPoint.Zero;
VFixedPoint tmp = VFixedPoint.Zero;
VInt3 normal = VInt3.zero;
if (sphereSphereSweep(testShape.getRadius(), castObject.getWorldTransform().position, ToPos, sphereShape.getRadius(),
collisionObjectTransform.position, ref d, ref tmp, ref normal))
{
CastResult result = new CastResult();
result.hitObject = collisionObject;
result.fraction = d;
result.normal = normal;
results.Add(result);
}
}
protected void stepUp(CollisionWorld collisionWorld)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
targetPosition = currentPosition + upAxisDirection[upAxis] * (stepHeight + (verticalOffset > VFixedPoint.Zero ? verticalOffset : VFixedPoint.Zero));
start.position = currentPosition + upAxisDirection[upAxis] * (me.getCollisionShape().getMargin() + addedMargin);
end.position = targetPosition;
VInt3 up = -upAxisDirection[upAxis];
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = VFixedPoint.One;
for (int i = 0; i < results.Count; i++)
{
VFixedPoint fraction = results[i].fraction;
if (VInt3.Dot(results[i].normal, up) < VFixedPoint.Zero)
{
fraction = VFixedPoint.One;
}
if (fraction < closestHitFraction)
{
closestHitFraction = fraction;
}
}
// we moved up only a fraction of the step height
currentStepOffset = stepHeight * closestHitFraction;
currentPosition = currentPosition * (VFixedPoint.One - closestHitFraction) + targetPosition * closestHitFraction;
verticalVelocity = VFixedPoint.Zero;
verticalOffset = VFixedPoint.Zero;
}
else
{
currentStepOffset = stepHeight;
currentPosition = targetPosition;
}
}
static void doSupport(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, VInt3 dir
, out VInt3 supportA, out VInt3 supportB, out VInt3 support)
{
VInt3 dirInA = dir;
dirInA = transformA.InverseTransformDirection(dirInA);
VInt3 dirInB = -dir;
dirInB = transformB.InverseTransformDirection(dirInB);
VInt3 pInA = new VInt3();
VInt3 qInB = new VInt3();
pInA = a.support(dirInA);
qInB = b.support(dirInB);
supportA = transformA.TransformPoint(pInA);
supportB = transformB.TransformPoint(qInB);
support = supportA - supportB;
}
public static void objectQuerySingle(CollisionObject castObject, VInt3 ToPos, CollisionObject collisionObject, List <CastResult> results, VFixedPoint allowedPenetration)
{
VIntTransform castTransform = castObject.getWorldTransform();
bool needSwap = castObject.getCollisionShape() is BoxShape;
BoxShape box = (BoxShape)(needSwap ? castObject.getCollisionShape() : collisionObject.getCollisionShape());
VIntTransform boxTransform = needSwap ? castTransform : collisionObject.getWorldTransform();
SphereShape sphere = (SphereShape)(needSwap ? collisionObject.getCollisionShape() : castObject.getCollisionShape());
VInt3 spherePos = needSwap ? collisionObject.getWorldTransform().position : castObject.getWorldTransform().position;
VInt3 toPos = needSwap ? spherePos - (ToPos - castTransform.position) : ToPos;
VFixedPoint dist = VFixedPoint.Zero;
VInt3 normal = VInt3.zero;
if (sweepSphereBox(sphere, spherePos, toPos, box, boxTransform, ref dist, ref normal))
{
CastResult result = new CastResult();
result.hitObject = collisionObject;
result.fraction = dist;
result.normal = normal * (needSwap ? -1 : 1);
results.Add(result);
}
}
public void refreshContactPoints(VIntTransform trA, VIntTransform trB)
{
VInt3 tmp = VInt3.zero;
for (int i = cachedPoints - 1; i >= 0; i--)
{
ManifoldPoint manifoldPoint = pointCache[i];
manifoldPoint.positionWorldOnA = trA.TransformPoint(manifoldPoint.localPointA);
manifoldPoint.positionWorldOnB = trB.TransformPoint(manifoldPoint.localPointB);
manifoldPoint.distance = VInt3.Dot(manifoldPoint.normalWorldOnB, manifoldPoint.positionWorldOnA - manifoldPoint.positionWorldOnB);
}
VFixedPoint distance2d = VFixedPoint.Zero;
VInt3 projectedDifference = VInt3.zero, projectedPoint = VInt3.zero;
for (int i = cachedPoints - 1; i >= 0; i--)
{
ManifoldPoint manifoldPoint = pointCache[i];
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
}
}
}
public void addCollisionObject(CollisionObject collisionObject, short collisionFilterGroup, short collisionFilterMask)
{
collisionObjects.Add(collisionObject);
// calculate new AABB
// TODO: check if it's overwritten or not
VIntTransform trans = collisionObject.getWorldTransform();
VInt3 minAabb = new VInt3();
VInt3 maxAabb = new VInt3();
collisionObject.getCollisionShape().getAabb(trans, out minAabb, out maxAabb);
BroadphaseNativeType type = collisionObject.getCollisionShape().getShapeType();
collisionObject.setBroadphaseHandle(getBroadphase().createProxy(
minAabb,
maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask,
dispatcher1));
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
BoxShape box0 = (BoxShape)body0.getCollisionShape();
BoxShape box1 = (BoxShape)body1.getCollisionShape();
VIntTransform transform0 = body0.getWorldTransform();
VIntTransform transform1 = body1.getWorldTransform();
VInt3 box0Extent = box0.getHalfExtent();
VInt3 box1Extent = box1.getHalfExtent();
doBoxBoxGenerateContacts(box0Extent, box1Extent, transform0, transform1, resultOut);
if (resultOut.getContactPointsNum() == 0)
{
VInt3 pa = VInt3.zero, pb = VInt3.zero, normal = VInt3.zero;
VFixedPoint depth = VFixedPoint.Zero;
PxGJKStatus result = EpaSolver.calcPenDepth(box0, box1, transform0, transform1, ref pa, ref pb, ref normal, ref depth);
if (result == PxGJKStatus.EPA_CONTACT)
{
ManifoldPoint contactPoint = new ManifoldPoint(pa, pb, normal, depth);
resultOut.addManifoldPoint(contactPoint);
}
}
}
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);
}