bool PhysicsCollisionObject::collidesWith(PhysicsCollisionObject* object) const
{
GP_ASSERT(Game::getInstance()->getPhysicsController() && Game::getInstance()->getPhysicsController()->_world);
GP_ASSERT(object && object->getCollisionObject());
GP_ASSERT(getCollisionObject());
static CollidesWithCallback callback;
callback.result = false;
Game::getInstance()->getPhysicsController()->_world->contactPairTest(getCollisionObject(), object->getCollisionObject(), callback);
return callback.result;
}
bool PhysicsCollisionObject::isStatic() const
{
switch (getType())
{
case GHOST_OBJECT:
case CHARACTER:
return false;
default:
GP_ASSERT(getCollisionObject());
return getCollisionObject()->isStaticObject();
}
}
glm::vec3 PlayerPhysicsComponent::getVelocity() const {
btRigidBody *rigidBody = dynamic_cast<btRigidBody*>(getCollisionObject());
if (!rigidBody) {
return glm::vec3(0.0f);
}
return toGlm(rigidBody->getLinearVelocity());
}
void RigidCompoundObject::removeAllChildren() {
btCompoundShape *shape = static_cast<btCompoundShape*>(getCollisionObject()->getCollisionShape());
while(shape->getNumChildShapes()) shape->removeChildShapeByIndex(0);
shape->recalculateLocalAabb();
btVector3 inertia(0,0,0);
float mass = getMass();
shape->calculateLocalInertia(mass, inertia);
getRigidBody()->setMassProps(mass,inertia);
SceneObject::removeAllChildren();
}
void RigidCompoundObject::removeChild(RigidObject* obj) {
btCompoundShape *shape = static_cast<btCompoundShape*>(getCollisionObject()->getCollisionShape());
shape->removeChildShape(obj->getCollisionObject()->getCollisionShape());
shape->recalculateLocalAabb();
btVector3 inertia(0,0,0);
float mass = getMass();
shape->calculateLocalInertia(mass, inertia);
getRigidBody()->setMassProps(mass,inertia);
SceneObject::removeChild(obj);
}
void RigidCompoundObject::addChild(RigidObject* obj, bool passOwnership) {
SceneObject::addChild(obj,passOwnership);
btCompoundShape *shape = static_cast<btCompoundShape*>(getCollisionObject()->getCollisionShape());
shape->addChildShape(icl2bullet_scaled_mat(obj->getTransformation()),obj->getCollisionObject()->getCollisionShape());
shape->recalculateLocalAabb();
btVector3 inertia(0,0,0);
float mass = getMass();
shape->calculateLocalInertia(mass, inertia);
getRigidBody()->setMassProps(mass,inertia);
}
bool SBCollisionManager::removeObjectFromCollisionSpace( const std::string& geomName )
{
SBGeomObject* geomObj = getCollisionObject(geomName);
if (geomObj)
{
if (collisionSpace)
collisionSpace->removeCollisionObjects(geomName);
return true;
}
return false;
}
bool SBCollisionManager::addObjectToCollisionSpace( const std::string& geomName )
{
SBGeomObject* geomObj = getCollisionObject(geomName);
if (geomObj)
{
collisionSpace->addCollisionObjects(geomName);
return true;
}
return false;
}
bool SBCollisionManager::removeCollisionObject( const std::string& geomName )
{
SBGeomObject* geomObj = getCollisionObject(geomName);
if (geomObj)
{
removeObjectFromCollisionSpace(geomName);
geomObjectMap.erase(geomName);
delete geomObj;
return true;
}
return false;
}
bool PhysicsCollisionObject::isDynamic() const
{
GP_ASSERT(getCollisionObject());
return !getCollisionObject()->isStaticOrKinematicObject();
}
void SBCollisionManager::afterUpdate(double time)
{
// determine if any of the characters are currently in collision
// horribly inefficient n^2 implementation for characters only
SBScene* scene = SmartBody::SBScene::getScene();
double timeDt = scene->getSimulationManager()->getTimeDt();
if (timeDt == 0.0)
timeDt = .016;
const std::vector<std::string>& characters = scene->getCharacterNames();
for (std::vector<std::string>::const_iterator iter = characters.begin();
iter != characters.end();
iter++)
{
SBCharacter* character = scene->getCharacter((*iter));
SrVec position = character->getPosition();
position[1] = 0.0;
SrVec& oldPosition = _positions[(*iter)];
_velocities[(*iter)] = (position - oldPosition) / (float) timeDt;
_positions[(*iter)] = position;
}
int curIteration = 0;
bool needMoreIterations = true;
SBEventManager* eventManager = SmartBody::SBScene::getScene()->getEventManager();
const std::string& collisionResMethod = getStringAttribute("collisionResolutionType");
while (needMoreIterations && curIteration < _maxIterations)
{
needMoreIterations = false;
SbmCollisionPairList potentialCollisions;
collisionSpace->getPotentialCollisionPairs(potentialCollisions);
for (unsigned int i=0;i<potentialCollisions.size();i++)
{
// unfiltered
//LOG("Collision Pair = %s %s",potentialCollisions[i].first.c_str(), potentialCollisions[i].second.c_str());
SBGeomObject* g1 = getCollisionObject(potentialCollisions[i].first);
SBGeomObject* g2 = getCollisionObject(potentialCollisions[i].second);
// skip self-collision within skeleton/character's joints
if(!_singleChrCapsuleMode)
{
SBJoint* j1 = dynamic_cast<SBJoint*>(g1->getAttachObj());
SBJoint* j2 = dynamic_cast<SBJoint*>(g2->getAttachObj());
if(j1 && j2)
if(j1->skeleton() == j2->skeleton()) continue; // belong to same character
}
// filtered, exclude all collisions within each character/skeleton
//LOG("Potential Collision Pair: %s %s",potentialCollisions[i].first.c_str(), potentialCollisions[i].second.c_str());
std::vector<SBGeomContact> contactPts;
SBCollisionUtil::collisionDetection(g1,g2,contactPts);
if (contactPts.size() > 0)
{
// collision handling here
if(_singleChrCapsuleMode)
{
SBCharacter* c1 = dynamic_cast<SBCharacter*>(g1->getAttachObj());
SBCharacter* c2 = dynamic_cast<SBCharacter*>(g2->getAttachObj());
if (c1 && c2)
{
SBEvent* collisionEvent = eventManager->createEvent("collision",c1->getName()+"/"+c2->getName());
eventManager->handleEvent(collisionEvent, time);
//LOG("Collision detected between character %s and character %s",c1->getName().c_str(), c2->getName().c_str());
delete collisionEvent; // free the memory
// collision resolution
if (collisionResMethod == "default")
{
SrVec v1 = _velocities[c1->getName()];
v1[1] = 0.0;
SrVec v2 = _velocities[c2->getName()];
v2[1] = 0.0;
//LOG("v1 len = %f, v2 len = %f",v1.len(),v2.len());
SBCharacter* cMove = (v1.len() > v2.len()) ? c1 : c2;
SBGeomContact& contact = contactPts[0];
SrVec normalDir = (v1.len() > v2.len()) ? contact.contactNormal : -contact.contactNormal;
normalDir[1] = 0.0;
SrVec newPos = cMove->getPosition() + normalDir*contact.penetrationDepth;
cMove->setPosition(newPos);
}
else if (collisionResMethod == "none")
{
// no collision resolution
}
else
{
// default ?
}
}
needMoreIterations = true;
}
else
{
// character joint <==> character joint (capsule) collision only
//bool createEvent = true;
//std::string obj1, obj2;
//SBJoint* j1 = dynamic_cast<SBJoint*>(g1->getAttachObj());
//if(j1) obj1 = j1->getName();
//else
//{
// SBCharacter* c1 = dynamic_cast<SBCharacter*>(g1->getAttachObj());
// if(c1) obj1 = c1->getName();
// else
// createEvent = false;
//}
//SBJoint* j2 = dynamic_cast<SBJoint*>(g2->getAttachObj());
//if(j2) obj2 = j2->getName();
//else
//{
// SBCharacter* c2 = dynamic_cast<SBCharacter*>(g2->getAttachObj());
// if(c2) obj2 = c2->getName();
// else
// createEvent = false;
//}
//if(createEvent)
//{
// Event* collisionEvent = eventManager->createEvent("collision",obj1+"/"+obj2);
// eventManager->handleEvent(collisionEvent, time);
// LOG("Collision detected between %s and %s", obj1.c_str(), obj2.c_str());
// delete collisionEvent; // free the memory
//}
const std::string& obj1 = potentialCollisions[i].first;
const std::string& obj2 = potentialCollisions[i].second;
//LOG("Collision detected between %s and %s", obj1.c_str(), obj2.c_str());
SBEvent* collisionEvent = eventManager->createEvent("collision",obj1+"/"+obj2);
eventManager->handleEvent(collisionEvent, time);
delete collisionEvent; // free the memory
}
}
}
curIteration++;
}
#if 0
for (std::vector<std::string>::iterator iter = characters.begin();
iter != characters.end();
iter++)
{
SBCharacter* character1 = scene->getCharacter((*iter));
SrVec position1 = character1->getPosition();
position1[1] = 0.0;
for (std::vector<std::string>::iterator iter2 = iter + 1;
iter2 != characters.end();
iter2++)
{
SBCharacter* character2 = scene->getCharacter((*iter2));
// determine if the two characters are in collision
std::vector<SBGeomContact> contactPts;
SBCollisionUtil::collisionDetection(character1->getGeomObject(),character2->getGeomObject(),contactPts);
if (contactPts.size() > 0)
{
//LOG("Collision between character %s and character %s",character1->getName().c_str(), character2->getName().c_str());
}
SrVec position2 = character2->getPosition();
position2[1] = 0.0;
float distance = dist(position1, position2);
if (distance < _characterRadius * 2)
{
float penetration = _characterRadius * 2 - distance;
needMoreIterations = true;
curIteration++;
// collision resolution
// who is moving faster?
SrVec& velocity1 = _velocities[(*iter)];
SrVec& velocity2 = _velocities[(*iter2)];
float magnitude1 = velocity1.len();
float magnitude2 = velocity2.len();
// move the object with greater velocity
if (magnitude1 > magnitude2)
{
SrVec diff = position1 - position2;
diff.normalize();
// move character1 in that direction
position1 += diff * penetration;
_positions[(*iter)] = position1;
SrVec curPosition = character1->getPosition();
curPosition.x = position1[0];
curPosition.z = position1[2];
character1->setPosition(curPosition);
}
else
{
SrVec diff = position2 - position1;
diff.normalize();
// move character1 in that direction
position2 += diff * penetration;
_positions[(*iter2)] = position2;
SrVec curPosition = character2->getPosition();
curPosition.x = position2[0];
curPosition.z = position2[2];
character2->setPosition(curPosition);
}
}
}
}
#endif
}
