Engine directory for ticket #1

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

@@ -0,0 +1,377 @@
+//-----------------------------------------------------------------------------
+// Copyright (c) 2012 GarageGames, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+//-----------------------------------------------------------------------------
+
+#include "platform/platform.h"
+#include "T3D/physics/bullet/btWorld.h"
+
+#include "T3D/physics/bullet/btPlugin.h"
+#include "T3D/physics/bullet/btCasts.h"
+#include "T3D/physics/physicsUserData.h"
+#include "core/stream/bitStream.h"
+#include "platform/profiler.h"
+#include "sim/netConnection.h"
+#include "console/console.h"
+#include "console/consoleTypes.h"
+#include "scene/sceneRenderState.h"
+#include "T3D/gameBase/gameProcess.h"
+
+
+BtWorld::BtWorld() :
+   mProcessList( NULL ),
+   mIsSimulating( false ),
+   mErrorReport( false ),
+   mTickCount( 0 ),
+   mIsEnabled( false ),
+   mEditorTimeScale( 1.0f ),
+   mDynamicsWorld( NULL ),
+   mThreadSupportCollision( NULL )
+{
+} 
+
+BtWorld::~BtWorld()
+{
+}
+
+bool BtWorld::initWorld( bool isServer, ProcessList *processList )
+{
+   // Collision configuration contains default setup for memory, collision setup.
+   mCollisionConfiguration = new btDefaultCollisionConfiguration();
+
+   // TODO: There is something wrong with multithreading
+   // and compound convex shapes... so disable it for now.
+   static const U32 smMaxThreads = 1;
+
+   // Different initialization with threading enabled.
+   if ( smMaxThreads > 1 )
+   {
+	   
+	   // TODO: ifdef assumes smMaxThread is always one at this point. MACOSX support to be decided
+#ifdef WIN32
+      mThreadSupportCollision = new Win32ThreadSupport( 
+         Win32ThreadSupport::Win32ThreadConstructionInfo(   isServer ? "bt_servercol" : "bt_clientcol",
+								                                    processCollisionTask,
+								                                    createCollisionLocalStoreMemory,
+								                                    smMaxThreads ) );
+      
+      mDispatcher = new	SpuGatheringCollisionDispatcher( mThreadSupportCollision,
+                                                         smMaxThreads,
+                                                         mCollisionConfiguration );
+#endif // WIN32
+   }
+   else
+   {
+      mThreadSupportCollision = NULL;
+      mDispatcher = new	btCollisionDispatcher( mCollisionConfiguration );
+   }
+  
+   btVector3 worldMin( -2000, -2000, -1000 );
+   btVector3 worldMax( 2000, 2000, 1000 );
+   btAxisSweep3 *sweepBP = new btAxisSweep3( worldMin, worldMax );
+   mBroadphase = sweepBP;
+   sweepBP->getOverlappingPairCache()->setInternalGhostPairCallback( new btGhostPairCallback() );
+
+   // The default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded).
+   mSolver = new btSequentialImpulseConstraintSolver;
+
+   mDynamicsWorld = new btDiscreteDynamicsWorld( mDispatcher, mBroadphase, mSolver, mCollisionConfiguration );
+   if ( !mDynamicsWorld )
+   {
+      Con::errorf( "BtWorld - %s failed to create dynamics world!", isServer ? "Server" : "Client" );
+      return false;
+   }
+
+   // Removing the randomization in the solver is required
+   // to make the simulation deterministic.
+   mDynamicsWorld->getSolverInfo().m_solverMode &= ~SOLVER_RANDMIZE_ORDER;
+
+   mDynamicsWorld->setGravity( btCast<btVector3>( mGravity ) );
+
+   AssertFatal( processList, "BtWorld::init() - We need a process list to create the world!" );
+   mProcessList = processList;
+   mProcessList->preTickSignal().notify( this, &BtWorld::getPhysicsResults );
+   mProcessList->postTickSignal().notify( this, &BtWorld::tickPhysics, 1000.0f );
+
+   return true;
+}
+
+void BtWorld::_destroy()
+{
+   // Release the tick processing signals.
+   if ( mProcessList )
+   {
+      mProcessList->preTickSignal().remove( this, &BtWorld::getPhysicsResults );
+      mProcessList->postTickSignal().remove( this, &BtWorld::tickPhysics );
+      mProcessList = NULL;
+   }
+
+   // TODO: Release any remaining
+   // orphaned rigid bodies here.
+
+   SAFE_DELETE( mDynamicsWorld );
+   SAFE_DELETE( mSolver );
+   SAFE_DELETE( mBroadphase );
+   SAFE_DELETE( mDispatcher );
+   SAFE_DELETE( mThreadSupportCollision );
+   SAFE_DELETE( mCollisionConfiguration );
+}
+
+void BtWorld::tickPhysics( U32 elapsedMs )
+{
+   if ( !mDynamicsWorld || !mIsEnabled )
+      return;
+
+   // Did we forget to call getPhysicsResults somewhere?
+   AssertFatal( !mIsSimulating, "PhysXWorld::tickPhysics() - Already simulating!" );
+
+   // The elapsed time should be non-zero and 
+   // a multiple of TickMs!
+   AssertFatal(   elapsedMs != 0 &&
+                  ( elapsedMs % TickMs ) == 0 , "PhysXWorld::tickPhysics() - Got bad elapsed time!" );
+
+   PROFILE_SCOPE(BtWorld_TickPhysics);
+
+   // Convert it to seconds.
+   const F32 elapsedSec = (F32)elapsedMs * 0.001f;
+
+   // Simulate... it is recommended to always use Bullet's default fixed timestep/
+   mDynamicsWorld->stepSimulation( elapsedSec * mEditorTimeScale );
+
+   mIsSimulating = true;
+
+   //Con::printf( "%s BtWorld::tickPhysics!", this == smClientWorld ? "Client" : "Server" );
+}
+
+void BtWorld::getPhysicsResults()
+{
+   if ( !mDynamicsWorld || !mIsSimulating ) 
+      return;
+
+   PROFILE_SCOPE(BtWorld_GetPhysicsResults);
+
+   // Get results from scene.
+  // mScene->fetchResults( NX_RIGID_BODY_FINISHED, true );
+   mIsSimulating = false;
+   mTickCount++;
+}
+
+void BtWorld::setEnabled( bool enabled )
+{
+   mIsEnabled = enabled;
+
+   if ( !mIsEnabled )
+      getPhysicsResults();
+}
+
+void BtWorld::destroyWorld()
+{
+   _destroy();
+}
+
+bool BtWorld::castRay( const Point3F &startPnt, const Point3F &endPnt, RayInfo *ri, const Point3F &impulse )
+{
+   btCollisionWorld::ClosestRayResultCallback result( btCast<btVector3>( startPnt ), btCast<btVector3>( endPnt ) );
+   mDynamicsWorld->rayTest( btCast<btVector3>( startPnt ), btCast<btVector3>( endPnt ), result );
+
+   if ( !result.hasHit() || !result.m_collisionObject )
+      return false;
+
+   if ( ri )
+   {
+      ri->object = PhysicsUserData::getObject( result.m_collisionObject->getUserPointer() );
+      
+      // If we were passed a RayInfo, we can only return true signifying a collision
+      // if we hit an object that actually has a torque object associated with it.
+      //
+      // In some ways this could be considered an error, either a physx object
+      // has raycast-collision enabled that shouldn't or someone did not set
+      // an object in this actor's userData.
+      //
+      if ( ri->object == NULL )
+         return false;
+
+      ri->distance = ( endPnt - startPnt ).len() * result.m_closestHitFraction;
+      ri->normal = btCast<Point3F>( result.m_hitNormalWorld );
+      ri->point = btCast<Point3F>( result.m_hitPointWorld );
+      ri->t = result.m_closestHitFraction;
+   }
+
+   /*
+   if ( impulse.isZero() ||
+        !actor.isDynamic() ||
+        actor.readBodyFlag( NX_BF_KINEMATIC ) )
+      return true;
+      
+   NxVec3 force = pxCast<NxVec3>( impulse );//worldRay.dir * forceAmt; 
+   actor.addForceAtPos( force, hitInfo.worldImpact, NX_IMPULSE );
+   */
+
+   return true;
+}
+
+PhysicsBody* BtWorld::castRay( const Point3F &start, const Point3F &end, U32 bodyTypes )
+{
+   btVector3 startPt = btCast<btVector3>( start );
+   btVector3 endPt = btCast<btVector3>( end );
+
+   btCollisionWorld::ClosestRayResultCallback result( startPt, endPt );
+   mDynamicsWorld->rayTest( startPt, endPt, result );
+
+   if ( !result.hasHit() || !result.m_collisionObject )
+      return NULL;
+
+   PhysicsUserData *userData = PhysicsUserData::cast( result.m_collisionObject->getUserPointer() );
+   if ( !userData )
+      return NULL;
+
+   return userData->getBody();
+}
+
+void BtWorld::explosion( const Point3F &pos, F32 radius, F32 forceMagnitude )
+{
+   /*
+   // Find Actors at the position within the radius
+   // and apply force to them.
+
+   NxVec3 nxPos = pxCast<NxVec3>( pos );
+   NxShape **shapes = (NxShape**)NxAlloca(10*sizeof(NxShape*));
+   NxSphere worldSphere( nxPos, radius );
+
+   NxU32 numHits = mScene->overlapSphereShapes( worldSphere, NX_ALL_SHAPES, 10, shapes, NULL );
+
+   for ( NxU32 i = 0; i < numHits; i++ )
+   {
+      NxActor &actor = shapes[i]->getActor();
+      
+      bool dynamic = actor.isDynamic();
+      
+      if ( !dynamic )
+         continue;
+
+      bool kinematic = actor.readBodyFlag( NX_BF_KINEMATIC );
+      
+      if ( kinematic )
+         continue;
+
+      NxVec3 force = actor.getGlobalPosition() - nxPos;
+      force.normalize();
+      force *= forceMagnitude;
+
+      actor.addForceAtPos( force, nxPos, NX_IMPULSE, true );
+   }
+   */
+}
+
+void BtWorld::onDebugDraw( const SceneRenderState *state )
+{
+   mDebugDraw.setCuller( &state->getFrustum() );
+
+   mDynamicsWorld->setDebugDrawer( &mDebugDraw );
+   mDynamicsWorld->debugDrawWorld();
+   mDynamicsWorld->setDebugDrawer( NULL );
+
+   mDebugDraw.flush();
+}
+
+void BtWorld::reset()
+{
+   if ( !mDynamicsWorld )
+      return;
+
+    ///create a copy of the array, not a reference!
+    btCollisionObjectArray copyArray = mDynamicsWorld->getCollisionObjectArray();
+
+    S32 numObjects = mDynamicsWorld->getNumCollisionObjects();
+    for ( S32 i=0; i < numObjects; i++ )
+    {
+            btCollisionObject* colObj = copyArray[i];
+            btRigidBody* body = btRigidBody::upcast(colObj);
+
+            if (body)
+            {
+                    if (body->getMotionState())
+                    {
+                            //btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
+                            //myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
+                            //body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
+                            //colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
+                            colObj->forceActivationState(ACTIVE_TAG);
+                            colObj->activate();
+                            colObj->setDeactivationTime(0);
+                            //colObj->setActivationState(WANTS_DEACTIVATION);
+                    }
+
+                    //removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
+                    //m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
+
+                    btRigidBody* body = btRigidBody::upcast(colObj);
+                    if (body && !body->isStaticObject())
+                    {
+                         btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
+                         btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
+                    }
+            }
+
+    }
+
+    // reset some internal cached data in the broadphase
+    mDynamicsWorld->getBroadphase()->resetPool( mDynamicsWorld->getDispatcher() );
+    mDynamicsWorld->getConstraintSolver()->reset();
+}
+
+/*
+ConsoleFunction( castForceRay, const char*, 4, 4, "( Point3F startPnt, Point3F endPnt, VectorF impulseVec )" )
+{
+   PhysicsWorld *world = PHYSICSPLUGIN->getWorld( "server" );
+   if ( !world )
+      return NULL;
+   
+   char *returnBuffer = Con::getReturnBuffer(256);
+
+   Point3F impulse;
+   Point3F startPnt, endPnt;
+   dSscanf( argv[1], "%f %f %f", &startPnt.x, &startPnt.y, &startPnt.z );
+   dSscanf( argv[2], "%f %f %f", &endPnt.x, &endPnt.y, &endPnt.z );
+   dSscanf( argv[3], "%f %f %f", &impulse.x, &impulse.y, &impulse.z );
+
+   Point3F hitPoint;
+
+   RayInfo rinfo;
+
+   bool hit = world->castRay( startPnt, endPnt, &rinfo, impulse );
+
+   DebugDrawer *ddraw = DebugDrawer::get();
+   if ( ddraw )
+   {
+      ddraw->drawLine( startPnt, endPnt, hit ? ColorF::RED : ColorF::GREEN );
+      ddraw->setLastTTL( 3000 );
+   }
+
+   if ( hit )
+   {
+      dSprintf(returnBuffer, 256, "%g %g %g",
+            rinfo.point.x, rinfo.point.y, rinfo.point.z );
+      return returnBuffer;
+   }
+   else 
+      return NULL;
+}
+*/