btPlayer.cpp

Engine/source/T3D/physics/bullet/btPlayer.cpp

More...

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