public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
{
_simplexSolver = solver;
_convexA = convexShapeA;
_convexB = convexShapeB;
}
public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)
{
_shapeA = shapeA;
_shapeB = shapeB;
_transformA = Matrix.Identity;
_transformB = Matrix.Identity;
}
public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_simplexSolver = simplexSolver;
_penetrationDepthSolver = penetrationDepthSolver;
_convexA = convexA;
_convexB = convexB;
}
public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
ISimplexSolver simplexSolver,
IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_cachedSeparatingAxis = new Vector3(0, 0, 1);
_penetrationDepthSolver = penetrationDepthSolver;
_simplexSolver = simplexSolver;
_minkowskiA = objectA;
_minkowskiB = objectB;
_ignoreMargin = false;
_lastUsedMethod = -1;
_catchDegeneracies = 1;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
{
float radialmargin = 0;
GjkEpaSolver.Results results;
if (GjkEpaSolver.Collide(convexA, transformA,
convexB, transformB,
radialmargin, out results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
ptrA = results.Witnesses[0];
ptrB = results.Witnesses[1];
return true;
}
ptrA = new Vector3();
ptrB = new Vector3();
return false;
}
public static bool Collide(ConvexShape shapeA, Matrix wtrsA,
ConvexShape shapeB, Matrix wtrsB,
float radialmargin,
out Results results)
{
/* Initialize */
results = new Results();
results.Witnesses = new Vector3[2];
results.Witnesses[0] =
results.Witnesses[1] =
results.Normal = new Vector3();
results.Depth = 0;
results.ResultStatus = Results.Status.Separated;
results.EpaIterations = 0;
results.GjkIterations = 0;
/* Use GJK to locate origin */
GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA,
wtrsB, wtrsB.Translation, shapeB,
radialmargin + GjkEpa.EpaAccuracy);
bool collide = gjk.SearchOrigin();
results.GjkIterations = gjk.Iterations + 1;
if (collide)
{
/* Then EPA for penetration depth */
GjkEpa.Epa epa = new GjkEpa.Epa(gjk);
float pd = epa.EvaluatePD();
results.EpaIterations = epa.Iterations + 1;
if (pd > 0)
{
results.ResultStatus = Results.Status.Penetrating;
results.Normal = epa.Normal;
results.Depth = pd;
results.Witnesses[0] = epa.Nearest[0];
results.Witnesses[1] = epa.Nearest[1];
return true;
}
else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; }
}
else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; }
return false;
}
public void setMinkowskiB(ConvexShape minkB)
{
_minkowskiB = minkB;
}
public void setMinkowskiA(ConvexShape minkA)
{
_minkowskiA = minkA;
}
// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
// This allows more customization.
public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
if (collisionShape.IsConvex)
{
CastResult castResult = new CastResult();
castResult.Fraction = 1f; //??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
{
//add hit
if (castResult.Normal.LengthSquared() > 0.0001f)
{
castResult.Normal.Normalize();
if (castResult.Fraction < resultCallback.ClosestHitFraction)
{
CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
new LocalShapeInfo(),
castResult.Normal,
castResult.Fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
else
{
if (collisionShape.IsConcave)
{
TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.HitFraction = resultCallback.ClosestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound)
{
CompoundShape compoundShape = collisionShape as CompoundShape;
for (int i = 0; i < compoundShape.ChildShapeCount; i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
}
public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
{
_simplexSolver = simplexSolver;
_convexA = shapeA;
_convexB = shapeB;
}
public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
Matrix wrotationB, Vector3 positionB, ConvexShape shapeB)
: this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0)
{
}
public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
{
_simplexSolver = simplexSolver;
_convexA = shapeA;
_convexB = shapeB;
}
public void setMinkowskiB(ConvexShape minkB)
{
_minkowskiB = minkB;
}
public void setMinkowskiA(ConvexShape minkA)
{
_minkowskiA = minkA;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
{
float radialmargin = 0;
GjkEpaSolver.Results results;
if (GjkEpaSolver.Collide(convexA, transformA,
convexB, transformB,
radialmargin, out results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
ptrA = results.Witnesses[0];
ptrB = results.Witnesses[1];
return(true);
}
ptrA = new Vector3();
ptrB = new Vector3();
return(false);
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
return false;
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f;//minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return res.HasResult;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
{
return(false);
}
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f; //minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return(res.HasResult);
}
public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
//Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
//col0->m_worldTransform,
float resultFraction = 1f;
float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
if (squareMotA < colA.CcdSquareMotionThreshold &&
squareMotB < colB.CcdSquareMotionThreshold)
{
return(resultFraction);
}
if (DisableCcd)
{
return(1f);
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convexA = colA.CollisionShape as ConvexShape;
SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
//Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
{
colA.HitFraction = result.Fraction;
}
if (colB.HitFraction > result.Fraction)
{
colB.HitFraction = result.Fraction;
}
if (resultFraction > result.Fraction)
{
resultFraction = result.Fraction;
}
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convexB = colB.CollisionShape as ConvexShape;
SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
{
colA.HitFraction = result.Fraction;
}
if (colB.HitFraction > result.Fraction)
{
colB.HitFraction = result.Fraction;
}
if (resultFraction > result.Fraction)
{
resultFraction = result.Fraction;
}
}
}
return(resultFraction);
}
