Engine/source/T3D/physics/physx3/px3Body.cpp

   1 //-----------------------------------------------------------------------------
   2 // Copyright (c) 2012 GarageGames, LLC
   3 //
   4 // Permission is hereby granted, free of charge, to any person obtaining a copy
   5 // of this software and associated documentation files (the "Software"), to
   6 // deal in the Software without restriction, including without limitation the
   7 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
   8 // sell copies of the Software, and to permit persons to whom the Software is
   9 // furnished to do so, subject to the following conditions:
  10 //
  11 // The above copyright notice and this permission notice shall be included in
  12 // all copies or substantial portions of the Software.
  13 //
  14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  19 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  20 // IN THE SOFTWARE.
  21 //-----------------------------------------------------------------------------
  22
  23 #include "platform/platform.h"
  24 #include "T3D/physics/physx3/px3Body.h"
  25
  26 #include "T3D/physics/physx3/px3.h"
  27 #include "T3D/physics/physx3/px3Casts.h"
  28 #include "T3D/physics/physx3/px3World.h"
  29 #include "T3D/physics/physx3/px3Collision.h"
  30
  31 #include "console/console.h"
  32 #include "console/consoleTypes.h"
  33
  34
  35 Px3Body::Px3Body() :
  36    mActor( NULL ),
  37    mMaterial( NULL ),
  38    mWorld( NULL ),
  39    mBodyFlags( 0 ),
  40    mIsEnabled( true ),
  41    mIsStatic(false)
  42 {
  43 }
  44
  45 Px3Body::~Px3Body()
  46 {
  47    _releaseActor();
  48 }
  49
  50 void Px3Body::_releaseActor()
  51 {
  52    if ( !mActor )
  53       return;
  54
  55    mWorld->releaseWriteLock();
  56
  57    mActor->userData = NULL;
  58
  59    mActor->release();
  60    mActor = NULL;
  61    mBodyFlags = 0;
  62
  63    if ( mMaterial )
  64    {
  65       mMaterial->release();
  66    }
  67
  68    mColShape = NULL;
  69 }
  70
  71 bool Px3Body::init(   PhysicsCollision *shape,
  72                      F32 mass,
  73                      U32 bodyFlags,
  74                      SceneObject *obj,
  75                      PhysicsWorld *world )
  76 {
  77    AssertFatal( obj, "Px3Body::init - Got a null scene object!" );
  78    AssertFatal( world, "Px3Body::init - Got a null world!" );
  79    AssertFatal( dynamic_cast<Px3World*>( world ), "Px3Body::init - The world is the wrong type!" );
  80    AssertFatal( shape, "Px3Body::init - Got a null collision shape!" );
  81    AssertFatal( dynamic_cast<Px3Collision*>( shape ), "Px3Body::init - The collision shape is the wrong type!" );
  82    AssertFatal( !((Px3Collision*)shape)->getShapes().empty(), "Px3Body::init - Got empty collision shape!" );
  83
  84    // Cleanup any previous actor.
  85    _releaseActor();
  86
  87    mWorld = (Px3World*)world;
  88    mColShape = (Px3Collision*)shape;
  89    mBodyFlags = bodyFlags;
  90
  91    const bool isKinematic = mBodyFlags & BF_KINEMATIC;
  92    const bool isTrigger = mBodyFlags & BF_TRIGGER;
  93    const bool isDebris = mBodyFlags & BF_DEBRIS;
  94
  95    if ( isKinematic )
  96    {
  97                 mActor = gPhysics3SDK->createRigidDynamic(physx::PxTransform(physx::PxIDENTITY()));
  98                 physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
  99                 actor->setRigidDynamicFlag(physx::PxRigidDynamicFlag::eKINEMATIC, true);
 100                 actor->setMass(getMax( mass, 1.0f ));
 101    }
 102    else if ( mass > 0.0f )
 103    {
 104       mActor = gPhysics3SDK->createRigidDynamic(physx::PxTransform(physx::PxIDENTITY()));
 105    }
 106    else
 107    {
 108       mActor = gPhysics3SDK->createRigidStatic(physx::PxTransform(physx::PxIDENTITY()));
 109       mIsStatic = true;
 110         }
 111
 112    mMaterial = gPhysics3SDK->createMaterial(0.6f,0.4f,0.1f);
 113
 114    // Add all the shapes.
 115    const Vector<Px3CollisionDesc*> &shapes = mColShape->getShapes();
 116    for ( U32 i=0; i < shapes.size(); i++ )
 117    {
 118            Px3CollisionDesc* desc = shapes[i];
 119            if( mass > 0.0f )
 120            {
 121                         if(desc->pGeometry->getType() == physx::PxGeometryType::eTRIANGLEMESH)
 122                         {
 123                                 Con::errorf("PhysX3 Dynamic Triangle Mesh is not supported.");
 124                         }
 125            }
 126            physx::PxShape * pShape = mActor->createShape(*desc->pGeometry,*mMaterial);
 127            physx::PxFilterData colData;
 128            if(isDebris)
 129                         colData.word0 = PX3_DEBRIS;
 130            else if(isTrigger)
 131         colData.word0 = PX3_TRIGGER;
 132            else
 133                    colData.word0 = PX3_DEFAULT;
 134
 135       //set local pose - actor->createShape with a local pose is deprecated in physx 3.3
 136       pShape->setLocalPose(desc->pose);
 137       //set the skin width
 138       pShape->setContactOffset(0.01f);
 139       pShape->setFlag(physx::PxShapeFlag::eSIMULATION_SHAPE, !isTrigger);
 140       pShape->setFlag(physx::PxShapeFlag::eSCENE_QUERY_SHAPE,true);
 141       pShape->setSimulationFilterData(colData);
 142       pShape->setQueryFilterData(colData);
 143    }
 144
 145    //mass & intertia has to be set after creating the shape
 146    if ( mass > 0.0f )
 147    {
 148                 physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 149                 physx::PxRigidBodyExt::setMassAndUpdateInertia(*actor,mass);
 150    }
 151
 152     // This sucks, but it has to happen if we want
 153    // to avoid write lock errors from PhysX right now.
 154    mWorld->releaseWriteLock();
 155
 156    mWorld->getScene()->addActor(*mActor);
 157    mIsEnabled = true;
 158
 159    if ( isDebris )
 160      mActor->setDominanceGroup( 31 );
 161
 162    mUserData.setObject( obj );
 163    mUserData.setBody( this );
 164    mActor->userData = &mUserData;
 165
 166    return true;
 167 }
 168
 169 void Px3Body::setMaterial(  F32 restitution,
 170                            F32 friction,
 171                            F32 staticFriction )
 172 {
 173    AssertFatal( mActor, "Px3Body::setMaterial - The actor is null!" );
 174
 175    if ( isDynamic() )
 176    {
 177       physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 178       actor->wakeUp();
 179    }
 180
 181          mMaterial->setRestitution(restitution);
 182          mMaterial->setStaticFriction(staticFriction);
 183          mMaterial->setDynamicFriction(friction);
 184
 185 }
 186
 187 void Px3Body::setSleepThreshold( F32 linear, F32 angular )
 188 {
 189    AssertFatal( mActor, "Px3Body::setSleepThreshold - The actor is null!" );
 190
 191    if(mIsStatic)
 192            return;
 193
 194    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 195    physx::PxF32 massNormalized= (linear*linear+angular*angular)/2.0f;
 196    actor->setSleepThreshold(massNormalized);
 197 }
 198
 199 void Px3Body::setDamping( F32 linear, F32 angular )
 200 {
 201    AssertFatal( mActor, "Px3Body::setDamping - The actor is null!" );
 202    if(mIsStatic)
 203            return;
 204
 205    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 206    actor->setLinearDamping( linear );
 207    actor->setAngularDamping( angular );
 208 }
 209
 210 void Px3Body::getState( PhysicsState *outState )
 211 {
 212    AssertFatal( mActor, "Px3Body::getState - The actor is null!" );
 213    AssertFatal( isDynamic(), "Px3Body::getState - This call is only for dynamics!" );
 214
 215    outState->position = px3Cast<Point3F>( mActor->getGlobalPose().p );
 216    outState->orientation = px3Cast<QuatF>( mActor->getGlobalPose().q );
 217
 218    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 219    outState->linVelocity = px3Cast<Point3F>( actor->getLinearVelocity() );
 220    outState->angVelocity = px3Cast<Point3F>( actor->getAngularVelocity() );
 221    outState->sleeping = actor->isSleeping();
 222    outState->momentum = px3Cast<Point3F>( (1.0f/actor->getMass()) * actor->getLinearVelocity() );
 223
 224 }
 225
 226 F32 Px3Body::getMass() const
 227 {
 228    AssertFatal( mActor, "PxBody::getCMassPosition - The actor is null!" );
 229    if(mIsStatic)
 230            return 0;
 231
 232    const physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 233    return actor->getMass();
 234 }
 235
 236 Point3F Px3Body::getCMassPosition() const
 237 {
 238    AssertFatal( mActor, "Px3Body::getCMassPosition - The actor is null!" );
 239    if(mIsStatic)
 240            return px3Cast<Point3F>(mActor->getGlobalPose().p);
 241
 242    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 243    physx::PxTransform pose = actor->getGlobalPose() * actor->getCMassLocalPose();
 244    return px3Cast<Point3F>(pose.p);
 245 }
 246
 247 void Px3Body::setLinVelocity( const Point3F &vel )
 248 {
 249    AssertFatal( mActor, "Px3Body::setLinVelocity - The actor is null!" );
 250    AssertFatal( isDynamic(), "Px3Body::setLinVelocity - This call is only for dynamics!" );
 251
 252    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 253    actor->setLinearVelocity( px3Cast<physx::PxVec3>( vel ) );
 254 }
 255
 256 void Px3Body::setAngVelocity( const Point3F &vel )
 257 {
 258    AssertFatal( mActor, "Px3Body::setAngVelocity - The actor is null!" );
 259    AssertFatal( isDynamic(), "Px3Body::setAngVelocity - This call is only for dynamics!" );
 260
 261    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 262    actor->setAngularVelocity(px3Cast<physx::PxVec3>( vel ) );
 263 }
 264
 265 Point3F Px3Body::getLinVelocity() const
 266 {
 267    AssertFatal( mActor, "Px3Body::getLinVelocity - The actor is null!" );
 268    AssertFatal( isDynamic(), "Px3Body::getLinVelocity - This call is only for dynamics!" );
 269
 270    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 271    return px3Cast<Point3F>( actor->getLinearVelocity() );
 272 }
 273
 274 Point3F Px3Body::getAngVelocity() const
 275 {
 276    AssertFatal( mActor, "Px3Body::getAngVelocity - The actor is null!" );
 277    AssertFatal( isDynamic(), "Px3Body::getAngVelocity - This call is only for dynamics!" );
 278
 279    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 280    return px3Cast<Point3F>( actor->getAngularVelocity() );
 281 }
 282
 283 void Px3Body::setSleeping( bool sleeping )
 284 {
 285    AssertFatal( mActor, "Px3Body::setSleeping - The actor is null!" );
 286    AssertFatal( isDynamic(), "Px3Body::setSleeping - This call is only for dynamics!" );
 287
 288    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 289    if ( sleeping )
 290       actor->putToSleep();
 291    else
 292       actor->wakeUp();
 293 }
 294
 295 bool Px3Body::isDynamic() const
 296 {
 297    AssertFatal( mActor, "PxBody::isDynamic - The actor is null!" );
 298    return !mIsStatic && ( mBodyFlags & BF_KINEMATIC ) == 0;
 299 }
 300
 301 PhysicsWorld* Px3Body::getWorld()
 302 {
 303    return mWorld;
 304 }
 305
 306 PhysicsCollision* Px3Body::getColShape()
 307 {
 308    return mColShape;
 309 }
 310
 311 MatrixF& Px3Body::getTransform( MatrixF *outMatrix )
 312 {
 313    AssertFatal( mActor, "Px3Body::getTransform - The actor is null!" );
 314
 315    *outMatrix = px3Cast<MatrixF>(mActor->getGlobalPose());
 316    return *outMatrix;
 317 }
 318
 319 Box3F Px3Body::getWorldBounds()
 320 {
 321    AssertFatal( mActor, "Px3Body::getTransform - The actor is null!" );
 322
 323    physx::PxBounds3 bounds;
 324    bounds.setEmpty();
 325    physx::PxBounds3 shapeBounds;
 326
 327
 328    U32 shapeCount = mActor->getNbShapes();
 329         physx::PxShape **shapes = new physx::PxShape*[shapeCount];
 330         mActor->getShapes(shapes, shapeCount);
 331    for ( U32 i = 0; i < shapeCount; i++ )
 332    {
 333       // Get the shape's bounds.
 334       shapeBounds = physx::PxShapeExt::getWorldBounds(*shapes[i],*mActor);
 335       // Combine them into the total bounds.
 336       bounds.include( shapeBounds );
 337    }
 338
 339    delete [] shapes;
 340
 341    return px3Cast<Box3F>( bounds );
 342 }
 343
 344 void Px3Body::setSimulationEnabled( bool enabled )
 345 {
 346    if ( mIsEnabled == enabled )
 347       return;
 348
 349    //Don't need to enable/disable eSIMULATION_SHAPE for trigger,it's disabled permanently
 350    if(mBodyFlags & BF_TRIGGER)
 351       return;
 352
 353    // This sucks, but it has to happen if we want
 354    // to avoid write lock errors from PhysX right now.
 355    mWorld->releaseWriteLock();
 356
 357    U32 shapeCount = mActor->getNbShapes();
 358         physx::PxShape **shapes = new physx::PxShape*[shapeCount];
 359         mActor->getShapes(shapes, shapeCount);
 360    for ( S32 i = 0; i < mActor->getNbShapes(); i++ )
 361    {
 362            shapes[i]->setFlag(physx::PxShapeFlag::eSIMULATION_SHAPE,!mIsEnabled);//?????
 363    }
 364
 365    delete [] shapes;
 366 }
 367 void Px3Body::setTransform( const MatrixF &transform )
 368 {
 369    AssertFatal( mActor, "Px3Body::setTransform - The actor is null!" );
 370
 371
 372    // This sucks, but it has to happen if we want
 373    // to avoid write lock errors from PhysX right now.
 374    mWorld->releaseWriteLock();
 375
 376
 377    mActor->setGlobalPose(px3Cast<physx::PxTransform>(transform),false);
 378
 379    if(mIsStatic)
 380            return;
 381
 382         physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 383         bool kinematic = actor->getRigidDynamicFlags() & physx::PxRigidDynamicFlag::eKINEMATIC;
 384    // If its dynamic we have more to do.
 385    if ( isDynamic() && !kinematic )
 386    {
 387       actor->setLinearVelocity( physx::PxVec3(0) );
 388       actor->setAngularVelocity( physx::PxVec3(0) );
 389       actor->wakeUp();
 390    }
 391 }
 392
 393 void Px3Body::applyCorrection( const MatrixF &transform )
 394 {
 395    AssertFatal( mActor, "Px3Body::applyCorrection - The actor is null!" );
 396    AssertFatal( isDynamic(), "Px3Body::applyCorrection - This call is only for dynamics!" );
 397
 398    // This sucks, but it has to happen if we want
 399    // to avoid write lock errors from PhysX right now.
 400    mWorld->releaseWriteLock();
 401
 402    mActor->setGlobalPose( px3Cast<physx::PxTransform>(transform) );
 403 }
 404
 405 void Px3Body::applyImpulse( const Point3F &origin, const Point3F &force )
 406 {
 407    AssertFatal( mActor, "Px3Body::applyImpulse - The actor is null!" );
 408
 409    // This sucks, but it has to happen if we want
 410    // to avoid write lock errors from PhysX right now.
 411    mWorld->releaseWriteLock();
 412    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 413    if ( mIsEnabled && isDynamic() )
 414    physx::PxRigidBodyExt::addForceAtPos(*actor,px3Cast<physx::PxVec3>(force),
 415                                                                                                 px3Cast<physx::PxVec3>(origin),
 416                                                                                                 physx::PxForceMode::eIMPULSE);
 417
 418 }
 419
 420 void Px3Body::applyTorque( const Point3F &torque )
 421 {
 422    AssertFatal(mActor, "Px3Body::applyImpulse - The actor is null!");
 423
 424    mWorld->releaseWriteLock();
 425    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 426    if (mIsEnabled && isDynamic())
 427       actor->addTorque( px3Cast<physx::PxVec3>(torque), physx::PxForceMode::eFORCE, true);
 428 }
 429
 430 void Px3Body::findContact(SceneObject **contactObject,
 431    VectorF *contactNormal,
 432    Vector<SceneObject*> *outOverlapObjects) const
 433 {
 434 }
 435
 436 void Px3Body::moveKinematicTo(const MatrixF &transform)
 437 {
 438    AssertFatal(mActor, "Px3Body::moveKinematicTo - The actor is null!");
 439
 440    const bool isKinematic = mBodyFlags & BF_KINEMATIC;
 441    if (!isKinematic)
 442    {
 443       Con::errorf("Px3Body::moveKinematicTo is only for kinematic bodies.");
 444       return;
 445    }
 446
 447    mWorld->lockScene();
 448
 449    physx::PxRigidDynamic *actor = mActor->is<physx::PxRigidDynamic>();
 450    actor->setKinematicTarget(px3Cast<physx::PxTransform>(transform));
 451
 452    mWorld->unlockScene();
 453 }
 454