public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
SphereShape sphereA = bodyA.CollisionShape as SphereShape;
SphereShape sphereB = bodyB.CollisionShape as SphereShape;
Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
float len = diff.Length();
float radiusA = sphereA.Radius;
float radiusB = sphereB.Radius;
//if distance positive, don't generate a new contact
if (len > (radiusA + radiusB))
return;
//distance (negative means penetration)
float dist = len - (radiusA + radiusB);
Vector3 normalOnSurfaceB = diff / len;
//point on A (worldspace)
Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
//point on B (worldspace)
Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
if (triBody.CollisionShape.IsConcave)
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
if (convexBody.CollisionShape.IsConvex)
{
float collisionMarginTriangle = concaveShape.Margin;
resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
_convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
}
}
}
public override void ProcessCollision(
CollisionObject bodyA,
CollisionObject bodyB,
DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Begin
CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
//Debug.Assert(collisionObject.getCollisionShape().isCompound());
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
int childrenNumber = _childCollisionAlgorithms.Count;
for (int i = 0; i < childrenNumber; i++)
{
CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
}
//by default, Bullet will use this near callback
public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;
CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;
if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.CollisionAlgorithm == null)
{
collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);
}
if (collisionPair.CollisionAlgorithm != null)
{
ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);
if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)
{
//discrete collision detection query
collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
if (dispatchInfo.TimeOfImpact > timeOfImpact)
{
dispatchInfo.TimeOfImpact = timeOfImpact;
}
}
}
}
}
public override void ProcessCollision(
CollisionObject bodyA,
CollisionObject bodyB,
DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Begin
CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
//Debug.Assert(collisionObject.getCollisionShape().isCompound());
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
int childrenNumber = _childCollisionAlgorithms.Count;
for (int i = 0; i < childrenNumber; i++)
{
CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
//swapped?
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
resultOut.SetPersistentManifold(_manifold);
ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//TODO: if (dispatchInfo.m_useContinuous)
_gjkPairDetector.setMinkowskiA(min0);
_gjkPairDetector.setMinkowskiB(min1);
input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
// input.m_maximumDistanceSquared = 1e30f;
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
_gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
if (squareMot0 < convexbody.CcdSquareMotionThreshold)
{
return 1;
}
Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
Matrix convexFromLocal = triInv * convexbody.WorldTransform;
Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
if (triBody.CollisionShape.IsConcave)
{
Vector3 rayAabbMin = convexFromLocal.Translation;
MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
Vector3 rayAabbMax = convexFromLocal.Translation;
MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
float ccdRadius0 = convexbody.CcdSweptSphereRadius;
rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
float curHitFraction = 1f; //is this available?
LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
convexbody.CcdSweptSphereRadius, curHitFraction);
raycastCallback.HitFraction = convexbody.HitFraction;
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
}
if (raycastCallback.HitFraction < convexbody.HitFraction)
{
convexbody.HitFraction = raycastCallback.HitFraction;
return raycastCallback.HitFraction;
}
}
return 1;
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
_dispatchInfo = dispatchInfo;
_collisionMarginTriangle = collisionMarginTriangle;
_resultOut = resultOut;
//recalc aabbs
Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;
CollisionShape convexShape = _convexBody.CollisionShape;
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);
float extraMargin = collisionMarginTriangle;
Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);
_aabbMax += extra;
_aabbMin -= extra;
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
_dispatchInfo = dispatchInfo;
_collisionMarginTriangle = collisionMarginTriangle;
_resultOut = resultOut;
//recalc aabbs
Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;
CollisionShape convexShape = _convexBody.CollisionShape;
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);
float extraMargin = collisionMarginTriangle;
Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);
_aabbMax += extra;
_aabbMin -= extra;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
SphereShape sphere = bodyA.CollisionShape as SphereShape;
TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
detector.GetClosestPoints(input, resultOut, null);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
float hitFraction = 1.0f;
for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
{
CollisionShape childShape = compoundShape.GetChildShape(i);
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
collisionObject, otherObject, dispatchInfo, resultOut
);
if (frac < hitFraction)
{
hitFraction = frac;
}
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
return hitFraction;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
return;
}
CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
Vector3 pOnBox, pOnSphere;
Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
float radius = sphereA.Radius;
float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
if (dist < MathHelper.Epsilon)
{
Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
}
}
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;
}
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);
}
public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
float hitFraction = 1.0f;
for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
{
CollisionShape childShape = compoundShape.GetChildShape(i);
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
collisionObject, otherObject, dispatchInfo, resultOut
);
if (frac < hitFraction)
{
hitFraction = frac;
}
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
return(hitFraction);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
return(1f);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
if (triBody.CollisionShape.IsConcave)
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
if (convexBody.CollisionShape.IsConvex)
{
float collisionMarginTriangle = concaveShape.Margin;
resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
_convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
}
}
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
return 1f;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
//swapped?
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
resultOut.SetPersistentManifold(_manifold);
ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//TODO: if (dispatchInfo.m_useContinuous)
_gjkPairDetector.setMinkowskiA(min0);
_gjkPairDetector.setMinkowskiB(min1);
input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
// input.m_maximumDistanceSquared = 1e30f;
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
_gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
if (squareMot0 < convexbody.CcdSquareMotionThreshold)
{
return(1);
}
Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
Matrix convexFromLocal = triInv * convexbody.WorldTransform;
Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
if (triBody.CollisionShape.IsConcave)
{
Vector3 rayAabbMin = convexFromLocal.Translation;
MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
Vector3 rayAabbMax = convexFromLocal.Translation;
MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
float ccdRadius0 = convexbody.CcdSweptSphereRadius;
rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
float curHitFraction = 1f; //is this available?
LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
convexbody.CcdSweptSphereRadius, curHitFraction);
raycastCallback.HitFraction = convexbody.HitFraction;
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
}
if (raycastCallback.HitFraction < convexbody.HitFraction)
{
convexbody.HitFraction = raycastCallback.HitFraction;
return(raycastCallback.HitFraction);
}
}
return(1);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
Vector3 pOnBox, pOnSphere;
Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
float radius = sphereA.Radius;
float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
if (dist < MathHelper.Epsilon)
{
Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
}
}
public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
return;
}
SphereShape sphereA = bodyA.CollisionShape as SphereShape;
SphereShape sphereB = bodyB.CollisionShape as SphereShape;
Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
float len = diff.Length();
float radiusA = sphereA.Radius;
float radiusB = sphereB.Radius;
//if distance positive, don't generate a new contact
if (len > (radiusA + radiusB))
{
return;
}
//distance (negative means penetration)
float dist = len - (radiusA + radiusB);
Vector3 normalOnSurfaceB = diff / len;
//point on A (worldspace)
Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
//point on B (worldspace)
Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
}
public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//not yet
return 1;
}
public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//not yet
return(1);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
return;
}
SphereShape sphere = bodyA.CollisionShape as SphereShape;
TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.MaximumDistanceSquared = 1e30f; //todo: tighter bounds
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
detector.GetClosestPoints(input, resultOut, null);
}
//by default, Bullet will use this near callback
public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;
CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;
if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.CollisionAlgorithm == null)
{
collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);
}
if (collisionPair.CollisionAlgorithm != null)
{
ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);
if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)
{
//discrete collision detection query
collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
if (dispatchInfo.TimeOfImpact > timeOfImpact)
dispatchInfo.TimeOfImpact = timeOfImpact;
}
}
}
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
}
