pxUInt FCollide_Sphere_Sphere( pxCollideable* objA, pxCollideable* objB, pxCollisionResult &result )
{
//PX_ASSERT_PTR2(objA,objB);
//Assert( objA->GetShape()->GetType() == pxcShapeType::PX_SHAPE_SPHERE );
//Assert( objB->GetShape()->GetType() == pxcShapeType::PX_SHAPE_SPHERE );
pxShape_Sphere * sphereA = (pxShape_Sphere*)objA->GetShape();
pxShape_Sphere * sphereB = (pxShape_Sphere*)objB->GetShape();
const pxReal margin = SMALL_NUMBER;//PX_COLLISION_MARGIN;
// sphere centers in world space
const pxVec3 posA = objA->GetOrigin();
const pxVec3 posB = objB->GetOrigin();
const pxReal radiusA = sphereA->GetRadius() + margin;
const pxReal radiusB = sphereB->GetRadius() + margin;
const pxVec3 axis = posA - posB;
const pxReal distanceSq = axis.LengthSqr();
//iff len positive, don't generate a new contact
if( distanceSq > squaref(radiusA + radiusB) )
{
return 0;
}
const pxReal len = mxSqrt(distanceSq);
//distance (negative means penetration)
const pxReal distance = len - (sphereA->mRadius + sphereB->mRadius);
// contact normalOnSurfaceB
pxVec3 normalOnSurfaceB(0.0f,1.0f,0.0f);
if( len > PX_EPSILON ) {
normalOnSurfaceB = axis / len;
}
//point on B (in world space)
const pxVec3 pB = posB + radiusB * normalOnSurfaceB;
pxContactManifold & manifold = result.manifold;
manifold.points[0].position = pB;
manifold.points[0].normal = normalOnSurfaceB;
manifold.points[0].depth = distance;
manifold.points[0].material.Set( objA->GetSurfaceInfo(), objB->GetSurfaceInfo() );
manifold.numPoints = 1;
result.oA = objA;
result.oB = objB;
return 1;
}
void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)dispatchInfo;
if (!m_manifoldPtr)
return;
resultOut->setPersistentManifold(m_manifoldPtr);
btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin();
btScalar len = diff.length();
btScalar radius0 = sphere0->getRadius();
btScalar radius1 = sphere1->getRadius();
#ifdef CLEAR_MANIFOLD
m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
{
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
btScalar dist = len - (radius0+radius1);
btVector3 normalOnSurfaceB(1,0,0);
if (len > SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD
}
void pxSphereSphereAgent::ProcessCollision(
pxCollideable* objA, pxCollideable* objB,
const pxProcessCollisionInput& input,
pxProcessCollisionOutput & result
)
{
pxShape_Sphere * shapeA = (pxShape_Sphere*)objA->GetShape();
pxShape_Sphere * shapeB = (pxShape_Sphere*)objB->GetShape();
const pxReal margin = PX_COLLISION_MARGIN;
// sphere centers in world space
const pxVec3 posA = objA->GetOrigin();
const pxVec3 posB = objB->GetOrigin();
const pxReal radiusA = shapeA->GetRadius() + margin;
const pxReal radiusB = shapeB->GetRadius() + margin;
const pxVec3 axis = posA - posB;
const pxReal distanceSq = axis.LengthSqr();
//iff len positive, don't generate a new contact
if( distanceSq > squaref(radiusA + radiusB) )
{
return;
}
const pxReal len = mxSqrt(distanceSq);
//distance (negative means penetration)
const pxReal distance = len - (shapeA->GetRadius() + shapeB->GetRadius());
// contact normalOnSurfaceB
pxVec3 normalOnSurfaceB(0.0f,1.0f,0.0f);
if( len > PX_EPSILON ) {
normalOnSurfaceB = axis / len;
}
//point on B (in world space)
const pxVec3 pB = posB + radiusB * normalOnSurfaceB;
manifold->points[0].position = pB;
manifold->points[0].normalAndDepth.mVec128 = normalOnSurfaceB.mVec128;
manifold->points[0].normalAndDepth.w = distance;
manifold->numPoints = 1;
manifold->oA = objA;
manifold->oB = objB;
}
