btPlayer.cpp
Engine/source/T3D/physics/bullet/btPlayer.cpp
Classes:
Detailed Description
1 2//----------------------------------------------------------------------------- 3// Copyright (c) 2012 GarageGames, LLC 4// 5// Permission is hereby granted, free of charge, to any person obtaining a copy 6// of this software and associated documentation files (the "Software"), to 7// deal in the Software without restriction, including without limitation the 8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9// sell copies of the Software, and to permit persons to whom the Software is 10// furnished to do so, subject to the following conditions: 11// 12// The above copyright notice and this permission notice shall be included in 13// all copies or substantial portions of the Software. 14// 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21// IN THE SOFTWARE. 22//----------------------------------------------------------------------------- 23 24#include "platform/platform.h" 25#include "T3D/physics/bullet/btPlayer.h" 26 27#include "T3D/physics/physicsPlugin.h" 28#include "T3D/physics/bullet/btWorld.h" 29#include "T3D/physics/bullet/btCasts.h" 30#include "collision/collision.h" 31 32BtPlayer::BtPlayer() 33 : PhysicsPlayer(), 34 mWorld( NULL ), 35 mObject( NULL ), 36 mGhostObject( NULL ), 37 mColShape( NULL ), 38 mOriginOffset( 0.0f ) 39{ 40} 41 42BtPlayer::~BtPlayer() 43{ 44 _releaseController(); 45} 46 47void BtPlayer::_releaseController() 48{ 49 if ( !mGhostObject ) 50 return; 51 52 mWorld->getDynamicsWorld()->removeCollisionObject( mGhostObject ); 53 54 SAFE_DELETE( mGhostObject ); 55 SAFE_DELETE( mColShape ); 56} 57 58void BtPlayer::init( const char *type, 59 const Point3F &size, 60 F32 runSurfaceCos, 61 F32 stepHeight, 62 SceneObject *obj, 63 PhysicsWorld *world ) 64{ 65 AssertFatal( obj, "BtPlayer::init - Got a null scene object!" ); 66 AssertFatal( world, "BtPlayer::init - Got a null world!" ); 67 AssertFatal( dynamic_cast<BtWorld*>( world ), "BtPlayer::init - The world is the wrong type!" ); 68 69 // Cleanup any previous controller. 70 _releaseController(); 71 72 mObject = obj; 73 mWorld = (BtWorld*)world; 74 75 mSlopeAngle = runSurfaceCos; 76 mStepHeight = stepHeight; 77 78 //if ( dStricmp( type, "Capsule" ) == 0 ) 79 { 80 F32 radius = getMax( size.x, size.y ) * 0.5f; 81 F32 height = size.z - ( radius * 2.0f ); 82 mColShape = new btCapsuleShapeZ( radius, height ); 83 mColShape->setMargin( 0.05f ); 84 mOriginOffset = ( height * 0.5 ) + radius; 85 } 86 //else 87 { 88 //mColShape = new btBoxShape( btVector3( 0.5f, 0.5f, 1.0f ) ); 89 //mOriginOffset = 1.0f; 90 } 91 92 mGhostObject = new btPairCachingGhostObject(); 93 mGhostObject->setCollisionShape( mColShape ); 94 mGhostObject->setCollisionFlags( btCollisionObject::CF_CHARACTER_OBJECT ); 95 mWorld->getDynamicsWorld()->addCollisionObject( mGhostObject, 96 btBroadphaseProxy::CharacterFilter, 97 btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter ); 98 99 mUserData.setObject( obj ); 100 mGhostObject->setUserPointer( &mUserData ); 101} 102 103Point3F BtPlayer::move( const VectorF &disp, CollisionList &outCol ) 104{ 105 AssertFatal( mGhostObject, "BtPlayer::move - The controller is null!" ); 106 107 if (!mWorld->isEnabled()) 108 { 109 btTransform currentTrans = mGhostObject->getWorldTransform(); 110 btVector3 currentPos = currentTrans.getOrigin(); 111 112 Point3F returnPos = btCast<Point3F>(currentPos); 113 114 returnPos.z -= mOriginOffset; 115 return returnPos; 116 } 117 118 // First recover from any penetrations from the previous tick. 119 U32 numPenetrationLoops = 0; 120 bool touchingContact = false; 121 while ( _recoverFromPenetration() ) 122 { 123 numPenetrationLoops++; 124 touchingContact = true; 125 if ( numPenetrationLoops > 4 ) 126 break; 127 } 128 129 btTransform newTrans = mGhostObject->getWorldTransform(); 130 btVector3 newPos = newTrans.getOrigin(); 131 132 // The move consists of 3 steps... the up step, the forward 133 // step, and the down step. 134 135 btVector3 forwardSweep( disp.x, disp.y, 0.0f ); 136 const bool hasForwardSweep = forwardSweep.length2() > 0.0f; 137 F32 upSweep = 0.0f; 138 F32 downSweep = 0.0f; 139 if ( disp[2] < 0.0f ) 140 downSweep = disp[2]; 141 else 142 upSweep = disp[2]; 143 144 // Only do auto stepping if the character is moving forward. 145 F32 stepOffset = mStepHeight; 146 if ( hasForwardSweep ) 147 upSweep += stepOffset; 148 149 // First we do the up step which includes the passed in 150 // upward displacement as well as the auto stepping. 151 if ( upSweep > 0.0f && 152 _sweep( &newPos, btVector3( 0.0f, 0.0f, upSweep ), NULL ) ) 153 { 154 // Keep track of how far we actually swept to make sure 155 // we do not remove too much in the down sweep. 156 F32 delta = newPos[2] - newTrans.getOrigin()[2]; 157 if ( delta < stepOffset ) 158 stepOffset = delta; 159 } 160 161 // Now do the forward step. 162 _stepForward( &newPos, forwardSweep, &outCol ); 163 164 // Now remove what remains of our auto step 165 // from the down sweep. 166 if ( hasForwardSweep ) 167 downSweep -= stepOffset; 168 169 // Do the downward sweep. 170 if ( downSweep < 0.0f ) 171 _sweep( &newPos, btVector3( 0.0f, 0.0f, downSweep ), &outCol ); 172 173 // Finally update the ghost with its new position. 174 newTrans.setOrigin( newPos ); 175 mGhostObject->setWorldTransform( newTrans ); 176 177 // Return the current position of the ghost. 178 newPos[2] -= mOriginOffset; 179 return btCast<Point3F>( newPos ); 180} 181 182bool BtPlayer::_recoverFromPenetration() 183{ 184 bool penetration = false; 185 186 btDynamicsWorld *collWorld = mWorld->getDynamicsWorld(); 187 188 collWorld->getDispatcher()->dispatchAllCollisionPairs( mGhostObject->getOverlappingPairCache(), 189 collWorld->getDispatchInfo(), 190 collWorld->getDispatcher() ); 191 192 btVector3 currPos = mGhostObject->getWorldTransform().getOrigin(); 193 btScalar maxPen = 0.0f; 194 btManifoldArray manifoldArray; 195 196 for ( U32 i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++ ) 197 { 198 btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; 199 200 if ( ((btCollisionObject*)collisionPair->m_pProxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE || 201 ((btCollisionObject*)collisionPair->m_pProxy1->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE ) 202 continue; 203 204 manifoldArray.resize(0); 205 if (collisionPair->m_algorithm) 206 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); 207 208 for ( U32 j=0; j < manifoldArray.size(); j++ ) 209 { 210 btPersistentManifold* manifold = manifoldArray[j]; 211 btScalar directionSign = manifold->getBody0() == mGhostObject ? -1.0f : 1.0f; 212 213 for ( U32 p=0; p < manifold->getNumContacts(); p++ ) 214 { 215 const btManifoldPoint&pt = manifold->getContactPoint(p); 216 217 if ( pt.getDistance() < -mColShape->getMargin() ) 218 { 219 if ( pt.getDistance() < maxPen ) 220 { 221 maxPen = pt.getDistance(); 222 //m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? 223 } 224 225 currPos += pt.m_normalWorldOnB * directionSign * pt.getDistance(); // * 0.25f; 226 penetration = true; 227 } 228 else 229 { 230 //printf("touching %f\n", pt.getDistance()); 231 } 232 } 233 234 //manifold->clearManifold(); 235 } 236 } 237 238 // Update the ghost transform. 239 btTransform newTrans = mGhostObject->getWorldTransform(); 240 newTrans.setOrigin( currPos ); 241 mGhostObject->setWorldTransform( newTrans ); 242 243 return penetration; 244} 245 246 247class BtPlayerSweepCallback : public btCollisionWorld::ClosestConvexResultCallback 248{ 249 typedef btCollisionWorld::ClosestConvexResultCallback Parent; 250 251public: 252 253 BtPlayerSweepCallback( btCollisionObject *me, const btVector3 &moveVec ) 254 : Parent( btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0) ), 255 mMe( me ), 256 mMoveVec( moveVec ) 257 { 258 } 259 260 virtual bool needsCollision(btBroadphaseProxy* proxy0) const 261 { 262 if ( proxy0->m_clientObject == mMe ) 263 return false; 264 265 return Parent::needsCollision( proxy0 ); 266 } 267 268 virtual btScalar addSingleResult( btCollisionWorld::LocalConvexResult &convexResult, 269 bool normalInWorldSpace ) 270 { 271 // NOTE: I shouldn't have to do any of this, but Bullet 272 // has some weird bugs. 273 // 274 // For one the plane type will return hits on a Z up surface 275 // for sweeps that have no Z sweep component. 276 // 277 // Second the normal returned here is sometimes backwards 278 // to the sweep direction... no clue why. 279 // 280 F32 dotN = mMoveVec.dot( convexResult.m_hitNormalLocal ); 281 if ( mFabs( dotN ) < 0.1f ) 282 return 1.0f; 283 284 if ( convexResult.m_hitCollisionObject->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE ) 285 return 1.0f; 286 287 return Parent::addSingleResult( convexResult, normalInWorldSpace ); 288 } 289 290protected: 291 btVector3 mMoveVec; 292 btCollisionObject *mMe; 293}; 294 295bool BtPlayer::_sweep( btVector3 *inOutCurrPos, const btVector3 &disp, CollisionList *outCol ) 296{ 297 btTransform start( btTransform::getIdentity() ); 298 start.setOrigin ( *inOutCurrPos ); 299 300 btTransform end( btTransform::getIdentity() ); 301 end.setOrigin ( *inOutCurrPos + disp ); 302 303 BtPlayerSweepCallback callback( mGhostObject, disp.normalized() ); 304 callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup; 305 callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask; 306 307 if (disp.length()>0.0001) 308 mGhostObject->convexSweepTest( mColShape, start, end, callback, 0.0f ); 309 310 inOutCurrPos->setInterpolate3( start.getOrigin(), end.getOrigin(), callback.m_closestHitFraction ); 311 if ( callback.hasHit() ) 312 { 313 if ( outCol ) 314 { 315 Collision& col = outCol->increment(); 316 dMemset( &col, 0, sizeof( col ) ); 317 318 col.normal = btCast<Point3F>( callback.m_hitNormalWorld ); 319 col.object = PhysicsUserData::getObject( callback.m_hitCollisionObject->getUserPointer() ); 320 321 F32 vd = col.normal.z; 322 if (vd < mSlopeAngle) 323 return false; 324 } 325 326 return true; 327 } 328 329 return false; 330} 331 332void BtPlayer::_stepForward( btVector3 *inOutCurrPos, const btVector3 &displacement, CollisionList *outCol ) 333{ 334 btTransform start( btTransform::getIdentity() ); 335 btTransform end( btTransform::getIdentity() ); 336 F32 fraction = 1.0f; 337 S32 maxIter = 10; 338 btVector3 disp = displacement; 339 340 while ( fraction > 0.01f && maxIter-- > 0 ) 341 { 342 // Setup the sweep start and end transforms. 343 start.setOrigin( *inOutCurrPos ); 344 end.setOrigin( *inOutCurrPos + disp ); 345 346 BtPlayerSweepCallback callback( mGhostObject, disp.length2() > SIMD_EPSILON ? disp.normalized() : disp ); 347 callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup; 348 callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask; 349 350 if (disp.length()>0.0001) 351 mGhostObject->convexSweepTest( mColShape, start, end, callback, 0.0f ); 352 353 // Subtract from the travel fraction. 354 fraction -= callback.m_closestHitFraction; 355 356 // Did we get a hit? 357 if ( callback.hasHit() ) 358 { 359 /* 360 // Get the real hit normal... Bullet returns the 'seperating normal' and not 361 // the normal of the hit object. 362 btTransform rayStart( btTransform::getIdentity() ); 363 rayStart.setOrigin( callback.m_hitPointWorld + callback.m_hitNormalWorld ); 364 btTransform rayEnd( btTransform::getIdentity() ); 365 rayEnd.setOrigin( callback.m_hitPointWorld - callback.m_hitNormalWorld ); 366 367 btCollisionWorld::ClosestRayResultCallback rayHit( rayStart.getOrigin(), rayEnd.getOrigin() ); 368 mWorld->getDynamicsWorld()->rayTestSingle( rayStart, 369 rayEnd, 370 callback.m_hitCollisionObject, 371 callback.m_hitCollisionObject->getCollisionShape(), 372 callback.m_hitCollisionObject->getWorldTransform(), 373 rayHit ); 374 375 if ( !rayHit.hasHit() ) 376 break; 377 */ 378 379 Collision& col = outCol->increment(); 380 dMemset( &col, 0, sizeof( col ) ); 381 382 col.normal = btCast<Point3F>( callback.m_hitNormalWorld ); 383 col.object = PhysicsUserData::getObject( callback.m_hitCollisionObject->getUserPointer() ); 384 385 // If the collision direction is sideways then modify the collision normal 386 // to remove any z component. This takes care of any sideways collisions 387 // with the round bottom of the capsule when it comes to the Player class 388 // velocity calculations. We want all sideways collisions to be treated 389 // as if they hit the side of a cylinder. 390 if (col.normal.z > 0.0f) 391 { 392 // This will only remove the z component of the collision normal 393 // for the bottom of the character controller, which would hit during 394 // a step. We'll leave the top hemisphere of the character's capsule 395 // alone as bumping one's head is an entirely different story. This 396 // helps with low doorways. 397 col.normal.z = 0.0f; 398 col.normal.normalizeSafe(); 399 } 400 401 // Interpolate to the new position. 402 inOutCurrPos->setInterpolate3( start.getOrigin(), end.getOrigin(), callback.m_closestHitFraction ); 403 404 // Subtract out the displacement along the collision normal. 405 F32 bd = -disp.dot( callback.m_hitNormalWorld ); 406 btVector3 dv = callback.m_hitNormalWorld * bd; 407 disp += dv; 408 } 409 else 410 { 411 // we moved whole way 412 *inOutCurrPos = end.getOrigin(); 413 break; 414 } 415 } 416} 417 418void BtPlayer::findContact( SceneObject **contactObject, 419 VectorF *contactNormal, 420 Vector<SceneObject*> *outOverlapObjects ) const 421{ 422 AssertFatal( mGhostObject, "BtPlayer::findContact - The controller is null!" ); 423 424 VectorF normal; 425 F32 maxDot = -1.0f; 426 427 // Go thru the contact points... get the first contact. 428 btHashedOverlappingPairCache *pairCache = mGhostObject->getOverlappingPairCache(); 429 btBroadphasePairArray& pairArray = pairCache->getOverlappingPairArray(); 430 U32 numPairs = pairArray.size(); 431 btManifoldArray manifoldArray; 432 433 for ( U32 i=0; i < numPairs; i++ ) 434 { 435 const btBroadphasePair &pair = pairArray[i]; 436 437 btBroadphasePair *collisionPair = pairCache->findPair( pair.m_pProxy0, pair.m_pProxy1 ); 438 if ( !collisionPair || !collisionPair->m_algorithm ) 439 continue; 440 441 btCollisionObject *other = (btCollisionObject*)pair.m_pProxy0->m_clientObject; 442 if ( other == mGhostObject ) 443 other = (btCollisionObject*)pair.m_pProxy1->m_clientObject; 444 445 if (!outOverlapObjects->contains(PhysicsUserData::getObject(other->getUserPointer()))) 446 outOverlapObjects->push_back( PhysicsUserData::getObject( other->getUserPointer() ) ); 447 448 if ( other->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE ) 449 continue; 450 451 manifoldArray.clear(); 452 collisionPair->m_algorithm->getAllContactManifolds( manifoldArray ); 453 454 for ( U32 j=0; j < manifoldArray.size(); j++ ) 455 { 456 btPersistentManifold *manifold = manifoldArray[j]; 457 btScalar directionSign = manifold->getBody0() == mGhostObject ? 1.0f : -1.0f; 458 459 for ( U32 p=0; p < manifold->getNumContacts(); p++ ) 460 { 461 const btManifoldPoint &pt = manifold->getContactPoint(p); 462 463 // Test the normal... is it the most vertical one we got? 464 normal = btCast<Point3F>( pt.m_normalWorldOnB * directionSign ); 465 F32 dot = mDot( normal, VectorF( 0, 0, 1 ) ); 466 if ( dot > maxDot ) 467 { 468 maxDot = dot; 469 470 btCollisionObject *colObject = (btCollisionObject*)collisionPair->m_pProxy0->m_clientObject; 471 *contactObject = PhysicsUserData::getObject( colObject->getUserPointer() ); 472 *contactNormal = normal; 473 } 474 } 475 } 476 } 477} 478 479void BtPlayer::enableCollision() 480{ 481 AssertFatal( mGhostObject, "BtPlayer::enableCollision - The controller is null!" ); 482 483 //mController->setCollision( true ); 484} 485 486void BtPlayer::disableCollision() 487{ 488 AssertFatal( mGhostObject, "BtPlayer::disableCollision - The controller is null!" ); 489 490 //mController->setCollision( false ); 491} 492 493PhysicsWorld* BtPlayer::getWorld() 494{ 495 return mWorld; 496} 497 498void BtPlayer::setTransform( const MatrixF &transform ) 499{ 500 AssertFatal( mGhostObject, "BtPlayer::setTransform - The ghost object is null!" ); 501 502 btTransform xfm = btCast<btTransform>( transform ); 503 xfm.getOrigin()[2] += mOriginOffset; 504 505 mGhostObject->setWorldTransform( xfm ); 506} 507 508MatrixF& BtPlayer::getTransform( MatrixF *outMatrix ) 509{ 510 AssertFatal( mGhostObject, "BtPlayer::getTransform - The ghost object is null!" ); 511 512 *outMatrix = btCast<MatrixF>( mGhostObject->getWorldTransform() ); 513 *outMatrix[11] -= mOriginOffset; 514 515 return *outMatrix; 516} 517 518void BtPlayer::setScale( const Point3F &scale ) 519{ 520} 521