2015-11-23 04:50:32 +00:00
# include "PhysicsServerCommandProcessor.h"
# include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
# include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
# include "../Importers/ImportURDFDemo/URDF2Bullet.h"
2016-06-01 05:55:13 +00:00
# include "TinyRendererVisualShapeConverter.h"
2015-11-23 04:50:32 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
# include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
# include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
# include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
# include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
# include "../CommonInterfaces/CommonRenderInterface.h"
# include "btBulletDynamicsCommon.h"
# include "LinearMath/btTransform.h"
# include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
# include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
# include "LinearMath/btSerializer.h"
# include "Bullet3Common/b3Logging.h"
# include "../CommonInterfaces/CommonGUIHelperInterface.h"
# include "SharedMemoryCommands.h"
struct UrdfLinkNameMapUtil
{
btMultiBody * m_mb ;
btDefaultSerializer * m_memSerializer ;
UrdfLinkNameMapUtil ( ) : m_mb ( 0 ) , m_memSerializer ( 0 )
{
}
} ;
struct SharedMemoryDebugDrawer : public btIDebugDraw
{
int m_debugMode ;
btAlignedObjectArray < SharedMemLines > m_lines2 ;
SharedMemoryDebugDrawer ( )
: m_debugMode ( 0 )
{
}
virtual void drawContactPoint ( const btVector3 & PointOnB , const btVector3 & normalOnB , btScalar distance , int lifeTime , const btVector3 & color )
{
}
virtual void reportErrorWarning ( const char * warningString )
{
}
virtual void draw3dText ( const btVector3 & location , const char * textString )
{
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
virtual void setDebugMode ( int debugMode )
{
m_debugMode = debugMode ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
virtual int getDebugMode ( ) const
{
return m_debugMode ;
}
virtual void drawLine ( const btVector3 & from , const btVector3 & to , const btVector3 & color )
{
SharedMemLines line ;
line . m_from = from ;
line . m_to = to ;
line . m_color = color ;
m_lines2 . push_back ( line ) ;
}
} ;
struct InteralBodyData
{
btMultiBody * m_multiBody ;
btRigidBody * m_rigidBody ;
int m_testData ;
btTransform m_rootLocalInertialFrame ;
2016-04-29 21:45:15 +00:00
btAlignedObjectArray < btTransform > m_linkLocalInertialFrames ;
2015-11-23 04:50:32 +00:00
InteralBodyData ( )
: m_multiBody ( 0 ) ,
m_rigidBody ( 0 ) ,
m_testData ( 0 )
{
m_rootLocalInertialFrame . setIdentity ( ) ;
}
} ;
///todo: templatize this
struct InternalBodyHandle : public InteralBodyData
{
BT_DECLARE_ALIGNED_ALLOCATOR ( ) ;
int m_nextFreeHandle ;
2016-06-24 14:31:17 +00:00
void SetNextFree ( int next )
2015-11-23 04:50:32 +00:00
{
m_nextFreeHandle = next ;
}
2016-06-24 14:31:17 +00:00
int GetNextFree ( ) const
2015-11-23 04:50:32 +00:00
{
return m_nextFreeHandle ;
}
} ;
class btCommandChunk
{
public :
int m_chunkCode ;
int m_length ;
void * m_oldPtr ;
int m_dna_nr ;
int m_number ;
} ;
class bCommandChunkPtr4
{
public :
bCommandChunkPtr4 ( ) { }
int code ;
int len ;
union
{
int m_uniqueInt ;
} ;
int dna_nr ;
int nr ;
} ;
// ----------------------------------------------------- //
class bCommandChunkPtr8
{
public :
bCommandChunkPtr8 ( ) { }
int code , len ;
union
{
int m_uniqueInts [ 2 ] ;
} ;
int dna_nr , nr ;
} ;
struct CommandLogger
{
FILE * m_file ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
void writeHeader ( unsigned char * buffer ) const
{
# ifdef BT_USE_DOUBLE_PRECISION
memcpy ( buffer , " BT3CMDd " , 7 ) ;
# else
memcpy ( buffer , " BT3CMDf " , 7 ) ;
# endif //BT_USE_DOUBLE_PRECISION
int littleEndian = 1 ;
littleEndian = ( ( char * ) & littleEndian ) [ 0 ] ;
if ( sizeof ( void * ) = = 8 )
{
buffer [ 7 ] = ' - ' ;
} else
{
buffer [ 7 ] = ' _ ' ;
}
if ( littleEndian )
{
buffer [ 8 ] = ' v ' ;
} else
{
buffer [ 8 ] = ' V ' ;
}
buffer [ 9 ] = 0 ;
buffer [ 10 ] = 0 ;
buffer [ 11 ] = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int ver = btGetVersion ( ) ;
if ( ver > = 0 & & ver < 999 )
{
sprintf ( ( char * ) & buffer [ 9 ] , " %d " , ver ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
void logCommand ( const SharedMemoryCommand & command )
{
btCommandChunk chunk ;
chunk . m_chunkCode = command . m_type ;
chunk . m_oldPtr = 0 ;
chunk . m_dna_nr = 0 ;
chunk . m_length = sizeof ( SharedMemoryCommand ) ;
chunk . m_number = 1 ;
fwrite ( ( const char * ) & chunk , sizeof ( btCommandChunk ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
}
CommandLogger ( const char * fileName )
{
m_file = fopen ( fileName , " wb " ) ;
unsigned char buf [ 15 ] ;
buf [ 12 ] = 12 ;
buf [ 13 ] = 13 ;
buf [ 14 ] = 14 ;
writeHeader ( buf ) ;
fwrite ( buf , 12 , 1 , m_file ) ;
}
virtual ~ CommandLogger ( )
{
fclose ( m_file ) ;
}
} ;
struct CommandLogPlayback
{
unsigned char m_header [ 12 ] ;
FILE * m_file ;
bool m_bitsVary ;
bool m_fileIs64bit ;
CommandLogPlayback ( const char * fileName )
{
m_file = fopen ( fileName , " rb " ) ;
if ( m_file )
{
fread ( m_header , 12 , 1 , m_file ) ;
}
unsigned char c = m_header [ 7 ] ;
m_fileIs64bit = ( c = = ' - ' ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
const bool VOID_IS_8 = ( ( sizeof ( void * ) = = 8 ) ) ;
m_bitsVary = ( VOID_IS_8 ! = m_fileIs64bit ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
virtual ~ CommandLogPlayback ( )
{
if ( m_file )
{
fclose ( m_file ) ;
m_file = 0 ;
}
}
bool processNextCommand ( SharedMemoryCommand * cmd )
{
if ( m_file )
{
size_t s = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_fileIs64bit )
{
bCommandChunkPtr8 chunk8 ;
s = fread ( ( void * ) & chunk8 , sizeof ( bCommandChunkPtr8 ) , 1 , m_file ) ;
} else
{
bCommandChunkPtr4 chunk4 ;
s = fread ( ( void * ) & chunk4 , sizeof ( bCommandChunkPtr4 ) , 1 , m_file ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( s = = 1 )
{
s = fread ( cmd , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
return ( s = = 1 ) ;
}
}
return false ;
}
} ;
struct PhysicsServerCommandProcessorInternalData
{
///handle management
btAlignedObjectArray < InternalBodyHandle > m_bodyHandles ;
int m_numUsedHandles ; // number of active handles
int m_firstFreeHandle ; // free handles list
InternalBodyHandle * getHandle ( int handle )
{
btAssert ( handle > = 0 ) ;
btAssert ( handle < m_bodyHandles . size ( ) ) ;
if ( ( handle < 0 ) | | ( handle > = m_bodyHandles . size ( ) ) )
{
return 0 ;
}
return & m_bodyHandles [ handle ] ;
}
const InternalBodyHandle * getHandle ( int handle ) const
{
return & m_bodyHandles [ handle ] ;
}
void increaseHandleCapacity ( int extraCapacity )
{
int curCapacity = m_bodyHandles . size ( ) ;
btAssert ( curCapacity = = m_numUsedHandles ) ;
int newCapacity = curCapacity + extraCapacity ;
m_bodyHandles . resize ( newCapacity ) ;
{
for ( int i = curCapacity ; i < newCapacity ; i + + )
m_bodyHandles [ i ] . SetNextFree ( i + 1 ) ;
m_bodyHandles [ newCapacity - 1 ] . SetNextFree ( - 1 ) ;
}
m_firstFreeHandle = curCapacity ;
}
void initHandles ( )
{
m_numUsedHandles = 0 ;
m_firstFreeHandle = - 1 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
increaseHandleCapacity ( 1 ) ;
}
void exitHandles ( )
{
m_bodyHandles . resize ( 0 ) ;
m_firstFreeHandle = - 1 ;
m_numUsedHandles = 0 ;
}
int allocHandle ( )
{
btAssert ( m_firstFreeHandle > = 0 ) ;
int handle = m_firstFreeHandle ;
m_firstFreeHandle = getHandle ( handle ) - > GetNextFree ( ) ;
m_numUsedHandles + + ;
if ( m_firstFreeHandle < 0 )
{
int curCapacity = m_bodyHandles . size ( ) ;
int additionalCapacity = m_bodyHandles . size ( ) ;
increaseHandleCapacity ( additionalCapacity ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
getHandle ( handle ) - > SetNextFree ( m_firstFreeHandle ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return handle ;
}
void freeHandle ( int handle )
{
btAssert ( handle > = 0 ) ;
getHandle ( handle ) - > SetNextFree ( m_firstFreeHandle ) ;
m_firstFreeHandle = handle ;
m_numUsedHandles - - ;
}
///end handle management
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
CommandLogger * m_commandLogger ;
CommandLogPlayback * m_logPlayback ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btScalar m_physicsDeltaTime ;
btAlignedObjectArray < btMultiBodyJointFeedback * > m_multiBodyJointFeedbacks ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btAlignedObjectArray < btBulletWorldImporter * > m_worldImporters ;
btAlignedObjectArray < UrdfLinkNameMapUtil * > m_urdfLinkNameMapper ;
btAlignedObjectArray < std : : string * > m_strings ;
btAlignedObjectArray < btCollisionShape * > m_collisionShapes ;
btBroadphaseInterface * m_broadphase ;
btCollisionDispatcher * m_dispatcher ;
btMultiBodyConstraintSolver * m_solver ;
btDefaultCollisionConfiguration * m_collisionConfiguration ;
btMultiBodyDynamicsWorld * m_dynamicsWorld ;
SharedMemoryDebugDrawer * m_remoteDebugDrawer ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
btAlignedObjectArray < int > m_sdfRecentLoadedBodies ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
struct GUIHelperInterface * m_guiHelper ;
int m_sharedMemoryKey ;
bool m_verboseOutput ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//data for picking objects
class btRigidBody * m_pickedBody ;
class btTypedConstraint * m_pickedConstraint ;
class btMultiBodyPoint2Point * m_pickingMultiBodyPoint2Point ;
btVector3 m_oldPickingPos ;
btVector3 m_hitPos ;
btScalar m_oldPickingDist ;
bool m_prevCanSleep ;
2016-06-01 05:55:13 +00:00
TinyRendererVisualShapeConverter m_visualConverter ;
2015-11-23 04:50:32 +00:00
PhysicsServerCommandProcessorInternalData ( )
:
m_commandLogger ( 0 ) ,
m_logPlayback ( 0 ) ,
m_physicsDeltaTime ( 1. / 240. ) ,
m_dynamicsWorld ( 0 ) ,
m_remoteDebugDrawer ( 0 ) ,
m_guiHelper ( 0 ) ,
m_sharedMemoryKey ( SHARED_MEMORY_KEY ) ,
m_verboseOutput ( false ) ,
m_pickedBody ( 0 ) ,
m_pickedConstraint ( 0 ) ,
m_pickingMultiBodyPoint2Point ( 0 )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
initHandles ( ) ;
#if 0
btAlignedObjectArray < int > bla ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
bla . resize ( 0 ) ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
bla . resize ( 0 ) ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
# endif
}
} ;
void PhysicsServerCommandProcessor : : setGuiHelper ( struct GUIHelperInterface * guiHelper )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( guiHelper )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
guiHelper - > createPhysicsDebugDrawer ( m_data - > m_dynamicsWorld ) ;
} else
{
if ( m_data - > m_guiHelper & & m_data - > m_dynamicsWorld & & m_data - > m_dynamicsWorld - > getDebugDrawer ( ) )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > setDebugDrawer ( 0 ) ;
}
}
m_data - > m_guiHelper = guiHelper ;
}
PhysicsServerCommandProcessor : : PhysicsServerCommandProcessor ( )
{
m_data = new PhysicsServerCommandProcessorInternalData ( ) ;
createEmptyDynamicsWorld ( ) ;
}
PhysicsServerCommandProcessor : : ~ PhysicsServerCommandProcessor ( )
{
deleteDynamicsWorld ( ) ;
if ( m_data - > m_commandLogger )
{
delete m_data - > m_commandLogger ;
m_data - > m_commandLogger = 0 ;
}
delete m_data ;
}
void PhysicsServerCommandProcessor : : createEmptyDynamicsWorld ( )
{
///collision configuration contains default setup for memory, collision setup
m_data - > m_collisionConfiguration = new btDefaultCollisionConfiguration ( ) ;
//m_collisionConfiguration->setConvexConvexMultipointIterations();
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data - > m_dispatcher = new btCollisionDispatcher ( m_data - > m_collisionConfiguration ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_broadphase = new btDbvtBroadphase ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_solver = new btMultiBodyConstraintSolver ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld = new btMultiBodyDynamicsWorld ( m_data - > m_dispatcher , m_data - > m_broadphase , m_data - > m_solver , m_data - > m_collisionConfiguration ) ;
m_data - > m_remoteDebugDrawer = new SharedMemoryDebugDrawer ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > setGravity ( btVector3 ( 0 , 0 , 0 ) ) ;
}
void PhysicsServerCommandProcessor : : deleteDynamicsWorld ( )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_multiBodyJointFeedbacks . size ( ) ; i + + )
{
delete m_data - > m_multiBodyJointFeedbacks [ i ] ;
}
m_data - > m_multiBodyJointFeedbacks . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_worldImporters . size ( ) ; i + + )
{
delete m_data - > m_worldImporters [ i ] ;
}
m_data - > m_worldImporters . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_urdfLinkNameMapper . size ( ) ; i + + )
{
delete m_data - > m_urdfLinkNameMapper [ i ] ;
}
m_data - > m_urdfLinkNameMapper . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_strings . size ( ) ; i + + )
{
delete m_data - > m_strings [ i ] ;
}
m_data - > m_strings . clear ( ) ;
2016-04-11 23:42:02 +00:00
btAlignedObjectArray < btTypedConstraint * > constraints ;
btAlignedObjectArray < btMultiBodyConstraint * > mbconstraints ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_dynamicsWorld )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int i ;
for ( i = m_data - > m_dynamicsWorld - > getNumConstraints ( ) - 1 ; i > = 0 ; i - - )
{
2016-04-11 23:42:02 +00:00
btTypedConstraint * constraint = m_data - > m_dynamicsWorld - > getConstraint ( i ) ;
constraints . push_back ( constraint ) ;
m_data - > m_dynamicsWorld - > removeConstraint ( constraint ) ;
2015-11-23 04:50:32 +00:00
}
2016-04-11 23:42:02 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumMultiBodyConstraints ( ) - 1 ; i > = 0 ; i - - )
{
btMultiBodyConstraint * mbconstraint = m_data - > m_dynamicsWorld - > getMultiBodyConstraint ( i ) ;
mbconstraints . push_back ( mbconstraint ) ;
m_data - > m_dynamicsWorld - > removeMultiBodyConstraint ( mbconstraint ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumCollisionObjects ( ) - 1 ; i > = 0 ; i - - )
{
btCollisionObject * obj = m_data - > m_dynamicsWorld - > getCollisionObjectArray ( ) [ i ] ;
btRigidBody * body = btRigidBody : : upcast ( obj ) ;
if ( body & & body - > getMotionState ( ) )
{
delete body - > getMotionState ( ) ;
}
m_data - > m_dynamicsWorld - > removeCollisionObject ( obj ) ;
delete obj ;
}
2016-04-11 23:42:02 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumMultibodies ( ) - 1 ; i > = 0 ; i - - )
{
btMultiBody * mb = m_data - > m_dynamicsWorld - > getMultiBody ( i ) ;
m_data - > m_dynamicsWorld - > removeMultiBody ( mb ) ;
delete mb ;
}
2015-11-23 04:50:32 +00:00
}
2016-04-11 23:42:02 +00:00
for ( int i = 0 ; i < constraints . size ( ) ; i + + )
{
delete constraints [ i ] ;
}
constraints . clear ( ) ;
for ( int i = 0 ; i < mbconstraints . size ( ) ; i + + )
{
delete mbconstraints [ i ] ;
}
mbconstraints . clear ( ) ;
//delete collision shapes
2015-11-23 04:50:32 +00:00
for ( int j = 0 ; j < m_data - > m_collisionShapes . size ( ) ; j + + )
{
btCollisionShape * shape = m_data - > m_collisionShapes [ j ] ;
delete shape ;
}
m_data - > m_collisionShapes . clear ( ) ;
delete m_data - > m_dynamicsWorld ;
m_data - > m_dynamicsWorld = 0 ;
delete m_data - > m_remoteDebugDrawer ;
m_data - > m_remoteDebugDrawer = 0 ;
delete m_data - > m_solver ;
m_data - > m_solver = 0 ;
delete m_data - > m_broadphase ;
m_data - > m_broadphase = 0 ;
delete m_data - > m_dispatcher ;
m_data - > m_dispatcher = 0 ;
delete m_data - > m_collisionConfiguration ;
m_data - > m_collisionConfiguration = 0 ;
}
bool PhysicsServerCommandProcessor : : supportsJointMotor ( btMultiBody * mb , int mbLinkIndex )
{
bool canHaveMotor = ( mb - > getLink ( mbLinkIndex ) . m_jointType = = btMultibodyLink : : eRevolute
| | mb - > getLink ( mbLinkIndex ) . m_jointType = = btMultibodyLink : : ePrismatic ) ;
return canHaveMotor ;
}
//for testing, create joint motors for revolute and prismatic joints
void PhysicsServerCommandProcessor : : createJointMotors ( btMultiBody * mb )
{
int numLinks = mb - > getNumLinks ( ) ;
for ( int i = 0 ; i < numLinks ; i + + )
{
int mbLinkIndex = i ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( supportsJointMotor ( mb , mbLinkIndex ) )
{
float maxMotorImpulse = 0.f ;
int dof = 0 ;
btScalar desiredVelocity = 0.f ;
btMultiBodyJointMotor * motor = new btMultiBodyJointMotor ( mb , mbLinkIndex , dof , desiredVelocity , maxMotorImpulse ) ;
//motor->setMaxAppliedImpulse(0);
2016-06-17 01:46:34 +00:00
mb - > getLink ( mbLinkIndex ) . m_userPtr = motor ;
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( motor ) ;
2016-04-09 01:17:17 +00:00
motor - > finalizeMultiDof ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
}
}
2016-06-13 17:11:28 +00:00
bool PhysicsServerCommandProcessor : : loadSdf ( const char * fileName , char * bufferServerToClient , int bufferSizeInBytes )
{
btAssert ( m_data - > m_dynamicsWorld ) ;
if ( ! m_data - > m_dynamicsWorld )
{
b3Error ( " loadSdf: No valid m_dynamicsWorld " ) ;
return false ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , & m_data - > m_visualConverter ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
bool useFixedBase = false ;
bool loadOk = u2b . loadSDF ( fileName , useFixedBase ) ;
if ( loadOk )
{
for ( int i = 0 ; i < u2b . getNumAllocatedCollisionShapes ( ) ; i + + )
{
btCollisionShape * shape = u2b . getAllocatedCollisionShape ( i ) ;
m_data - > m_collisionShapes . push_back ( shape ) ;
if ( shape - > isCompound ( ) )
{
btCompoundShape * compound = ( btCompoundShape * ) shape ;
for ( int childIndex = 0 ; childIndex < compound - > getNumChildShapes ( ) ; childIndex + + )
{
m_data - > m_collisionShapes . push_back ( compound - > getChildShape ( childIndex ) ) ;
}
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
btTransform rootTrans ;
rootTrans . setIdentity ( ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " loaded %s OK! " , fileName ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
for ( int m = 0 ; m < u2b . getNumModels ( ) ; m + + )
{
u2b . activateModel ( m ) ;
btMultiBody * mb = 0 ;
//get a body index
int bodyUniqueId = m_data - > allocHandle ( ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
{
btScalar mass = 0 ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
int urdfLinkIndex = u2b . getRootLinkIndex ( ) ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , bodyHandle - > m_rootLocalInertialFrame ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b . getRootLinkIndex ( ) ;
b3Printf ( " urdf root link index = %d \n " , rootLinkIndex ) ;
MyMultiBodyCreator creation ( m_data - > m_guiHelper ) ;
u2b . getRootTransformInWorld ( rootTrans ) ;
bool useMultiBody = true ;
ConvertURDF2Bullet ( u2b , creation , rootTrans , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) , true ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb = creation . getBulletMultiBody ( ) ;
if ( mb )
{
bodyHandle - > m_multiBody = mb ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
m_data - > m_sdfRecentLoadedBodies . push_back ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
createJointMotors ( mb ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
btScalar mass ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
btTransform localInertialFrame ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , localInertialFrame ) ;
bodyHandle - > m_linkLocalInertialFrames . push_back ( localInertialFrame ) ;
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb - > getLink ( i ) . m_linkName = linkName - > c_str ( ) ;
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb - > getLink ( i ) . m_jointName = jointName - > c_str ( ) ;
}
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
mb - > setBaseName ( baseName - > c_str ( ) ) ;
} else
{
b3Warning ( " No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later. " ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
}
}
2016-06-24 14:31:17 +00:00
return loadOk ;
2016-06-13 17:11:28 +00:00
}
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : loadUrdf ( const char * fileName , const btVector3 & pos , const btQuaternion & orn ,
bool useMultiBody , bool useFixedBase , int * bodyUniqueIdPtr , char * bufferServerToClient , int bufferSizeInBytes )
{
btAssert ( m_data - > m_dynamicsWorld ) ;
if ( ! m_data - > m_dynamicsWorld )
{
b3Error ( " loadUrdf: No valid m_dynamicsWorld " ) ;
return false ;
}
2016-05-20 01:37:15 +00:00
2016-06-24 14:31:17 +00:00
2016-06-01 05:55:13 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , & m_data - > m_visualConverter ) ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
bool loadOk = u2b . loadURDF ( fileName , useFixedBase ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( loadOk )
{
//get a body index
int bodyUniqueId = m_data - > allocHandle ( ) ;
if ( bodyUniqueIdPtr )
* bodyUniqueIdPtr = bodyUniqueId ;
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
{
btScalar mass = 0 ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
int urdfLinkIndex = u2b . getRootLinkIndex ( ) ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , bodyHandle - > m_rootLocalInertialFrame ) ;
}
if ( m_data - > m_verboseOutput )
{
b3Printf ( " loaded %s OK! " , fileName ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btTransform tr ;
tr . setIdentity ( ) ;
tr . setOrigin ( pos ) ;
tr . setRotation ( orn ) ;
//int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation ( m_data - > m_guiHelper ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
ConvertURDF2Bullet ( u2b , creation , tr , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) ) ;
2016-06-24 14:31:17 +00:00
2016-04-11 23:42:02 +00:00
for ( int i = 0 ; i < u2b . getNumAllocatedCollisionShapes ( ) ; i + + )
{
btCollisionShape * shape = u2b . getAllocatedCollisionShape ( i ) ;
m_data - > m_collisionShapes . push_back ( shape ) ;
if ( shape - > isCompound ( ) )
{
btCompoundShape * compound = ( btCompoundShape * ) shape ;
for ( int childIndex = 0 ; childIndex < compound - > getNumChildShapes ( ) ; childIndex + + )
{
m_data - > m_collisionShapes . push_back ( compound - > getChildShape ( childIndex ) ) ;
}
}
2016-06-24 14:31:17 +00:00
2016-04-11 23:42:02 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btMultiBody * mb = creation . getBulletMultiBody ( ) ;
if ( useMultiBody )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( mb )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
bodyHandle - > m_multiBody = mb ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
createJointMotors ( mb ) ;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil * util = new UrdfLinkNameMapUtil ;
m_data - > m_urdfLinkNameMapper . push_back ( util ) ;
util - > m_mb = mb ;
util - > m_memSerializer = new btDefaultSerializer ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getBaseCollider ( ) , 0 ) ;
2016-04-29 21:45:15 +00:00
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getLink ( i ) . m_collider , 0 ) ;
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
2016-04-29 21:45:15 +00:00
btScalar mass ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
btTransform localInertialFrame ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , localInertialFrame ) ;
bodyHandle - > m_linkLocalInertialFrames . push_back ( localInertialFrame ) ;
2015-11-23 04:50:32 +00:00
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
util - > m_memSerializer - > registerNameForPointer ( linkName - > c_str ( ) , linkName - > c_str ( ) ) ;
mb - > getLink ( i ) . m_linkName = linkName - > c_str ( ) ;
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
util - > m_memSerializer - > registerNameForPointer ( jointName - > c_str ( ) , jointName - > c_str ( ) ) ;
mb - > getLink ( i ) . m_jointName = jointName - > c_str ( ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
util - > m_memSerializer - > registerNameForPointer ( baseName - > c_str ( ) , baseName - > c_str ( ) ) ;
mb - > setBaseName ( baseName - > c_str ( ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
util - > m_memSerializer - > insertHeader ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int len = mb - > calculateSerializeBufferSize ( ) ;
btChunk * chunk = util - > m_memSerializer - > allocate ( len , 1 ) ;
const char * structType = mb - > serialize ( chunk - > m_oldPtr , util - > m_memSerializer ) ;
util - > m_memSerializer - > finalizeChunk ( chunk , structType , BT_MULTIBODY_CODE , mb ) ;
2016-06-13 17:11:28 +00:00
return true ;
2015-11-23 04:50:32 +00:00
} else
{
b3Warning ( " No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later. " ) ;
return false ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
btAssert ( 0 ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return true ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return false ;
}
2016-04-09 01:17:17 +00:00
void PhysicsServerCommandProcessor : : replayLogCommand ( char * bufferServerToClient , int bufferSizeInBytes )
{
if ( m_data - > m_logPlayback )
{
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
SharedMemoryCommand clientCmd ;
SharedMemoryStatus serverStatus ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
bool hasCommand = m_data - > m_logPlayback - > processNextCommand ( & clientCmd ) ;
if ( hasCommand )
{
processCommand ( clientCmd , serverStatus , bufferServerToClient , bufferSizeInBytes ) ;
}
}
}
2015-11-23 04:50:32 +00:00
2016-06-15 01:41:19 +00:00
int PhysicsServerCommandProcessor : : createBodyInfoStream ( int bodyUniqueId , char * bufferServerToClient , int bufferSizeInBytes )
{
int streamSizeInBytes = 0 ;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
btMultiBody * mb = bodyHandle - > m_multiBody ;
if ( mb )
{
UrdfLinkNameMapUtil * util = new UrdfLinkNameMapUtil ;
m_data - > m_urdfLinkNameMapper . push_back ( util ) ;
util - > m_mb = mb ;
util - > m_memSerializer = new btDefaultSerializer ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getBaseCollider ( ) , 0 ) ;
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getLink ( i ) . m_collider , 0 ) ;
util - > m_memSerializer - > registerNameForPointer ( mb - > getLink ( i ) . m_linkName , mb - > getLink ( i ) . m_linkName ) ;
util - > m_memSerializer - > registerNameForPointer ( mb - > getLink ( i ) . m_jointName , mb - > getLink ( i ) . m_jointName ) ;
}
util - > m_memSerializer - > registerNameForPointer ( mb - > getBaseName ( ) , mb - > getBaseName ( ) ) ;
2016-06-24 14:31:17 +00:00
2016-06-15 01:41:19 +00:00
util - > m_memSerializer - > insertHeader ( ) ;
int len = mb - > calculateSerializeBufferSize ( ) ;
btChunk * chunk = util - > m_memSerializer - > allocate ( len , 1 ) ;
const char * structType = mb - > serialize ( chunk - > m_oldPtr , util - > m_memSerializer ) ;
util - > m_memSerializer - > finalizeChunk ( chunk , structType , BT_MULTIBODY_CODE , mb ) ;
streamSizeInBytes = util - > m_memSerializer - > getCurrentBufferSize ( ) ;
2015-11-23 04:50:32 +00:00
2016-06-15 01:41:19 +00:00
}
return streamSizeInBytes ;
}
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : processCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = false ;
{
///we ignore overflow of integer for now
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
2016-04-09 01:17:17 +00:00
# if 1
2015-11-23 04:50:32 +00:00
if ( m_data - > m_commandLogger )
{
2016-04-09 01:17:17 +00:00
m_data - > m_commandLogger - > logCommand ( clientCmd ) ;
2015-11-23 04:50:32 +00:00
}
# endif
//m_data->m_testBlock1->m_numProcessedClientCommands++;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//no timestamp yet
int timeStamp = 0 ;
//consume the command
switch ( clientCmd . m_type )
{
#if 0
case CMD_SEND_BULLET_DATA_STREAM :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_SEND_BULLET_DATA_STREAM length %d " , clientCmd . m_dataStreamArguments . m_streamChunkLength ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btBulletWorldImporter * worldImporter = new btBulletWorldImporter ( m_data - > m_dynamicsWorld ) ;
m_data - > m_worldImporters . push_back ( worldImporter ) ;
bool completedOk = worldImporter - > loadFileFromMemory ( m_data - > m_testBlock1 - > m_bulletStreamDataClientToServer , clientCmd . m_dataStreamArguments . m_streamChunkLength ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( completedOk )
{
SharedMemoryStatus & status = m_data - > createServerStatus ( CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED , clientCmd . m_sequenceNumber , timeStamp ) ;
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
m_data - > submitServerStatus ( status ) ;
} else
{
SharedMemoryStatus & status = m_data - > createServerStatus ( CMD_BULLET_DATA_STREAM_RECEIVED_FAILED , clientCmd . m_sequenceNumber , timeStamp ) ;
m_data - > submitServerStatus ( status ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
# endif
case CMD_REQUEST_DEBUG_LINES :
{
int curFlags = m_data - > m_remoteDebugDrawer - > getDebugMode ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int debugMode = clientCmd . m_requestDebugLinesArguments . m_debugMode ; //clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
int startingLineIndex = clientCmd . m_requestDebugLinesArguments . m_startingLineIndex ;
if ( startingLineIndex < 0 )
{
b3Warning ( " startingLineIndex should be non-negative " ) ;
startingLineIndex = 0 ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( clientCmd . m_requestDebugLinesArguments . m_startingLineIndex = = 0 )
{
m_data - > m_remoteDebugDrawer - > m_lines2 . resize ( 0 ) ;
//|btIDebugDraw::DBG_DrawAabb|
// btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ;
m_data - > m_remoteDebugDrawer - > setDebugMode ( debugMode ) ;
btIDebugDraw * oldDebugDrawer = m_data - > m_dynamicsWorld - > getDebugDrawer ( ) ;
m_data - > m_dynamicsWorld - > setDebugDrawer ( m_data - > m_remoteDebugDrawer ) ;
m_data - > m_dynamicsWorld - > debugDrawWorld ( ) ;
m_data - > m_dynamicsWorld - > setDebugDrawer ( oldDebugDrawer ) ;
m_data - > m_remoteDebugDrawer - > setDebugMode ( curFlags ) ;
}
//9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color'
int maxNumLines = bufferSizeInBytes / ( sizeof ( float ) * 9 ) - 1 ;
if ( startingLineIndex > m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) )
{
b3Warning ( " m_startingLineIndex exceeds total number of debug lines " ) ;
startingLineIndex = m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int numLines = btMin ( maxNumLines , m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) - startingLineIndex ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( numLines )
{
float * linesFrom = ( float * ) bufferServerToClient ;
float * linesTo = ( float * ) ( bufferServerToClient + numLines * 3 * sizeof ( float ) ) ;
float * linesColor = ( float * ) ( bufferServerToClient + 2 * numLines * 3 * sizeof ( float ) ) ;
for ( int i = 0 ; i < numLines ; i + + )
{
linesFrom [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . x ( ) ;
linesTo [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . x ( ) ;
linesColor [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . x ( ) ;
linesFrom [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . y ( ) ;
linesTo [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . y ( ) ;
linesColor [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . y ( ) ;
linesFrom [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . z ( ) ;
linesTo [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . z ( ) ;
linesColor [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . z ( ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_DEBUG_LINES_COMPLETED ;
serverStatusOut . m_sendDebugLinesArgs . m_numDebugLines = numLines ;
serverStatusOut . m_sendDebugLinesArgs . m_startingLineIndex = startingLineIndex ;
serverStatusOut . m_sendDebugLinesArgs . m_numRemainingDebugLines = m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) - ( startingLineIndex + numLines ) ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
2016-05-18 06:57:19 +00:00
case CMD_REQUEST_CAMERA_IMAGE_DATA :
{
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
int startPixelIndex = clientCmd . m_requestPixelDataArguments . m_startPixelIndex ;
int width , height ;
int numPixelsCopied = 0 ;
2016-06-24 14:31:17 +00:00
2016-06-07 23:11:58 +00:00
if (
2016-06-24 14:31:17 +00:00
( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 ) & &
2016-06-07 23:11:58 +00:00
( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT ) ! = 0 )
{
m_data - > m_visualConverter . setWidthAndHeight ( clientCmd . m_requestPixelDataArguments . m_pixelWidth ,
clientCmd . m_requestPixelDataArguments . m_pixelHeight ) ;
2016-06-24 14:31:17 +00:00
}
2016-06-01 18:04:10 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL ) ! = 0 )
2016-06-01 05:55:13 +00:00
{
2016-06-01 18:04:10 +00:00
m_data - > m_guiHelper - > copyCameraImageData ( 0 , 0 , 0 , 0 , 0 , & width , & height , 0 ) ;
2016-06-24 14:31:17 +00:00
}
2016-06-02 00:47:41 +00:00
else
2016-06-01 18:04:10 +00:00
{
2016-06-02 00:47:41 +00:00
m_data - > m_visualConverter . getWidthAndHeight ( width , height ) ;
2016-06-01 05:55:13 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
int numTotalPixels = width * height ;
int numRemainingPixels = numTotalPixels - startPixelIndex ;
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
if ( numRemainingPixels > 0 )
{
int maxNumPixels = bufferSizeInBytes / 8 - 1 ;
unsigned char * pixelRGBA = ( unsigned char * ) bufferServerToClient ;
int numRequestedPixels = btMin ( maxNumPixels , numRemainingPixels ) ;
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
float * depthBuffer = ( float * ) ( bufferServerToClient + numRequestedPixels * 4 ) ;
2016-06-24 14:31:17 +00:00
2016-06-01 18:04:10 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL ) ! = 0 )
{
m_data - > m_guiHelper - > copyCameraImageData ( pixelRGBA , numRequestedPixels , depthBuffer , numRequestedPixels , startPixelIndex , & width , & height , & numPixelsCopied ) ;
} else
{
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
if ( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 )
{
2016-06-07 01:54:05 +00:00
// printf("-------------------------------\nRendering\n");
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ) ! = 0 )
{
m_data - > m_visualConverter . render (
clientCmd . m_requestPixelDataArguments . m_viewMatrix ,
clientCmd . m_requestPixelDataArguments . m_projectionMatrix ) ;
} else
{
m_data - > m_visualConverter . render ( ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
}
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
m_data - > m_visualConverter . copyCameraImageData ( pixelRGBA , numRequestedPixels , depthBuffer , numRequestedPixels , startPixelIndex , & width , & height , & numPixelsCopied ) ;
2016-06-01 18:04:10 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
//each pixel takes 4 RGBA values and 1 float = 8 bytes
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
} else
{
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
serverStatusOut . m_type = CMD_CAMERA_IMAGE_COMPLETED ;
serverStatusOut . m_sendPixelDataArguments . m_numPixelsCopied = numPixelsCopied ;
serverStatusOut . m_sendPixelDataArguments . m_numRemainingPixels = numRemainingPixels - numPixelsCopied ;
serverStatusOut . m_sendPixelDataArguments . m_startingPixelIndex = startPixelIndex ;
serverStatusOut . m_sendPixelDataArguments . m_imageWidth = width ;
serverStatusOut . m_sendPixelDataArguments . m_imageHeight = height ;
2016-05-18 06:57:19 +00:00
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2016-05-18 06:57:19 +00:00
break ;
}
2016-06-15 01:41:19 +00:00
case CMD_REQUEST_BODY_INFO :
{
const SdfRequestInfoArgs & sdfInfoArgs = clientCmd . m_sdfRequestInfoArgs ;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream ( sdfInfoArgs . m_bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2016-06-24 14:31:17 +00:00
2016-06-15 01:41:19 +00:00
serverStatusOut . m_type = CMD_BODY_INFO_COMPLETED ;
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = sdfInfoArgs . m_bodyUniqueId ;
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = streamSizeInBytes ;
hasStatus = true ;
break ;
}
2016-06-13 17:11:28 +00:00
case CMD_LOAD_SDF :
{
const SdfArgs & sdfArgs = clientCmd . m_sdfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_SDF:%s " , sdfArgs . m_sdfFileName ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
bool completedOk = loadSdf ( sdfArgs . m_sdfFileName , bufferServerToClient , bufferSizeInBytes ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut . m_sdfLoadedArgs . m_numBodies = m_data - > m_sdfRecentLoadedBodies . size ( ) ;
int maxBodies = btMin ( MAX_SDF_BODIES , serverStatusOut . m_sdfLoadedArgs . m_numBodies ) ;
for ( int i = 0 ; i < maxBodies ; i + + )
{
serverStatusOut . m_sdfLoadedArgs . m_bodyUniqueIds [ i ] = m_data - > m_sdfRecentLoadedBodies [ i ] ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
serverStatusOut . m_type = CMD_SDF_LOADING_COMPLETED ;
hasStatus = true ;
break ;
}
2015-11-23 04:50:32 +00:00
case CMD_LOAD_URDF :
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
const UrdfArgs & urdfArgs = clientCmd . m_urdfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_URDF:%s " , urdfArgs . m_urdfFileName ) ;
}
btAssert ( ( clientCmd . m_updateFlags & URDF_ARGS_FILE_NAME ) ! = 0 ) ;
btAssert ( urdfArgs . m_urdfFileName ) ;
btVector3 initialPos ( 0 , 0 , 0 ) ;
btQuaternion initialOrn ( 0 , 0 , 0 , 1 ) ;
if ( clientCmd . m_updateFlags & URDF_ARGS_INITIAL_POSITION )
{
initialPos [ 0 ] = urdfArgs . m_initialPosition [ 0 ] ;
initialPos [ 1 ] = urdfArgs . m_initialPosition [ 1 ] ;
initialPos [ 2 ] = urdfArgs . m_initialPosition [ 2 ] ;
}
if ( clientCmd . m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION )
{
initialOrn [ 0 ] = urdfArgs . m_initialOrientation [ 0 ] ;
initialOrn [ 1 ] = urdfArgs . m_initialOrientation [ 1 ] ;
initialOrn [ 2 ] = urdfArgs . m_initialOrientation [ 2 ] ;
initialOrn [ 3 ] = urdfArgs . m_initialOrientation [ 3 ] ;
}
bool useMultiBody = ( clientCmd . m_updateFlags & URDF_ARGS_USE_MULTIBODY ) ? urdfArgs . m_useMultiBody : true ;
bool useFixedBase = ( clientCmd . m_updateFlags & URDF_ARGS_USE_FIXED_BASE ) ? urdfArgs . m_useFixedBase : false ;
int bodyUniqueId ;
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf ( urdfArgs . m_urdfFileName ,
initialPos , initialOrn ,
useMultiBody , useFixedBase , & bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( completedOk )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_URDF_LOADING_COMPLETED ;
2016-04-11 23:42:02 +00:00
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_urdfLinkNameMapper . size ( ) )
{
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = m_data - > m_urdfLinkNameMapper . at ( m_data - > m_urdfLinkNameMapper . size ( ) - 1 ) - > m_memSerializer - > getCurrentBufferSize ( ) ;
}
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = bodyUniqueId ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
serverStatusOut . m_type = CMD_URDF_LOADING_FAILED ;
hasStatus = true ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
case CMD_CREATE_SENSOR :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_CREATE_SENSOR " ) ;
}
int bodyUniqueId = clientCmd . m_createSensorArguments . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btAssert ( mb ) ;
for ( int i = 0 ; i < clientCmd . m_createSensorArguments . m_numJointSensorChanges ; i + + )
{
int jointIndex = clientCmd . m_createSensorArguments . m_jointIndex [ i ] ;
if ( clientCmd . m_createSensorArguments . m_enableJointForceSensor [ i ] )
{
if ( mb - > getLink ( jointIndex ) . m_jointFeedback )
{
b3Warning ( " CMD_CREATE_SENSOR: sensor for joint [%d] already enabled " , jointIndex ) ;
} else
{
btMultiBodyJointFeedback * fb = new btMultiBodyJointFeedback ( ) ;
fb - > m_reactionForces . setZero ( ) ;
mb - > getLink ( jointIndex ) . m_jointFeedback = fb ;
m_data - > m_multiBodyJointFeedbacks . push_back ( fb ) ;
} ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
if ( mb - > getLink ( jointIndex ) . m_jointFeedback )
{
m_data - > m_multiBodyJointFeedbacks . remove ( mb - > getLink ( jointIndex ) . m_jointFeedback ) ;
delete mb - > getLink ( jointIndex ) . m_jointFeedback ;
mb - > getLink ( jointIndex ) . m_jointFeedback = 0 ;
} else
{
b3Warning ( " CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d] " , jointIndex ) ;
} ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
b3Warning ( " No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet " ) ;
}
#if 0
//todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
/*
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getNumConstraints ( ) ; i + + )
{
btTypedConstraint * c = m_data - > m_dynamicsWorld - > getConstraint ( i ) ;
btJointFeedback * fb = new btJointFeedback ( ) ;
m_data - > m_jointFeedbacks . push_back ( fb ) ;
c - > setJointFeedback ( fb ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
*/
# endif
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
case CMD_SEND_DESIRED_STATE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_SEND_DESIRED_STATE " ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int bodyUniqueId = clientCmd . m_sendDesiredStateCommandArgument . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btAssert ( mb ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
switch ( clientCmd . m_sendDesiredStateCommandArgument . m_controlMode )
{
case CONTROL_MODE_TORQUE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_TORQUE " ) ;
}
2016-06-24 14:31:17 +00:00
// mb->clearForcesAndTorques();
2015-11-23 04:50:32 +00:00
int torqueIndex = 0 ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
for ( int dof = 0 ; dof < mb - > getLink ( link ) . m_dofCount ; dof + + )
{
double torque = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ torqueIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
torque = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ torqueIndex ] ;
mb - > addJointTorqueMultiDof ( link , dof , torque ) ;
}
torqueIndex + + ;
}
2015-11-23 04:50:32 +00:00
}
}
break ;
}
case CONTROL_MODE_VELOCITY :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_VELOCITY " ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int dofIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom entries of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
if ( supportsJointMotor ( mb , link ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor )
2015-11-23 04:50:32 +00:00
{
2016-06-04 02:03:56 +00:00
btScalar desiredVelocity = 0.f ;
2016-06-24 14:31:17 +00:00
bool hasDesiredVelocity = false ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ dofIndex ] ;
2016-06-24 18:06:56 +00:00
btScalar kd = 0.1f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ dofIndex ] ;
}
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
btScalar kp = 0.f ;
motor - > setPositionTarget ( 0 , kp ) ;
2016-06-24 14:31:17 +00:00
hasDesiredVelocity = true ;
}
if ( hasDesiredVelocity )
{
btScalar maxImp = 1000000.f * m_data - > m_physicsDeltaTime ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ dofIndex ] * m_data - > m_physicsDeltaTime ;
}
motor - > setMaxAppliedImpulse ( maxImp ) ;
}
2015-11-23 04:50:32 +00:00
numMotors + + ;
}
}
dofIndex + = mb - > getLink ( link ) . m_dofCount ;
}
}
break ;
}
2016-06-04 02:03:56 +00:00
2015-11-23 04:50:32 +00:00
case CONTROL_MODE_POSITION_VELOCITY_PD :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_POSITION_VELOCITY_PD " ) ;
}
//compute the force base on PD control
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7 ; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
if ( supportsJointMotor ( mb , link ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor )
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
bool hasDesiredPosOrVel = false ;
btScalar kp = 0.f ;
btScalar kd = 0.f ;
2016-06-04 02:03:56 +00:00
btScalar desiredVelocity = 0.f ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
hasDesiredPosOrVel = true ;
2016-06-04 02:03:56 +00:00
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ velIndex ] ;
2016-06-24 14:31:17 +00:00
kd = 0.1 ;
}
2016-06-04 02:03:56 +00:00
btScalar desiredPosition = 0.f ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ posIndex ] & SIM_DESIRED_STATE_HAS_Q ) ! = 0 )
{
hasDesiredPosOrVel = true ;
2016-06-04 02:03:56 +00:00
desiredPosition = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] ;
2016-06-24 14:31:17 +00:00
kp = 0.1 ;
}
2016-06-04 02:03:56 +00:00
2016-06-24 14:31:17 +00:00
if ( hasDesiredPosOrVel )
{
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KP ) ! = 0 )
{
kp = clientCmd . m_sendDesiredStateCommandArgument . m_Kp [ velIndex ] ;
}
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ velIndex ] ;
}
2016-06-24 18:06:56 +00:00
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
motor - > setPositionTarget ( desiredPosition , kp ) ;
2016-06-24 14:31:17 +00:00
btScalar maxImp = 1000000.f * m_data - > m_physicsDeltaTime ;
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ velIndex ] * m_data - > m_physicsDeltaTime ;
motor - > setMaxAppliedImpulse ( maxImp ) ;
}
2015-11-23 04:50:32 +00:00
numMotors + + ;
}
}
velIndex + = mb - > getLink ( link ) . m_dofCount ;
posIndex + = mb - > getLink ( link ) . m_posVarCount ;
}
}
break ;
}
default :
{
b3Warning ( " m_controlMode not implemented yet " ) ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_REQUEST_ACTUAL_STATE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Sending the actual state (Q,U) " ) ;
}
int bodyUniqueId = clientCmd . m_requestActualStateInformationCommandArgument . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED ;
serverCmd . m_sendActualStateArgs . m_bodyUniqueId = bodyUniqueId ;
int totalDegreeOfFreedomQ = 0 ;
2016-06-24 14:31:17 +00:00
int totalDegreeOfFreedomU = 0 ;
2015-11-23 04:50:32 +00:00
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
btTransform tr ;
tr . setOrigin ( mb - > getBasePos ( ) ) ;
tr . setRotation ( mb - > getWorldToBaseRot ( ) . inverse ( ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 0 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 1 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 2 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 3 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 4 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 5 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 6 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 3 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base position in world space, carthesian
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 0 ] = tr . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 1 ] = tr . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 2 ] = tr . getOrigin ( ) [ 2 ] ;
//base orientation, quaternion x,y,z,w, in world space, carthesian
2016-06-24 14:31:17 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 4 ] = tr . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 5 ] = tr . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 6 ] = tr . getRotation ( ) [ 3 ] ;
totalDegreeOfFreedomQ + = 7 ; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 0 ] = mb - > getBaseVel ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 1 ] = mb - > getBaseVel ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 2 ] = mb - > getBaseVel ( ) [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base angular velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 3 ] = mb - > getBaseOmega ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 4 ] = mb - > getBaseOmega ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 5 ] = mb - > getBaseOmega ( ) [ 2 ] ;
totalDegreeOfFreedomU + = 6 ; //3 linear and 3 angular DOF
}
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + )
{
for ( int d = 0 ; d < mb - > getLink ( l ) . m_posVarCount ; d + + )
{
serverCmd . m_sendActualStateArgs . m_actualStateQ [ totalDegreeOfFreedomQ + + ] = mb - > getJointPosMultiDof ( l ) [ d ] ;
}
for ( int d = 0 ; d < mb - > getLink ( l ) . m_dofCount ; d + + )
{
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ totalDegreeOfFreedomU + + ] = mb - > getJointVelMultiDof ( l ) [ d ] ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( 0 = = mb - > getLink ( l ) . m_jointFeedback )
{
for ( int d = 0 ; d < 6 ; d + + )
{
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + d ] = 0 ;
}
} else
{
btVector3 sensedForce = mb - > getLink ( l ) . m_jointFeedback - > m_reactionForces . getLinear ( ) ;
btVector3 sensedTorque = mb - > getLink ( l ) . m_jointFeedback - > m_reactionForces . getAngular ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 0 ] = sensedForce [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 1 ] = sensedForce [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 2 ] = sensedForce [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 3 ] = sensedTorque [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 4 ] = sensedTorque [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 5 ] = sensedTorque [ 2 ] ;
}
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
serverCmd . m_sendActualStateArgs . m_jointMotorForce [ l ] = 0 ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
if ( supportsJointMotor ( mb , l ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) body - > m_multiBody - > getLink ( l ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor & & m_data - > m_physicsDeltaTime > btScalar ( 0 ) )
2016-04-09 01:17:17 +00:00
{
btScalar force = motor - > getAppliedImpulse ( 0 ) / m_data - > m_physicsDeltaTime ;
serverCmd . m_sendActualStateArgs . m_jointMotorForce [ l ] =
force ;
//if (force>0)
//{
// b3Printf("force = %f\n", force);
//}
}
}
2016-04-29 21:45:15 +00:00
btVector3 linkLocalInertialOrigin = body - > m_linkLocalInertialFrames [ l ] . getOrigin ( ) ;
btQuaternion linkLocalInertialRotation = body - > m_linkLocalInertialFrames [ l ] . getRotation ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-24 00:29:46 +00:00
btVector3 linkOrigin = mb - > getLink ( l ) . m_cachedWorldTransform . getOrigin ( ) ;
btQuaternion linkRotation = mb - > getLink ( l ) . m_cachedWorldTransform . getRotation ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-24 00:29:46 +00:00
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 0 ] = linkOrigin . getX ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 1 ] = linkOrigin . getY ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 2 ] = linkOrigin . getZ ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 3 ] = linkRotation . x ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 4 ] = linkRotation . y ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 5 ] = linkRotation . z ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 6 ] = linkRotation . w ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-29 21:45:15 +00:00
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 0 ] = linkLocalInertialOrigin . getX ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 1 ] = linkLocalInertialOrigin . getY ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 2 ] = linkLocalInertialOrigin . getZ ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-29 21:45:15 +00:00
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 3 ] = linkLocalInertialRotation . x ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 4 ] = linkLocalInertialRotation . y ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 5 ] = linkLocalInertialRotation . z ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 6 ] = linkLocalInertialRotation . w ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
}
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ ;
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomU = totalDegreeOfFreedomU ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
if ( body & & body - > m_rigidBody )
{
btRigidBody * rb = body - > m_rigidBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED ;
serverCmd . m_sendActualStateArgs . m_bodyUniqueId = bodyUniqueId ;
int totalDegreeOfFreedomQ = 0 ;
int totalDegreeOfFreedomU = 0 ;
btTransform tr = rb - > getWorldTransform ( ) ;
//base position in world space, carthesian
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 0 ] = tr . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 1 ] = tr . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 2 ] = tr . getOrigin ( ) [ 2 ] ;
//base orientation, quaternion x,y,z,w, in world space, carthesian
2016-06-24 14:31:17 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 4 ] = tr . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 5 ] = tr . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 6 ] = tr . getRotation ( ) [ 3 ] ;
totalDegreeOfFreedomQ + = 7 ; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 0 ] = rb - > getLinearVelocity ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 1 ] = rb - > getLinearVelocity ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 2 ] = rb - > getLinearVelocity ( ) [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base angular velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 3 ] = rb - > getAngularVelocity ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 4 ] = rb - > getAngularVelocity ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 5 ] = rb - > getAngularVelocity ( ) [ 2 ] ;
totalDegreeOfFreedomU + = 6 ; //3 linear and 3 angular DOF
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ ;
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomU = totalDegreeOfFreedomU ;
hasStatus = true ;
} else
{
b3Warning ( " Request state but no multibody or rigid body available " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_ACTUAL_STATE_UPDATE_FAILED ;
hasStatus = true ;
}
}
break ;
}
case CMD_STEP_FORWARD_SIMULATION :
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Step simulation request " ) ;
}
2016-04-14 00:21:43 +00:00
///todo(erwincoumans) move this damping inside Bullet
for ( int i = 0 ; i < m_data - > m_bodyHandles . size ( ) ; i + + )
{
applyJointDamping ( i ) ;
}
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > stepSimulation ( m_data - > m_physicsDeltaTime , 0 ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED ;
hasStatus = true ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME )
{
m_data - > m_physicsDeltaTime = clientCmd . m_physSimParamArgs . m_deltaTime ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_GRAVITY )
{
btVector3 grav ( clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 0 ] ,
clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 1 ] ,
clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 2 ] ) ;
this - > m_data - > m_dynamicsWorld - > setGravity ( grav ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Updated Gravity: %f,%f,%f " , grav [ 0 ] , grav [ 1 ] , grav [ 2 ] ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} ;
case CMD_INIT_POSE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Server Init Pose not implemented yet " ) ;
}
int bodyUniqueId = clientCmd . m_initPoseArgs . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION )
{
btVector3 zero ( 0 , 0 , 0 ) ;
2016-06-24 14:31:17 +00:00
btAssert ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 0 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 1 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 2 ] ) ;
2015-11-23 04:50:32 +00:00
mb - > setBaseVel ( zero ) ;
mb - > setBasePos ( btVector3 (
clientCmd . m_initPoseArgs . m_initialStateQ [ 0 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 1 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 2 ] ) ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION )
{
2016-06-24 14:31:17 +00:00
btAssert ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 3 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 4 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 5 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 6 ] ) ;
2015-11-23 04:50:32 +00:00
mb - > setBaseOmega ( btVector3 ( 0 , 0 , 0 ) ) ;
mb - > setWorldToBaseRot ( btQuaternion (
clientCmd . m_initPoseArgs . m_initialStateQ [ 3 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 4 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 5 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 6 ] ) ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_JOINT_STATE )
{
int dofIndex = 7 ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ dofIndex ] ) & & ( mb - > getLink ( i ) . m_dofCount = = 1 ) )
2015-11-23 04:50:32 +00:00
{
mb - > setJointPos ( i , clientCmd . m_initPoseArgs . m_initialStateQ [ dofIndex ] ) ;
mb - > setJointVel ( i , 0 ) ;
}
dofIndex + = mb - > getLink ( i ) . m_dofCount ;
}
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
case CMD_RESET_SIMULATION :
{
//clean up all data
if ( m_data & & m_data - > m_guiHelper & & m_data - > m_guiHelper - > getRenderInterface ( ) )
{
m_data - > m_guiHelper - > getRenderInterface ( ) - > removeAllInstances ( ) ;
}
2016-06-01 05:55:13 +00:00
if ( m_data )
{
m_data - > m_visualConverter . resetAll ( ) ;
}
2015-11-23 04:50:32 +00:00
deleteDynamicsWorld ( ) ;
createEmptyDynamicsWorld ( ) ;
m_data - > exitHandles ( ) ;
m_data - > initHandles ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RESET_SIMULATION_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_CREATE_RIGID_BODY :
case CMD_CREATE_BOX_COLLISION_SHAPE :
{
btVector3 halfExtents ( 1 , 1 , 1 ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS )
{
halfExtents = btVector3 (
clientCmd . m_createBoxShapeArguments . m_halfExtentsX ,
clientCmd . m_createBoxShapeArguments . m_halfExtentsY ,
clientCmd . m_createBoxShapeArguments . m_halfExtentsZ ) ;
}
btTransform startTrans ;
startTrans . setIdentity ( ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION )
{
startTrans . setOrigin ( btVector3 (
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 0 ] ,
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 1 ] ,
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 2 ] ) ) ;
}
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
startTrans . setRotation ( btQuaternion (
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 0 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 1 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 2 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 3 ] ) ) ;
}
btScalar mass = 0.f ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_MASS )
{
mass = clientCmd . m_createBoxShapeArguments . m_mass ;
}
int shapeType = COLLISION_SHAPE_TYPE_BOX ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE )
{
shapeType = clientCmd . m_createBoxShapeArguments . m_collisionShapeType ;
}
btBulletWorldImporter * worldImporter = new btBulletWorldImporter ( m_data - > m_dynamicsWorld ) ;
m_data - > m_worldImporters . push_back ( worldImporter ) ;
btCollisionShape * shape = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
switch ( shapeType )
{
case COLLISION_SHAPE_TYPE_CYLINDER_X :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 0 ] ;
shape = worldImporter - > createCylinderShapeX ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CYLINDER_Y :
{
btScalar radius = halfExtents [ 0 ] ;
btScalar height = halfExtents [ 1 ] ;
shape = worldImporter - > createCylinderShapeY ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CYLINDER_Z :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 2 ] ;
shape = worldImporter - > createCylinderShapeZ ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_X :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 0 ] ;
shape = worldImporter - > createCapsuleShapeX ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_Y :
{
btScalar radius = halfExtents [ 0 ] ;
btScalar height = halfExtents [ 1 ] ;
shape = worldImporter - > createCapsuleShapeY ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_Z :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 2 ] ;
shape = worldImporter - > createCapsuleShapeZ ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_SPHERE :
{
btScalar radius = halfExtents [ 0 ] ;
shape = worldImporter - > createSphereShape ( radius ) ;
break ;
}
case COLLISION_SHAPE_TYPE_BOX :
default :
{
shape = worldImporter - > createBoxShape ( halfExtents ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
bool isDynamic = ( mass > 0 ) ;
btRigidBody * rb = worldImporter - > createRigidBody ( isDynamic , mass , startTrans , shape , 0 ) ;
rb - > setRollingFriction ( 0.2 ) ;
//m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
btVector4 colorRGBA ( 1 , 0 , 0 , 1 ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_COLOR )
{
colorRGBA [ 0 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 0 ] ;
colorRGBA [ 1 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 1 ] ;
colorRGBA [ 2 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 2 ] ;
colorRGBA [ 3 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 3 ] ;
}
m_data - > m_guiHelper - > createCollisionShapeGraphicsObject ( rb - > getCollisionShape ( ) ) ;
m_data - > m_guiHelper - > createCollisionObjectGraphicsObject ( rb , colorRGBA ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RIGID_BODY_CREATION_COMPLETED ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int bodyUniqueId = m_data - > allocHandle ( ) ;
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
serverCmd . m_rigidBodyCreateArgs . m_bodyUniqueId = bodyUniqueId ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
bodyHandle - > m_rigidBody = rb ;
hasStatus = true ;
break ;
}
case CMD_PICK_BODY :
{
pickBody ( btVector3 ( clientCmd . m_pickBodyArguments . m_rayFromWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 2 ] ) ,
btVector3 ( clientCmd . m_pickBodyArguments . m_rayToWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 2 ] ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_MOVE_PICKED_BODY :
{
movePickedBody ( btVector3 ( clientCmd . m_pickBodyArguments . m_rayFromWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 2 ] ) ,
btVector3 ( clientCmd . m_pickBodyArguments . m_rayToWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 2 ] ) ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY :
{
removePickingConstraint ( ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
2016-06-27 01:18:30 +00:00
case CMD_APPLY_EXTERNAL_FORCE :
{
for ( int i = 0 ; i < clientCmd . m_externalForceArguments . m_numForcesAndTorques ; + + i )
{
InteralBodyData * body = m_data - > getHandle ( clientCmd . m_externalForceArguments . m_bodyUniqueIds [ i ] ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
bool isLinkFrame = ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_LINK_FRAME ) ! = 0 ) ;
if ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_FORCE ) ! = 0 )
{
btVector3 forceLocal ( clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 2 ] ) ;
btVector3 positionLocal (
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 2 ] ) ;
if ( clientCmd . m_externalForceArguments . m_linkIds [ i ] = = - 1 )
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * forceLocal ;
btVector3 relPosWorld = isLinkFrame ? positionLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * positionLocal ;
mb - > addBaseForce ( forceWorld ) ;
mb - > addBaseTorque ( relPosWorld . cross ( forceWorld ) ) ;
//b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
} else
{
int link = clientCmd . m_externalForceArguments . m_linkIds [ i ] ;
btVector3 forceWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * forceLocal ;
btVector3 relPosWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * positionLocal ;
mb - > addLinkForce ( link , forceWorld ) ;
mb - > addLinkTorque ( link , relPosWorld . cross ( forceWorld ) ) ;
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
}
}
if ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_TORQUE ) ! = 0 )
{
btVector3 torqueLocal ( clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 2 ] ) ;
if ( clientCmd . m_externalForceArguments . m_linkIds [ i ] = = - 1 )
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * torqueLocal ;
mb - > addBaseTorque ( torqueWorld ) ;
//b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
} else
{
int link = clientCmd . m_externalForceArguments . m_linkIds [ i ] ;
btVector3 torqueWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * torqueLocal ;
mb - > addLinkTorque ( link , torqueWorld ) ;
//b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
}
}
}
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
2015-11-23 04:50:32 +00:00
default :
{
b3Error ( " Unknown command encountered " ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_UNKNOWN_COMMAND_FLUSHED ;
hasStatus = true ;
}
} ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
}
return hasStatus ;
}
void PhysicsServerCommandProcessor : : renderScene ( )
{
if ( m_data - > m_guiHelper )
{
m_data - > m_guiHelper - > syncPhysicsToGraphics ( m_data - > m_dynamicsWorld ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_guiHelper - > render ( m_data - > m_dynamicsWorld ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
void PhysicsServerCommandProcessor : : physicsDebugDraw ( int debugDrawFlags )
{
if ( m_data - > m_dynamicsWorld )
{
if ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) )
{
m_data - > m_dynamicsWorld - > getDebugDrawer ( ) - > setDebugMode ( debugDrawFlags ) ;
m_data - > m_dynamicsWorld - > debugDrawWorld ( ) ;
}
}
}
bool PhysicsServerCommandProcessor : : pickBody ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
{
if ( m_data - > m_dynamicsWorld = = 0 )
return false ;
btCollisionWorld : : ClosestRayResultCallback rayCallback ( rayFromWorld , rayToWorld ) ;
m_data - > m_dynamicsWorld - > rayTest ( rayFromWorld , rayToWorld , rayCallback ) ;
if ( rayCallback . hasHit ( ) )
{
btVector3 pickPos = rayCallback . m_hitPointWorld ;
btRigidBody * body = ( btRigidBody * ) btRigidBody : : upcast ( rayCallback . m_collisionObject ) ;
if ( body )
{
//other exclusions?
if ( ! ( body - > isStaticObject ( ) | | body - > isKinematicObject ( ) ) )
{
m_data - > m_pickedBody = body ;
m_data - > m_pickedBody - > setActivationState ( DISABLE_DEACTIVATION ) ;
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body - > getCenterOfMassTransform ( ) . inverse ( ) * pickPos ;
btPoint2PointConstraint * p2p = new btPoint2PointConstraint ( * body , localPivot ) ;
m_data - > m_dynamicsWorld - > addConstraint ( p2p , true ) ;
m_data - > m_pickedConstraint = p2p ;
btScalar mousePickClamping = 30.f ;
p2p - > m_setting . m_impulseClamp = mousePickClamping ;
//very weak constraint for picking
p2p - > m_setting . m_tau = 0.001f ;
}
} else
{
btMultiBodyLinkCollider * multiCol = ( btMultiBodyLinkCollider * ) btMultiBodyLinkCollider : : upcast ( rayCallback . m_collisionObject ) ;
if ( multiCol & & multiCol - > m_multiBody )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_prevCanSleep = multiCol - > m_multiBody - > getCanSleep ( ) ;
multiCol - > m_multiBody - > setCanSleep ( false ) ;
btVector3 pivotInA = multiCol - > m_multiBody - > worldPosToLocal ( multiCol - > m_link , pickPos ) ;
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( multiCol - > m_multiBody , multiCol - > m_link , 0 , pivotInA , pickPos ) ;
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling = 1 ;
p2p - > setMaxAppliedImpulse ( 2 * scaling ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( p2p ) ;
2016-06-24 14:31:17 +00:00
m_data - > m_pickingMultiBodyPoint2Point = p2p ;
2015-11-23 04:50:32 +00:00
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_data - > m_oldPickingPos = rayToWorld ;
m_data - > m_hitPos = pickPos ;
m_data - > m_oldPickingDist = ( pickPos - rayFromWorld ) . length ( ) ;
// printf("hit !\n");
//add p2p
}
return false ;
}
bool PhysicsServerCommandProcessor : : movePickedBody ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
{
if ( m_data - > m_pickedBody & & m_data - > m_pickedConstraint )
{
btPoint2PointConstraint * pickCon = static_cast < btPoint2PointConstraint * > ( m_data - > m_pickedConstraint ) ;
if ( pickCon )
{
//keep it at the same picking distance
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btVector3 dir = rayToWorld - rayFromWorld ;
dir . normalize ( ) ;
dir * = m_data - > m_oldPickingDist ;
btVector3 newPivotB = rayFromWorld + dir ;
pickCon - > setPivotB ( newPivotB ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_pickingMultiBodyPoint2Point )
{
//keep it at the same picking distance
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btVector3 dir = rayToWorld - rayFromWorld ;
dir . normalize ( ) ;
dir * = m_data - > m_oldPickingDist ;
btVector3 newPivotB = rayFromWorld + dir ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_pickingMultiBodyPoint2Point - > setPivotInB ( newPivotB ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return false ;
}
void PhysicsServerCommandProcessor : : removePickingConstraint ( )
{
if ( m_data - > m_pickedConstraint )
{
m_data - > m_dynamicsWorld - > removeConstraint ( m_data - > m_pickedConstraint ) ;
delete m_data - > m_pickedConstraint ;
m_data - > m_pickedConstraint = 0 ;
m_data - > m_pickedBody = 0 ;
}
if ( m_data - > m_pickingMultiBodyPoint2Point )
{
m_data - > m_pickingMultiBodyPoint2Point - > getMultiBodyA ( ) - > setCanSleep ( m_data - > m_prevCanSleep ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > removeMultiBodyConstraint ( m_data - > m_pickingMultiBodyPoint2Point ) ;
delete m_data - > m_pickingMultiBodyPoint2Point ;
m_data - > m_pickingMultiBodyPoint2Point = 0 ;
}
}
void PhysicsServerCommandProcessor : : enableCommandLogging ( bool enable , const char * fileName )
{
if ( enable )
{
if ( 0 = = m_data - > m_commandLogger )
{
m_data - > m_commandLogger = new CommandLogger ( fileName ) ;
}
} else
{
if ( 0 ! = m_data - > m_commandLogger )
{
delete m_data - > m_commandLogger ;
m_data - > m_commandLogger = 0 ;
}
}
}
void PhysicsServerCommandProcessor : : replayFromLogFile ( const char * fileName )
{
CommandLogPlayback * pb = new CommandLogPlayback ( fileName ) ;
m_data - > m_logPlayback = pb ;
}
2016-04-14 00:21:43 +00:00
void PhysicsServerCommandProcessor : : applyJointDamping ( int bodyUniqueId )
{
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body ) {
btMultiBody * mb = body - > m_multiBody ;
if ( mb ) {
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + ) {
for ( int d = 0 ; d < mb - > getLink ( l ) . m_dofCount ; d + + ) {
double damping_coefficient = mb - > getLink ( l ) . m_jointDamping ;
double damping = - damping_coefficient * mb - > getJointVelMultiDof ( l ) [ d ] ;
mb - > addJointTorqueMultiDof ( l , d , damping ) ;
}
}
}
}
}