void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
{
//bool removeFromBroadphase = false;
{
BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
if (bp)
{
//
// only clear the cached algorithms
//
GetBroadphase()->CleanProxyFromPairs(bp);
GetBroadphase()->DestroyProxy(bp);
collisionObject->m_broadphaseHandle = 0;
}
}
std::vector<CollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
if (!(i == m_collisionObjects.end()))
{
std::swap(*i, m_collisionObjects.back());
m_collisionObjects.pop_back();
}
}
void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
{
m_collisionObjects.push_back(collisionObject);
//calculate new AABB
SimdTransform trans = collisionObject->m_worldTransform;
SimdVector3 minAabb;
SimdVector3 maxAabb;
collisionObject->m_collisionShape->GetAabb(trans,minAabb,maxAabb);
int type = collisionObject->m_collisionShape->GetShapeType();
collisionObject->m_broadphaseHandle = GetBroadphase()->CreateProxy(
minAabb,
maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask
);
}
CollisionWorld::~CollisionWorld()
{
//clean up remaining objects
std::vector<CollisionObject*>::iterator i;
for (i=m_collisionObjects.begin();
!(i==m_collisionObjects.end()); i++)
{
CollisionObject* collisionObject= (*i);
BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
if (bp)
{
//
// only clear the cached algorithms
//
GetBroadphase()->CleanProxyFromPairs(bp);
GetBroadphase()->DestroyProxy(bp);
}
}
}
void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
{
RigidBody* body = ctrl->GetRigidBody();
//this m_userPointer is just used for triggers, see CallbackTriggers
body->m_userPointer = ctrl;
body->setGravity( m_gravity );
m_controllers.push_back(ctrl);
m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
assert(body->m_broadphaseHandle);
BroadphaseInterface* scene = GetBroadphase();
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
assert(shapeinterface);
const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
SimdPoint3 minAabb,maxAabb;
shapeinterface->GetAabb(t,minAabb,maxAabb);
float timeStep = 0.02f;
//extent it with the motion
SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
float maxAabbx = maxAabb.getX();
float maxAabby = maxAabb.getY();
float maxAabbz = maxAabb.getZ();
float minAabbx = minAabb.getX();
float minAabby = minAabb.getY();
float minAabbz = minAabb.getZ();
if (linMotion.x() > 0.f)
maxAabbx += linMotion.x();
else
minAabbx += linMotion.x();
if (linMotion.y() > 0.f)
maxAabby += linMotion.y();
else
minAabby += linMotion.y();
if (linMotion.z() > 0.f)
maxAabbz += linMotion.z();
else
minAabbz += linMotion.z();
minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
}
void CcdPhysicsEnvironment::UpdateAabbs(float timeStep)
{
std::vector<CcdPhysicsController*>::iterator i;
BroadphaseInterface* scene = GetBroadphase();
//
// update aabbs, only for moving objects (!)
//
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
SimdPoint3 minAabb,maxAabb;
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
body->getLinearVelocity(),
//body->getAngularVelocity(),
SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
timeStep,minAabb,maxAabb);
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
minAabb -= manifoldExtraExtents;
maxAabb += manifoldExtraExtents;
BroadphaseProxy* bp = body->m_broadphaseHandle;
if (bp)
{
SimdVector3 color (1,1,0);
if (m_debugDrawer)
{
//draw aabb
switch (body->GetActivationState())
{
case ISLAND_SLEEPING:
{
color.setValue(1,1,1);
break;
}
case WANTS_DEACTIVATION:
{
color.setValue(0,0,1);
break;
}
case ACTIVE_TAG:
{
break;
}
case DISABLE_DEACTIVATION:
{
color.setValue(1,0,1);
};
};
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}
}
if ( (maxAabb-minAabb).length2() < 1e12f)
{
scene->SetAabb(bp,minAabb,maxAabb);
} else
{
//something went wrong, investigate
//removeCcdPhysicsController(ctrl);
body->SetActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe)
{
reportMe = false;
printf("Overflow in AABB, object removed from simulation \n");
printf("If you can reproduce this, please email [email protected]\n");
printf("Please include above information, your Platform, version of OS.\n");
printf("Thanks.\n");
}
}
}
}
}
