bool Ellipsoid::SweepAgainst( const CollisionMesh& m, const Vector& v, const AABB& SweepAABB, CollisionInfo* const pInfo /*= NULL*/ ) const
{
CollisionInfo Info;
CollisionInfo MinInfo;
if( pInfo )
{
Info.CopyInParametersFrom( *pInfo );
}
for( uint i = 0; i < m.m_NumTris; ++i )
{
if( SweepAgainst( m.m_Tris[i], v, SweepAABB, &Info ) )
{
if( Info.m_HitT < MinInfo.m_HitT || !MinInfo.m_Collision )
{
MinInfo = Info;
}
}
}
if( MinInfo.m_Collision )
{
if( pInfo )
{
pInfo->CopyOutParametersFrom( MinInfo );
}
return true;
}
return false;
}
void GameProjectile::update( float dt )
{
GameObject::update( dt );
if ( !m_hasHit )
{
Vector3 vel = velocity();
vel += Vector3( 0, -9.81f, 0 ) * dt * m_gravity;
setVelocity( vel );
m_movedDistance += velocity().length() * dt;
CollisionInfo cinfo;
move( velocity() * dt, &cinfo );
if ( cinfo.isCollision(CollisionInfo::COLLIDE_ALL) )
hit( &cinfo );
if ( m_spinSpeed > 0.f )
setRotation( rotation() * Matrix3x3( m_spinAxis, m_spinSpeed * dt) );
}
m_age += dt;
if ( m_age > m_ageLimit )
m_removable = true;
if ( m_movedDistance > m_maxRange )
m_removable = true;
}
bool CollisionCallbackManager::intersectAndReflectWithTerrain(Object * const object, Result & result)
{
CollisionProperty const * collision = object->getCollisionProperty();
NOT_NULL(collision);
Capsule queryCapsule_w(collision->getQueryCapsule_w());
float const radius = queryCapsule_w.getRadius();
Vector const deltaTraveled_w(queryCapsule_w.getDelta());
Vector direction_w(deltaTraveled_w);
if (direction_w.normalize())
{
Vector const begin_w(queryCapsule_w.getPointA());
Vector const end_w(queryCapsule_w.getPointB() + direction_w * radius);
DEBUG_REPORT_LOG(ms_debugReport, ("terrain begin = %f %f %f\n", begin_w.x, begin_w.y, begin_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain end = %f %f %f\n", end_w.x, end_w.y, end_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain direction = %f %f %f\n", direction_w.x, direction_w.y, direction_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain length = %f\n", deltaTraveled_w.magnitude()));
TerrainObject const * const terrainObject = TerrainObject::getConstInstance();
if (terrainObject != 0)
{
CollisionInfo info;
if (terrainObject->collide (begin_w, end_w, info))
{
#ifdef _DEBUG
// calculate the parametric time for logging
float const actualDistance = begin_w.magnitudeBetween(info.getPoint());
float const attemptedDistance = begin_w.magnitudeBetween(end_w);
float const parametricTime = (attemptedDistance != 0.0f) ? (actualDistance / attemptedDistance) : 0.0f;
#endif
Vector const & pointOfCollision_w = info.getPoint();
#ifdef _DEBUG
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain HIT!\n"));
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain time = %f\n", parametricTime));
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain point = %f %f %f\n", pointOfCollision_w.x, pointOfCollision_w.y, pointOfCollision_w.z));
#endif
result.m_pointOfCollision_p = pointOfCollision_w;
result.m_normalOfSurface_p = info.getNormal();
result.m_deltaToMoveBack_p = pointOfCollision_w - end_w;
result.m_newReflection_p = result.m_normalOfSurface_p.reflectIncoming(direction_w);
#ifdef _DEBUG
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain = %f %f %f\n", result.m_deltaToMoveBack_p.x, result.m_deltaToMoveBack_p.y, result.m_deltaToMoveBack_p.z));
#endif
return true;
}
}
}
return false;
}
void GameWeapon::fire( GameCharacter* character, const Vector3& direction, bool applyRecoilError )
{
assert( m_mesh );
if ( m_shells > 0 )
{
// find bullet launch position
Vector3 launchPos = launchPosition();
// check that we are not too close any obstacle
GamePointObject* pointCollisionChecker = character->visibilityCollisionChecker();
pointCollisionChecker->setPosition( character->cell(), character->position() );
Vector3 startpoint = character->headWorldPosition();
pointCollisionChecker->moveWithoutColliding( startpoint - pointCollisionChecker->position() );
CollisionInfo cinfo;
pointCollisionChecker->move( launchPos - pointCollisionChecker->position(), &cinfo );
bool removeInFirstUpdate = cinfo.isCollision(CollisionInfo::COLLIDE_ALL);
fireVisuals( character );
// Apply recoil error
Vector3 launchVector = direction;
if ( applyRecoilError )
launchVector = Vector3( perturbVector( direction, m_accumulatedRecoilError, character->up(), character->right() ) );
// Create projectile(s)
float limitangle = m_spreadConeLimitAngle;
for ( int i = 0; i < m_shotsPerLaunch; ++i )
{
GameProjectile* projectile = m_projectileMgr->createProjectile( m_bullet, cell(), this, launchPos, perturbVector( launchVector, limitangle, character->up(), character->right() ) );
if ( removeInFirstUpdate )
projectile->removeInNextUpdate();
}
// Accumulate recoil error
m_accumulatedRecoilError += m_recoilErrorPerShot;
if ( m_accumulatedRecoilError > m_maxRecoilError )
m_accumulatedRecoilError = m_maxRecoilError;
// Decrease shells
m_shells--;
}
else
{
// Call script function for empty clip
pushMethod( "signalEmpty" );
vm()->pushTable( character );
call(1, 0);
m_clipEmpty = true;
}
// reset timer for weapon becoming ready for next shot
m_fireDeltaTime = 0.f;
}
bool GameDynamicObject::isVisiblePoint( const Vector3& worldPos, const Vector3& point ) const
{
// check if character is occluded by level geometry
m_visibilityChecker->setPosition( cell(), position() );
m_visibilityChecker->moveWithoutColliding( point-m_visibilityChecker->position() );
CollisionInfo cinfo;
m_visibilityChecker->move( worldPos-m_visibilityChecker->position(), &cinfo );
return !cinfo.isCollision( CollisionInfo::COLLIDE_ALL );
}
void PhysicsSimulation::updateContacts() {
PerformanceTimer perfTimer("contacts");
int numCollisions = _collisions.size();
for (int i = 0; i < numCollisions; ++i) {
CollisionInfo* collision = _collisions.getCollision(i);
quint64 key = collision->getShapePairKey();
if (key == 0) {
continue;
}
QMap<quint64, ContactPoint>::iterator itr = _contacts.find(key);
if (itr == _contacts.end()) {
_contacts.insert(key, ContactPoint(*collision, _frameCount));
} else {
itr.value().updateContact(*collision, _frameCount);
}
}
}
void PhysicsSimulation::resolveCollisions() {
PerformanceTimer perfTimer("resolve");
// walk all collisions, accumulate movement on shapes, and build a list of affected shapes
QSet<Shape*> shapes;
int numCollisions = _collisions.size();
for (int i = 0; i < numCollisions; ++i) {
CollisionInfo* collision = _collisions.getCollision(i);
collision->apply();
// there is always a shapeA
shapes.insert(collision->getShapeA());
// but need to check for valid shapeB
if (collision->_shapeB) {
shapes.insert(collision->getShapeB());
}
}
// walk all affected shapes and apply accumulated movement
QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
while (shapeItr != shapes.constEnd()) {
(*shapeItr)->applyAccumulatedDelta();
++shapeItr;
}
}
ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) :
_lastFrame(frame), _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()),
_offsetA(0.0f), _offsetB(0.0f),
_relativeMassA(0.5f), _relativeMassB(0.5f),
_numPointsA(0), _numPoints(0), _normal(0.0f) {
glm::vec3 pointA = collision._contactPoint;
glm::vec3 pointB = collision._contactPoint - collision._penetration;
float pLength = glm::length(collision._penetration);
if (pLength > EPSILON) {
_normal = collision._penetration / pLength;
}
if (_shapeA->getID() > _shapeB->getID()) {
// swap so that _shapeA always has lower ID
_shapeA = collision.getShapeB();
_shapeB = collision.getShapeA();
_normal = - _normal;
pointA = pointB;
pointB = collision._contactPoint;
}
// bring the contact points inside the shapes to help maintain collision updates
pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
_offsetA = pointA - _shapeA->getTranslation();
_offsetB = pointB - _shapeB->getTranslation();
_shapeA->getVerletPoints(_points);
_numPointsA = _points.size();
_shapeB->getVerletPoints(_points);
_numPoints = _points.size();
// compute and cache relative masses
float massA = EPSILON;
for (int i = 0; i < _numPointsA; ++i) {
massA += _points[i]->getMass();
}
float massB = EPSILON;
for (int i = _numPointsA; i < _numPoints; ++i) {
massB += _points[i]->getMass();
}
float invTotalMass = 1.0f / (massA + massB);
_relativeMassA = massA * invTotalMass;
_relativeMassB = massB * invTotalMass;
// _contactPoint will be the weighted average of the two
_contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
// compute offsets for shapeA
for (int i = 0; i < _numPointsA; ++i) {
glm::vec3 offset = _points[i]->_position - pointA;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
// compute offsets for shapeB
for (int i = _numPointsA; i < _numPoints; ++i) {
glm::vec3 offset = _points[i]->_position - pointB;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
}
/** Gère la collision entre toutes les boites
**/
void EngineBox::interCollision() {
for(unsigned int i=0; i<_boxList->size(); i++) {
Box *b1=(*_boxList)[i];
for(unsigned int j=i+1; j<_boxList->size(); j++) {
Box *b2=(*_boxList)[j];
CollisionInfo collisionInfo;
bool collide=Box::detectCollision(b1,b2,&collisionInfo);
if (collide) {
b1->enableVisualEffect(1);
b2->enableVisualEffect(1); // sert uniquement à faire un retour visuel de collision.
// début Réponse à la collision :
// décomposition du calcul : normal, vP1_old : vitesse du point de contact par rapport à b1, r1xN : produit vectoriel PG x normal, etc
Vector3 normal=collisionInfo.axis();
normal.normalize();
Vector3 r1=(b1->position()-collisionInfo.applicationPoint());
Vector3 vP1_old=b1->velocity()+r1.cross(b1->omega());
Vector3 r2=(b2->position()-collisionInfo.applicationPoint());
Vector3 vP2_old=b2->velocity()+r2.cross(b2->omega());
Vector3 r1xN=-r1.cross(normal);
Vector3 r2xN=-r2.cross(normal);
// calcul de l'impulsion.
double impulseNum;
double impulseDen;
impulseNum=-(1+0.5)*(normal.dot(b1->velocity()-b2->velocity())+b1->omega().dot(r1xN)-b2->omega().dot(r2xN));
impulseDen=1.0/b1->mass()+1.0/b2->mass()+1.0/b1->inertia()*r1xN.dot(r1xN)+1.0/b2->inertia()*r2xN.dot(r2xN);
double impulse=impulseNum/impulseDen;
// calcul de la force d'impulsion
Vector3 force=impulse*normal;
// correction du mouvement :
// - on corrige la vitesse angulaire et la vitesse des boites
// - on corrige la position (car on répond à une position en recouvrement).
b1->addVelocityCorrec(force/b1->mass());
b1->addOmegaCorrec(force.cross(r1)/b1->inertia());
b2->addVelocityCorrec(-force/b2->mass());
b2->addOmegaCorrec(-force.cross(r2)/b2->inertia());
b1->addPositionCorrec(collisionInfo.axis()*collisionInfo.mtd());
b2->addPositionCorrec(-collisionInfo.axis()*collisionInfo.mtd());
// fin réponse à la collision
}
else {
b1->disableVisualEffect(1); // pas d'effet visuel de détection de collision
b2->disableVisualEffect(1);
}
}
}
}
msr::airlib::MultirotorState to() const
{
return msr::airlib::MultirotorState(collision.to(), kinematics_estimated.to(),
gps_location.to(), timestamp, landed_state, rc_data.to());
}