2015-11-23 04:50:32 +00:00
# include "PhysicsServerCommandProcessor.h"
2017-10-08 01:50:23 +00:00
# include "../CommonInterfaces/CommonRenderInterface.h"
2017-09-24 01:05:23 +00:00
2015-11-23 04:50:32 +00:00
# include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
# include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
# include "../Importers/ImportURDFDemo/URDF2Bullet.h"
2016-08-10 01:40:12 +00:00
# include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
2018-01-17 20:48:48 +00:00
2018-02-17 03:44:33 +00:00
# include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
2018-08-24 06:04:17 +00:00
# include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
# include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
# include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
2015-11-23 04:50:32 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
# include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
# include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
# include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
2016-08-23 01:14:29 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
2017-06-07 23:22:02 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h"
2017-06-19 17:14:26 +00:00
# include "../Importers/ImportURDFDemo/UrdfParser.h"
# include "../Utils/b3ResourcePath.h"
# include "Bullet3Common/b3FileUtils.h"
2017-06-23 21:43:28 +00:00
# include "../OpenGLWindow/GLInstanceGraphicsShape.h"
2016-08-26 17:35:10 +00:00
# include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
2016-09-29 19:07:54 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
2017-02-16 22:19:09 +00:00
# include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
2017-05-05 00:51:40 +00:00
# include "Bullet3Common/b3HashMap.h"
# include "../Utils/ChromeTraceUtil.h"
2017-06-30 20:35:07 +00:00
# include "stb_image/stb_image.h"
2016-08-10 01:40:12 +00:00
# include "BulletInverseDynamics/MultiBodyTree.hpp"
2016-09-08 22:15:58 +00:00
# include "IKTrajectoryHelper.h"
2015-11-23 04:50:32 +00:00
# include "btBulletDynamicsCommon.h"
2017-02-17 22:25:53 +00:00
# include "../Utils/RobotLoggingUtil.h"
2015-11-23 04:50:32 +00:00
# include "LinearMath/btTransform.h"
2016-12-31 22:43:15 +00:00
# include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
2017-05-18 02:29:12 +00:00
# include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
2018-01-08 20:25:56 +00:00
# include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
2017-11-23 02:12:02 +00:00
# include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h"
2015-11-23 04:50:32 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
# include "LinearMath/btSerializer.h"
# include "Bullet3Common/b3Logging.h"
# include "../CommonInterfaces/CommonGUIHelperInterface.h"
# include "SharedMemoryCommands.h"
2017-04-29 17:32:30 +00:00
# include "LinearMath/btRandom.h"
2017-05-03 17:49:04 +00:00
# include "Bullet3Common/b3ResizablePool.h"
2017-05-05 00:51:40 +00:00
# include "../Utils/b3Clock.h"
2017-09-23 02:17:57 +00:00
# include "b3PluginManager.h"
2017-12-31 23:37:16 +00:00
# include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
2018-03-07 22:51:51 +00:00
# include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
2018-06-19 16:28:48 +00:00
# include "LinearMath/TaskScheduler/btThreadSupportInterface.h"
2017-09-23 02:17:57 +00:00
2018-07-27 13:48:24 +00:00
# ifndef SKIP_COLLISION_FILTER_PLUGIN
# include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
# endif
2018-06-05 22:59:01 +00:00
# ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
2018-06-05 01:41:41 +00:00
# include "plugins/pdControlPlugin/pdControlPlugin.h"
2018-06-05 22:59:01 +00:00
# endif //SKIP_STATIC_PD_CONTROL_PLUGIN
2017-10-03 22:00:52 +00:00
# ifdef STATIC_LINK_VR_PLUGIN
# include "plugins/vrSyncPlugin/vrSyncPlugin.h"
# endif
2018-01-17 20:48:48 +00:00
# ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
# include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
# endif
2017-04-29 17:32:30 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
# include "../TinyAudio/b3SoundEngine.h"
# endif
2015-11-23 04:50:32 +00:00
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
# include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
# include "BulletSoftBody/btSoftBodySolvers.h"
# include "BulletSoftBody/btSoftBodyHelpers.h"
2016-10-18 06:40:38 +00:00
# include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
2016-10-12 18:51:04 +00:00
# include "../SoftDemo/BunnyMesh.h"
2016-10-19 00:38:43 +00:00
# else
# include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
# endif
2015-11-23 04:50:32 +00:00
2018-05-12 02:52:06 +00:00
extern bool gJointFeedbackInWorldSpace ;
extern bool gJointFeedbackInJointFrame ;
2016-10-23 14:14:50 +00:00
int gInternalSimFlags = 0 ;
2016-11-30 01:08:47 +00:00
bool gResetSimulation = 0 ;
2017-01-06 01:41:58 +00:00
int gVRTrackingObjectUniqueId = - 1 ;
2017-05-18 02:29:12 +00:00
int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION ;
2017-01-06 01:41:58 +00:00
btTransform gVRTrackingObjectTr = btTransform : : getIdentity ( ) ;
2016-10-09 01:40:09 +00:00
2017-05-31 02:54:55 +00:00
2017-01-06 01:41:58 +00:00
btVector3 gVRTeleportPos1 ( 0 , 0 , 0 ) ;
btQuaternion gVRTeleportOrn ( 0 , 0 , 0 , 1 ) ;
2016-11-17 00:12:59 +00:00
2016-09-22 15:50:28 +00:00
btScalar simTimeScalingFactor = 1 ;
2016-09-28 18:17:11 +00:00
btScalar gRhsClamp = 1.f ;
2016-09-15 23:57:00 +00:00
2015-11-23 04:50:32 +00:00
struct UrdfLinkNameMapUtil
{
btMultiBody * m_mb ;
2017-08-30 02:14:27 +00:00
btAlignedObjectArray < btGeneric6DofSpring2Constraint * > m_rigidBodyJoints ;
2015-11-23 04:50:32 +00:00
btDefaultSerializer * m_memSerializer ;
UrdfLinkNameMapUtil ( ) : m_mb ( 0 ) , m_memSerializer ( 0 )
{
}
2016-07-17 04:29:31 +00:00
virtual ~ UrdfLinkNameMapUtil ( )
{
}
2015-11-23 04:50:32 +00:00
} ;
2018-06-19 16:28:48 +00:00
class b3ThreadPool {
public :
b3ThreadPool ( const char * name = " b3ThreadPool " ) {
btThreadSupportInterface : : ConstructionInfo info ( name , threadFunction ) ;
m_threadSupportInterface = btThreadSupportInterface : : create ( info ) ;
}
~ b3ThreadPool ( ) {
delete m_threadSupportInterface ;
}
const int numWorkers ( ) const { return m_threadSupportInterface - > getNumWorkerThreads ( ) ; }
void runTask ( int threadIdx , btThreadSupportInterface : : ThreadFunc func , void * arg ) {
2018-06-22 18:41:02 +00:00
FunctionContext & ctx = m_functionContexts [ threadIdx ] ;
2018-06-19 16:28:48 +00:00
ctx . func = func ;
ctx . arg = arg ;
m_threadSupportInterface - > runTask ( threadIdx , ( void * ) & ctx ) ;
}
void waitForAllTasks ( ) {
BT_PROFILE ( " b3ThreadPool_waitForAllTasks " ) ;
m_threadSupportInterface - > waitForAllTasks ( ) ;
}
private :
struct FunctionContext {
btThreadSupportInterface : : ThreadFunc func ;
void * arg ;
} ;
static void threadFunction ( void * userPtr ) {
BT_PROFILE ( " b3ThreadPool_threadFunction " ) ;
FunctionContext * ctx = ( FunctionContext * ) userPtr ;
ctx - > func ( ctx - > arg ) ;
}
btThreadSupportInterface * m_threadSupportInterface ;
FunctionContext m_functionContexts [ BT_MAX_THREAD_COUNT ] ;
} ;
2015-11-23 04:50:32 +00:00
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 ) ;
}
} ;
2017-10-08 01:50:23 +00:00
struct InternalVisualShapeData
{
int m_tinyRendererVisualShapeIndex ;
int m_OpenGLGraphicsIndex ;
2018-03-29 02:08:18 +00:00
b3AlignedObjectArray < UrdfVisual > m_visualShapes ;
b3AlignedObjectArray < std : : string > m_pathPrefixes ;
2017-10-08 01:50:23 +00:00
void clear ( )
{
2018-03-29 02:08:18 +00:00
m_tinyRendererVisualShapeIndex = - 1 ;
m_OpenGLGraphicsIndex = - 1 ;
m_visualShapes . clear ( ) ;
m_pathPrefixes . clear ( ) ;
2017-10-08 01:50:23 +00:00
}
} ;
2017-06-30 20:35:07 +00:00
struct InternalCollisionShapeData
2017-06-03 17:57:56 +00:00
{
btCollisionShape * m_collisionShape ;
2017-06-30 05:06:27 +00:00
b3AlignedObjectArray < UrdfCollision > m_urdfCollisionObjects ;
2017-06-03 17:57:56 +00:00
void clear ( )
{
m_collisionShape = 0 ;
}
} ;
2017-05-01 18:14:09 +00:00
2018-06-02 20:40:08 +00:00
# include "SharedMemoryUserData.h"
2017-06-30 20:35:07 +00:00
struct InternalBodyData
2015-11-23 04:50:32 +00:00
{
btMultiBody * m_multiBody ;
btRigidBody * m_rigidBody ;
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btSoftBody * m_softBody ;
# endif
2015-11-23 04:50:32 +00:00
int m_testData ;
2017-03-29 22:37:33 +00:00
std : : string m_bodyName ;
2015-11-23 04:50:32 +00:00
btTransform m_rootLocalInertialFrame ;
2016-04-29 21:45:15 +00:00
btAlignedObjectArray < btTransform > m_linkLocalInertialFrames ;
2017-08-30 02:14:27 +00:00
btAlignedObjectArray < btGeneric6DofSpring2Constraint * > m_rigidBodyJoints ;
btAlignedObjectArray < std : : string > m_rigidBodyJointNames ;
btAlignedObjectArray < std : : string > m_rigidBodyLinkNames ;
2018-07-03 15:45:19 +00:00
btAlignedObjectArray < int > m_userDataHandles ;
2018-03-15 03:47:56 +00:00
2017-05-01 18:14:09 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
2017-05-05 00:51:40 +00:00
b3HashMap < btHashInt , SDFAudioSource > m_audioSources ;
2017-05-01 18:14:09 +00:00
# endif //B3_ENABLE_TINY_AUDIO
2015-11-23 04:50:32 +00:00
2018-03-15 03:47:56 +00:00
InternalBodyData ( )
2015-11-23 04:50:32 +00:00
{
2017-05-04 04:53:29 +00:00
clear ( ) ;
}
void clear ( )
{
m_multiBody = 0 ;
m_rigidBody = 0 ;
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_softBody = 0 ;
# endif
2017-05-04 04:53:29 +00:00
m_testData = 0 ;
m_bodyName = " " ;
2015-11-23 04:50:32 +00:00
m_rootLocalInertialFrame . setIdentity ( ) ;
2017-05-04 04:53:29 +00:00
m_linkLocalInertialFrames . clear ( ) ;
2017-08-30 02:14:27 +00:00
m_rigidBodyJoints . clear ( ) ;
m_rigidBodyJointNames . clear ( ) ;
m_rigidBodyLinkNames . clear ( ) ;
2018-07-03 15:45:19 +00:00
m_userDataHandles . clear ( ) ;
2015-11-23 04:50:32 +00:00
}
2017-05-04 04:53:29 +00:00
2015-11-23 04:50:32 +00:00
} ;
2016-11-14 22:08:05 +00:00
struct InteralUserConstraintData
{
btTypedConstraint * m_rbConstraint ;
btMultiBodyConstraint * m_mbConstraint ;
2017-01-23 03:08:31 +00:00
b3UserConstraint m_userConstraintData ;
2016-11-14 22:08:05 +00:00
InteralUserConstraintData ( )
: m_rbConstraint ( 0 ) ,
m_mbConstraint ( 0 )
{
}
} ;
2017-06-30 20:35:07 +00:00
struct InternalTextureData
{
int m_tinyRendererTextureId ;
int m_openglTextureId ;
void clear ( )
{
m_tinyRendererTextureId = - 1 ;
m_openglTextureId = - 1 ;
}
} ;
typedef b3PoolBodyHandle < InternalTextureData > InternalTextureHandle ;
typedef b3PoolBodyHandle < InternalBodyData > InternalBodyHandle ;
typedef b3PoolBodyHandle < InternalCollisionShapeData > InternalCollisionShapeHandle ;
2017-10-08 01:50:23 +00:00
typedef b3PoolBodyHandle < InternalVisualShapeData > InternalVisualShapeHandle ;
2015-11-23 04:50:32 +00:00
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 )
{
2017-09-06 20:18:39 +00:00
if ( m_file )
{
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 ) ;
2018-03-15 03:47:56 +00:00
2017-09-06 20:18:39 +00:00
switch ( command . m_type )
{
2018-03-15 03:47:56 +00:00
2017-09-06 20:18:39 +00:00
case CMD_LOAD_MJCF :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_mjcfArguments , sizeof ( MjcfArgs ) , 1 , m_file ) ;
break ;
}
case CMD_REQUEST_BODY_INFO :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_sdfRequestInfoArgs , sizeof ( SdfRequestInfoArgs ) , 1 , m_file ) ;
break ;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_requestVisualShapeDataArguments , sizeof ( RequestVisualShapeDataArgs ) , 1 , m_file ) ;
break ;
}
case CMD_LOAD_URDF :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_urdfArguments , sizeof ( UrdfArgs ) , 1 , m_file ) ;
break ;
}
case CMD_INIT_POSE :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_initPoseArgs , sizeof ( InitPoseArgs ) , 1 , m_file ) ;
break ;
} ;
case CMD_REQUEST_ACTUAL_STATE :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_requestActualStateInformationCommandArgument ,
sizeof ( RequestActualStateArgs ) , 1 , m_file ) ;
break ;
} ;
case CMD_SEND_DESIRED_STATE :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_sendDesiredStateCommandArgument , sizeof ( SendDesiredStateArgs ) , 1 , m_file ) ;
break ;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
2017-10-05 18:43:14 +00:00
fwrite ( ( const char * ) & command . m_physSimParamArgs , sizeof ( b3PhysicsSimulationParameters ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
break ;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION :
{
fwrite ( ( const char * ) & command . m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command . m_requestContactPointArguments , sizeof ( RequestContactDataArgs ) , 1 , m_file ) ;
break ;
}
case CMD_STEP_FORWARD_SIMULATION :
case CMD_RESET_SIMULATION :
case CMD_REQUEST_INTERNAL_DATA :
{
break ;
} ;
default :
{
fwrite ( ( const char * ) & command , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
}
} ;
}
2015-11-23 04:50:32 +00:00
}
CommandLogger ( const char * fileName )
{
m_file = fopen ( fileName , " wb " ) ;
2017-09-06 20:18:39 +00:00
if ( m_file )
{
unsigned char buf [ 15 ] ;
buf [ 12 ] = 12 ;
buf [ 13 ] = 13 ;
buf [ 14 ] = 14 ;
writeHeader ( buf ) ;
fwrite ( buf , 12 , 1 , m_file ) ;
}
2015-11-23 04:50:32 +00:00
}
virtual ~ CommandLogger ( )
{
2017-09-06 20:18:39 +00:00
if ( m_file )
{
fclose ( m_file ) ;
}
2015-11-23 04:50:32 +00:00
}
} ;
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 )
{
2017-01-24 16:36:46 +00:00
size_t bytesRead ;
bytesRead = fread ( m_header , 12 , 1 , m_file ) ;
2015-11-23 04:50:32 +00:00
}
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 )
{
2017-09-06 20:18:39 +00:00
//for a little while, keep this flag to be able to read 'old' log files
//#define BACKWARD_COMPAT
# if BACKWARD_COMPAT
SharedMemoryCommand unused ;
# endif //BACKWARD_COMPAT
bool result = false ;
2018-04-11 00:21:14 +00:00
size_t s = 0 ;
2015-11-23 04:50:32 +00:00
if ( m_file )
{
2018-04-11 00:21:14 +00:00
2017-09-06 20:18:39 +00:00
int commandType = - 1 ;
2015-11-23 04:50:32 +00:00
if ( m_fileIs64bit )
{
bCommandChunkPtr8 chunk8 ;
s = fread ( ( void * ) & chunk8 , sizeof ( bCommandChunkPtr8 ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
commandType = chunk8 . code ;
2015-11-23 04:50:32 +00:00
} else
{
bCommandChunkPtr4 chunk4 ;
s = fread ( ( void * ) & chunk4 , sizeof ( bCommandChunkPtr4 ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
commandType = chunk4 . code ;
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( s = = 1 )
{
2017-09-06 20:18:39 +00:00
memset ( cmd , 0 , sizeof ( SharedMemoryCommand ) ) ;
2018-03-15 03:47:56 +00:00
cmd - > m_type = commandType ;
2017-09-06 20:18:39 +00:00
# ifdef BACKWARD_COMPAT
s = fread ( & unused , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
cmd - > m_updateFlags = unused . m_updateFlags ;
# endif
switch ( commandType )
{
case CMD_LOAD_MJCF :
{
# ifdef BACKWARD_COMPAT
cmd - > m_mjcfArguments = unused . m_mjcfArguments ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_mjcfArguments , sizeof ( MjcfArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_REQUEST_BODY_INFO :
{
# ifdef BACKWARD_COMPAT
cmd - > m_sdfRequestInfoArgs = unused . m_sdfRequestInfoArgs ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_sdfRequestInfoArgs , sizeof ( SdfRequestInfoArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO :
{
# ifdef BACKWARD_COMPAT
cmd - > m_requestVisualShapeDataArguments = unused . m_requestVisualShapeDataArguments ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_requestVisualShapeDataArguments , sizeof ( RequestVisualShapeDataArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_LOAD_URDF :
{
# ifdef BACKWARD_COMPAT
cmd - > m_urdfArguments = unused . m_urdfArguments ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_urdfArguments , sizeof ( UrdfArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_INIT_POSE :
{
# ifdef BACKWARD_COMPAT
cmd - > m_initPoseArgs = unused . m_initPoseArgs ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_initPoseArgs , sizeof ( InitPoseArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
} ;
case CMD_REQUEST_ACTUAL_STATE :
{
2018-03-15 03:47:56 +00:00
# ifdef BACKWARD_COMPAT
2017-09-06 20:18:39 +00:00
cmd - > m_requestActualStateInformationCommandArgument = unused . m_requestActualStateInformationCommandArgument ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_requestActualStateInformationCommandArgument , sizeof ( RequestActualStateArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
} ;
case CMD_SEND_DESIRED_STATE :
{
2018-03-15 03:47:56 +00:00
# ifdef BACKWARD_COMPAT
2017-09-06 20:18:39 +00:00
cmd - > m_sendDesiredStateCommandArgument = unused . m_sendDesiredStateCommandArgument ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_sendDesiredStateCommandArgument , sizeof ( SendDesiredStateArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
2018-03-15 03:47:56 +00:00
# ifdef BACKWARD_COMPAT
2017-09-06 20:18:39 +00:00
cmd - > m_physSimParamArgs = unused . m_physSimParamArgs ;
# else
2018-04-12 16:28:30 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_physSimParamArgs , sizeof ( b3PhysicsSimulationParameters ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION :
{
2018-03-15 03:47:56 +00:00
# ifdef BACKWARD_COMPAT
2017-09-06 20:18:39 +00:00
cmd - > m_requestContactPointArguments = unused . m_requestContactPointArguments ;
# else
2018-04-11 00:21:14 +00:00
s = fread ( & cmd - > m_updateFlags , sizeof ( int ) , 1 , m_file ) ;
s = fread ( & cmd - > m_requestContactPointArguments , sizeof ( RequestContactDataArgs ) , 1 , m_file ) ;
2017-09-06 20:18:39 +00:00
# endif
result = true ;
break ;
}
case CMD_STEP_FORWARD_SIMULATION :
case CMD_RESET_SIMULATION :
case CMD_REQUEST_INTERNAL_DATA :
{
result = true ;
break ;
}
default :
{
s = fread ( cmd , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
result = ( s = = 1 ) ;
}
} ;
2015-11-23 04:50:32 +00:00
}
}
2017-09-06 20:18:39 +00:00
return result ;
2015-11-23 04:50:32 +00:00
}
} ;
2016-10-13 06:03:36 +00:00
struct SaveWorldObjectData
{
b3AlignedObjectArray < int > m_bodyUniqueIds ;
std : : string m_fileName ;
} ;
2016-11-10 19:22:22 +00:00
struct MyBroadphaseCallback : public btBroadphaseAabbCallback
{
b3AlignedObjectArray < int > m_bodyUniqueIds ;
b3AlignedObjectArray < int > m_links ;
MyBroadphaseCallback ( )
{
}
virtual ~ MyBroadphaseCallback ( )
{
}
void clear ( )
{
m_bodyUniqueIds . clear ( ) ;
m_links . clear ( ) ;
}
virtual bool process ( const btBroadphaseProxy * proxy )
{
btCollisionObject * colObj = ( btCollisionObject * ) proxy - > m_clientObject ;
btMultiBodyLinkCollider * mbl = btMultiBodyLinkCollider : : upcast ( colObj ) ;
if ( mbl )
{
int bodyUniqueId = mbl - > m_multiBody - > getUserIndex2 ( ) ;
m_bodyUniqueIds . push_back ( bodyUniqueId ) ;
m_links . push_back ( mbl - > m_link ) ;
return true ;
}
int bodyUniqueId = colObj - > getUserIndex2 ( ) ;
if ( bodyUniqueId > = 0 )
{
m_bodyUniqueIds . push_back ( bodyUniqueId ) ;
2017-04-17 17:01:44 +00:00
//it is not a multibody, so use -1 otherwise
m_links . push_back ( - 1 ) ;
2016-11-10 19:22:22 +00:00
}
return true ;
}
} ;
2017-01-16 16:23:49 +00:00
enum MyFilterModes
{
FILTER_GROUPAMASKB_AND_GROUPBMASKA = 0 ,
FILTER_GROUPAMASKB_OR_GROUPBMASKA
} ;
struct MyOverlapFilterCallback : public btOverlapFilterCallback
{
int m_filterMode ;
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
MyOverlapFilterCallback ( )
: m_filterMode ( FILTER_GROUPAMASKB_AND_GROUPBMASKA )
{
}
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
virtual ~ MyOverlapFilterCallback ( )
{ }
// return true when pairs need collision
virtual bool needBroadphaseCollision ( btBroadphaseProxy * proxy0 , btBroadphaseProxy * proxy1 ) const
{
if ( m_filterMode = = FILTER_GROUPAMASKB_AND_GROUPBMASKA )
{
bool collides = ( proxy0 - > m_collisionFilterGroup & proxy1 - > m_collisionFilterMask ) ! = 0 ;
collides = collides & & ( proxy1 - > m_collisionFilterGroup & proxy0 - > m_collisionFilterMask ) ;
return collides ;
}
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
if ( m_filterMode = = FILTER_GROUPAMASKB_OR_GROUPBMASKA )
{
bool collides = ( proxy0 - > m_collisionFilterGroup & proxy1 - > m_collisionFilterMask ) ! = 0 ;
collides = collides | | ( proxy1 - > m_collisionFilterGroup & proxy0 - > m_collisionFilterMask ) ;
return collides ;
}
return false ;
}
} ;
2017-02-17 22:25:53 +00:00
struct InternalStateLogger
{
int m_loggingUniqueId ;
int m_loggingType ;
InternalStateLogger ( )
: m_loggingUniqueId ( 0 ) ,
m_loggingType ( 0 )
{
}
2017-02-17 23:43:38 +00:00
virtual ~ InternalStateLogger ( ) { }
2017-02-17 22:25:53 +00:00
virtual void stop ( ) = 0 ;
2017-03-29 23:29:34 +00:00
virtual void logState ( btScalar timeStep ) = 0 ;
2017-02-17 22:25:53 +00:00
} ;
2017-03-16 16:13:33 +00:00
struct VideoMP4Loggger : public InternalStateLogger
{
struct GUIHelperInterface * m_guiHelper ;
std : : string m_fileName ;
VideoMP4Loggger ( int loggerUid , const char * fileName , GUIHelperInterface * guiHelper )
: m_guiHelper ( guiHelper )
{
m_fileName = fileName ;
m_loggingUniqueId = loggerUid ;
m_loggingType = STATE_LOGGING_VIDEO_MP4 ;
m_guiHelper - > dumpFramesToVideo ( fileName ) ;
}
virtual void stop ( )
{
m_guiHelper - > dumpFramesToVideo ( 0 ) ;
}
2017-03-29 23:29:34 +00:00
virtual void logState ( btScalar timeStep )
2017-03-16 16:13:33 +00:00
{
//dumping video frames happens in another thread
//we could add some overlay of timestamp here, if needed/wanted
}
} ;
2017-02-17 22:25:53 +00:00
struct MinitaurStateLogger : public InternalStateLogger
{
int m_loggingTimeStamp ;
std : : string m_fileName ;
int m_minitaurBodyUniqueId ;
FILE * m_logFileHandle ;
std : : string m_structTypes ;
btMultiBody * m_minitaurMultiBody ;
btAlignedObjectArray < int > m_motorIdList ;
2017-02-21 18:23:18 +00:00
MinitaurStateLogger ( int loggingUniqueId , const std : : string & fileName , btMultiBody * minitaurMultiBody , btAlignedObjectArray < int > & motorIdList )
2017-02-17 22:25:53 +00:00
: m_loggingTimeStamp ( 0 ) ,
m_logFileHandle ( 0 ) ,
m_minitaurMultiBody ( minitaurMultiBody )
{
2017-03-07 19:52:26 +00:00
m_loggingUniqueId = loggingUniqueId ;
2017-02-17 22:25:53 +00:00
m_loggingType = STATE_LOGGING_MINITAUR ;
m_motorIdList . resize ( motorIdList . size ( ) ) ;
for ( int m = 0 ; m < motorIdList . size ( ) ; m + + )
{
m_motorIdList [ m ] = motorIdList [ m ] ;
}
btAlignedObjectArray < std : : string > structNames ;
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
structNames . push_back ( " t " ) ;
structNames . push_back ( " r " ) ;
structNames . push_back ( " p " ) ;
structNames . push_back ( " y " ) ;
2018-03-15 03:47:56 +00:00
2017-02-17 22:25:53 +00:00
structNames . push_back ( " q0 " ) ;
structNames . push_back ( " q1 " ) ;
structNames . push_back ( " q2 " ) ;
structNames . push_back ( " q3 " ) ;
structNames . push_back ( " q4 " ) ;
structNames . push_back ( " q5 " ) ;
structNames . push_back ( " q6 " ) ;
structNames . push_back ( " q7 " ) ;
structNames . push_back ( " u0 " ) ;
structNames . push_back ( " u1 " ) ;
structNames . push_back ( " u2 " ) ;
structNames . push_back ( " u3 " ) ;
structNames . push_back ( " u4 " ) ;
structNames . push_back ( " u5 " ) ;
structNames . push_back ( " u6 " ) ;
structNames . push_back ( " u7 " ) ;
structNames . push_back ( " dx " ) ;
structNames . push_back ( " mo " ) ;
m_structTypes = " IffffffffffffffffffffB " ;
const char * fileNameC = fileName . c_str ( ) ;
m_logFileHandle = createMinitaurLogFile ( fileNameC , structNames , m_structTypes ) ;
}
virtual void stop ( )
{
if ( m_logFileHandle )
{
closeMinitaurLogFile ( m_logFileHandle ) ;
m_logFileHandle = 0 ;
}
}
virtual void logState ( btScalar timeStep )
{
if ( m_logFileHandle )
{
2018-03-15 03:47:56 +00:00
2017-02-22 01:36:54 +00:00
//btVector3 pos = m_minitaurMultiBody->getBasePos();
2017-02-17 22:25:53 +00:00
MinitaurLogRecord logData ;
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
2017-03-06 05:49:20 +00:00
btScalar motorDir [ 8 ] = { 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 } ;
2017-02-17 22:25:53 +00:00
btQuaternion orn = m_minitaurMultiBody - > getBaseWorldTransform ( ) . getRotation ( ) ;
btMatrix3x3 mat ( orn ) ;
btScalar roll = 0 ;
btScalar pitch = 0 ;
btScalar yaw = 0 ;
mat . getEulerZYX ( yaw , pitch , roll ) ;
2018-03-15 03:47:56 +00:00
2017-02-17 22:25:53 +00:00
logData . m_values . push_back ( m_loggingTimeStamp ) ;
logData . m_values . push_back ( ( float ) roll ) ;
logData . m_values . push_back ( ( float ) pitch ) ;
logData . m_values . push_back ( ( float ) yaw ) ;
for ( int i = 0 ; i < 8 ; i + + )
{
float jointAngle = ( float ) motorDir [ i ] * m_minitaurMultiBody - > getJointPos ( m_motorIdList [ i ] ) ;
logData . m_values . push_back ( jointAngle ) ;
}
for ( int i = 0 ; i < 8 ; i + + )
{
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) m_minitaurMultiBody - > getLink ( m_motorIdList [ i ] ) . m_userPtr ;
if ( motor & & timeStep > btScalar ( 0 ) )
{
btScalar force = motor - > getAppliedImpulse ( 0 ) / timeStep ;
logData . m_values . push_back ( ( float ) force ) ;
}
}
//x is forward component, estimated speed forward
float xd_speed = m_minitaurMultiBody - > getBaseVel ( ) [ 0 ] ;
logData . m_values . push_back ( xd_speed ) ;
char mode = 6 ;
logData . m_values . push_back ( mode ) ;
//at the moment, appendMinitaurLogData will directly write to disk (potential delay)
//better to fill a huge memory buffer and once in a while write it to disk
appendMinitaurLogData ( m_logFileHandle , m_structTypes , logData ) ;
fflush ( m_logFileHandle ) ;
2018-03-15 03:47:56 +00:00
2017-02-17 22:25:53 +00:00
m_loggingTimeStamp + + ;
}
}
} ;
2017-01-16 16:23:49 +00:00
2017-03-29 23:29:34 +00:00
struct b3VRControllerEvents
{
b3VRControllerEvent m_vrEvents [ MAX_VR_CONTROLLERS ] ;
2018-03-15 03:47:56 +00:00
2017-03-29 23:29:34 +00:00
b3VRControllerEvents ( )
{
init ( ) ;
}
virtual ~ b3VRControllerEvents ( )
{
}
void init ( )
{
for ( int i = 0 ; i < MAX_VR_CONTROLLERS ; i + + )
{
2017-04-08 05:53:36 +00:00
m_vrEvents [ i ] . m_deviceType = 0 ;
2017-03-29 23:29:34 +00:00
m_vrEvents [ i ] . m_numButtonEvents = 0 ;
m_vrEvents [ i ] . m_numMoveEvents = 0 ;
for ( int b = 0 ; b < MAX_VR_BUTTONS ; b + + )
{
m_vrEvents [ i ] . m_buttons [ b ] = 0 ;
}
}
}
void addNewVREvents ( const struct b3VRControllerEvent * vrEvents , int numVREvents )
{
//update m_vrEvents
for ( int i = 0 ; i < numVREvents ; i + + )
{
int controlledId = vrEvents [ i ] . m_controllerId ;
if ( vrEvents [ i ] . m_numMoveEvents )
{
m_vrEvents [ controlledId ] . m_analogAxis = vrEvents [ i ] . m_analogAxis ;
2017-10-05 19:59:58 +00:00
for ( int a = 0 ; a < 10 ; a + + )
{
m_vrEvents [ controlledId ] . m_auxAnalogAxis [ a ] = vrEvents [ i ] . m_auxAnalogAxis [ a ] ;
}
} else
{
m_vrEvents [ controlledId ] . m_analogAxis = 0 ;
for ( int a = 0 ; a < 10 ; a + + )
{
m_vrEvents [ controlledId ] . m_auxAnalogAxis [ a ] = 0 ;
}
2017-03-29 23:29:34 +00:00
}
if ( vrEvents [ i ] . m_numMoveEvents + vrEvents [ i ] . m_numButtonEvents )
{
m_vrEvents [ controlledId ] . m_controllerId = vrEvents [ i ] . m_controllerId ;
2017-04-08 05:53:36 +00:00
m_vrEvents [ controlledId ] . m_deviceType = vrEvents [ i ] . m_deviceType ;
2017-03-29 23:29:34 +00:00
m_vrEvents [ controlledId ] . m_pos [ 0 ] = vrEvents [ i ] . m_pos [ 0 ] ;
m_vrEvents [ controlledId ] . m_pos [ 1 ] = vrEvents [ i ] . m_pos [ 1 ] ;
m_vrEvents [ controlledId ] . m_pos [ 2 ] = vrEvents [ i ] . m_pos [ 2 ] ;
m_vrEvents [ controlledId ] . m_orn [ 0 ] = vrEvents [ i ] . m_orn [ 0 ] ;
m_vrEvents [ controlledId ] . m_orn [ 1 ] = vrEvents [ i ] . m_orn [ 1 ] ;
m_vrEvents [ controlledId ] . m_orn [ 2 ] = vrEvents [ i ] . m_orn [ 2 ] ;
m_vrEvents [ controlledId ] . m_orn [ 3 ] = vrEvents [ i ] . m_orn [ 3 ] ;
}
m_vrEvents [ controlledId ] . m_numButtonEvents + = vrEvents [ i ] . m_numButtonEvents ;
m_vrEvents [ controlledId ] . m_numMoveEvents + = vrEvents [ i ] . m_numMoveEvents ;
for ( int b = 0 ; b < MAX_VR_BUTTONS ; b + + )
{
m_vrEvents [ controlledId ] . m_buttons [ b ] | = vrEvents [ i ] . m_buttons [ b ] ;
if ( vrEvents [ i ] . m_buttons [ b ] & eButtonIsDown )
{
m_vrEvents [ controlledId ] . m_buttons [ b ] | = eButtonIsDown ;
} else
{
m_vrEvents [ controlledId ] . m_buttons [ b ] & = ~ eButtonIsDown ;
}
}
}
} ;
} ;
struct VRControllerStateLogger : public InternalStateLogger
{
b3VRControllerEvents m_vrEvents ;
int m_loggingTimeStamp ;
2017-04-08 18:48:12 +00:00
int m_deviceTypeFilter ;
2017-03-29 23:29:34 +00:00
std : : string m_fileName ;
FILE * m_logFileHandle ;
std : : string m_structTypes ;
2017-04-08 18:48:12 +00:00
VRControllerStateLogger ( int loggingUniqueId , int deviceTypeFilter , const std : : string & fileName )
2017-03-29 23:29:34 +00:00
: m_loggingTimeStamp ( 0 ) ,
2017-04-08 18:48:12 +00:00
m_deviceTypeFilter ( deviceTypeFilter ) ,
2017-03-29 23:29:34 +00:00
m_fileName ( fileName ) ,
m_logFileHandle ( 0 )
{
m_loggingUniqueId = loggingUniqueId ;
m_loggingType = STATE_LOGGING_VR_CONTROLLERS ;
btAlignedObjectArray < std : : string > structNames ;
structNames . push_back ( " stepCount " ) ;
structNames . push_back ( " timeStamp " ) ;
structNames . push_back ( " controllerId " ) ;
structNames . push_back ( " numMoveEvents " ) ;
structNames . push_back ( " m_numButtonEvents " ) ;
structNames . push_back ( " posX " ) ;
structNames . push_back ( " posY " ) ;
structNames . push_back ( " posZ " ) ;
structNames . push_back ( " oriX " ) ;
structNames . push_back ( " oriY " ) ;
structNames . push_back ( " oriZ " ) ;
structNames . push_back ( " oriW " ) ;
structNames . push_back ( " analogAxis " ) ;
structNames . push_back ( " buttons0 " ) ;
structNames . push_back ( " buttons1 " ) ;
structNames . push_back ( " buttons2 " ) ;
structNames . push_back ( " buttons3 " ) ;
structNames . push_back ( " buttons4 " ) ;
structNames . push_back ( " buttons5 " ) ;
structNames . push_back ( " buttons6 " ) ;
2017-04-12 22:02:47 +00:00
structNames . push_back ( " deviceType " ) ;
m_structTypes = " IfIIIffffffffIIIIIIII " ;
2017-03-29 23:29:34 +00:00
const char * fileNameC = fileName . c_str ( ) ;
m_logFileHandle = createMinitaurLogFile ( fileNameC , structNames , m_structTypes ) ;
}
virtual void stop ( )
{
if ( m_logFileHandle )
{
closeMinitaurLogFile ( m_logFileHandle ) ;
m_logFileHandle = 0 ;
}
}
virtual void logState ( btScalar timeStep )
{
if ( m_logFileHandle )
{
2018-03-15 03:47:56 +00:00
2017-03-29 23:29:34 +00:00
int stepCount = m_loggingTimeStamp ;
float timeStamp = m_loggingTimeStamp * timeStep ;
2017-04-08 18:48:12 +00:00
2017-03-29 23:29:34 +00:00
for ( int i = 0 ; i < MAX_VR_CONTROLLERS ; i + + )
{
b3VRControllerEvent & event = m_vrEvents . m_vrEvents [ i ] ;
2017-04-08 18:48:12 +00:00
if ( m_deviceTypeFilter & event . m_deviceType )
2017-03-29 23:29:34 +00:00
{
2017-04-08 18:48:12 +00:00
if ( event . m_numButtonEvents + event . m_numMoveEvents )
2017-03-29 23:29:34 +00:00
{
2017-04-08 18:48:12 +00:00
MinitaurLogRecord logData ;
//serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event;
//log the event
logData . m_values . push_back ( stepCount ) ;
logData . m_values . push_back ( timeStamp ) ;
logData . m_values . push_back ( event . m_controllerId ) ;
logData . m_values . push_back ( event . m_numMoveEvents ) ;
logData . m_values . push_back ( event . m_numButtonEvents ) ;
logData . m_values . push_back ( event . m_pos [ 0 ] ) ;
logData . m_values . push_back ( event . m_pos [ 1 ] ) ;
logData . m_values . push_back ( event . m_pos [ 2 ] ) ;
logData . m_values . push_back ( event . m_orn [ 0 ] ) ;
logData . m_values . push_back ( event . m_orn [ 1 ] ) ;
logData . m_values . push_back ( event . m_orn [ 2 ] ) ;
logData . m_values . push_back ( event . m_orn [ 3 ] ) ;
logData . m_values . push_back ( event . m_analogAxis ) ;
int packedButtons [ 7 ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;
int packedButtonIndex = 0 ;
int packedButtonShift = 0 ;
//encode the 64 buttons into 7 int (3 bits each), each int stores 10 buttons
for ( int b = 0 ; b < MAX_VR_BUTTONS ; b + + )
2017-03-29 23:29:34 +00:00
{
2017-04-08 18:48:12 +00:00
int buttonMask = event . m_buttons [ b ] ;
buttonMask = buttonMask < < ( packedButtonShift * 3 ) ;
packedButtons [ packedButtonIndex ] | = buttonMask ;
packedButtonShift + + ;
if ( packedButtonShift > = 10 )
2017-03-29 23:29:34 +00:00
{
2017-04-08 18:48:12 +00:00
packedButtonShift = 0 ;
packedButtonIndex + + ;
if ( packedButtonIndex > = 7 )
{
btAssert ( 0 ) ;
break ;
}
2017-03-29 23:29:34 +00:00
}
}
2017-04-08 18:48:12 +00:00
for ( int b = 0 ; b < 7 ; b + + )
{
logData . m_values . push_back ( packedButtons [ b ] ) ;
}
2017-04-12 22:02:47 +00:00
logData . m_values . push_back ( event . m_deviceType ) ;
2017-04-08 18:48:12 +00:00
appendMinitaurLogData ( m_logFileHandle , m_structTypes , logData ) ;
2017-03-29 23:29:34 +00:00
2017-04-08 18:48:12 +00:00
event . m_numButtonEvents = 0 ;
event . m_numMoveEvents = 0 ;
for ( int b = 0 ; b < MAX_VR_BUTTONS ; b + + )
{
event . m_buttons [ b ] = 0 ;
}
2017-03-29 23:29:34 +00:00
}
}
}
fflush ( m_logFileHandle ) ;
m_loggingTimeStamp + + ;
}
}
} ;
2017-02-18 01:41:57 +00:00
struct GenericRobotStateLogger : public InternalStateLogger
{
float m_loggingTimeStamp ;
std : : string m_fileName ;
FILE * m_logFileHandle ;
std : : string m_structTypes ;
2017-09-22 14:53:21 +00:00
const btMultiBodyDynamicsWorld * m_dynamicsWorld ;
2017-02-20 20:17:12 +00:00
btAlignedObjectArray < int > m_bodyIdList ;
bool m_filterObjectUniqueId ;
2017-03-06 05:49:20 +00:00
int m_maxLogDof ;
2017-08-31 02:41:15 +00:00
int m_logFlags ;
2017-03-06 05:49:20 +00:00
2017-09-22 14:53:21 +00:00
GenericRobotStateLogger ( int loggingUniqueId , const std : : string & fileName , const btMultiBodyDynamicsWorld * dynamicsWorld , int maxLogDof , int logFlags )
2017-02-18 01:41:57 +00:00
: m_loggingTimeStamp ( 0 ) ,
m_logFileHandle ( 0 ) ,
2017-02-25 00:01:18 +00:00
m_dynamicsWorld ( dynamicsWorld ) ,
2017-03-06 05:49:20 +00:00
m_filterObjectUniqueId ( false ) ,
2017-08-31 02:41:15 +00:00
m_maxLogDof ( maxLogDof ) ,
m_logFlags ( logFlags )
2017-02-18 01:41:57 +00:00
{
2017-03-07 19:52:26 +00:00
m_loggingUniqueId = loggingUniqueId ;
2017-02-18 01:41:57 +00:00
m_loggingType = STATE_LOGGING_GENERIC_ROBOT ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
btAlignedObjectArray < std : : string > structNames ;
2017-03-04 21:19:43 +00:00
structNames . push_back ( " stepCount " ) ;
2017-02-18 01:41:57 +00:00
structNames . push_back ( " timeStamp " ) ;
structNames . push_back ( " objectId " ) ;
structNames . push_back ( " posX " ) ;
structNames . push_back ( " posY " ) ;
structNames . push_back ( " posZ " ) ;
structNames . push_back ( " oriX " ) ;
structNames . push_back ( " oriY " ) ;
structNames . push_back ( " oriZ " ) ;
structNames . push_back ( " oriW " ) ;
structNames . push_back ( " velX " ) ;
structNames . push_back ( " velY " ) ;
structNames . push_back ( " velZ " ) ;
structNames . push_back ( " omegaX " ) ;
structNames . push_back ( " omegaY " ) ;
structNames . push_back ( " omegaZ " ) ;
structNames . push_back ( " qNum " ) ;
2017-03-06 05:49:20 +00:00
2017-04-12 22:02:47 +00:00
m_structTypes = " IfifffffffffffffI " ;
2017-03-06 05:49:20 +00:00
for ( int i = 0 ; i < m_maxLogDof ; i + + )
{
m_structTypes . append ( " f " ) ;
char jointName [ 256 ] ;
sprintf ( jointName , " q%d " , i ) ;
structNames . push_back ( jointName ) ;
}
2017-08-31 02:41:15 +00:00
2017-03-06 05:49:20 +00:00
for ( int i = 0 ; i < m_maxLogDof ; i + + )
{
m_structTypes . append ( " f " ) ;
char jointName [ 256 ] ;
2017-09-02 18:35:54 +00:00
sprintf ( jointName , " u%d " , i ) ;
2017-03-06 05:49:20 +00:00
structNames . push_back ( jointName ) ;
}
2017-08-31 02:41:15 +00:00
if ( m_logFlags & STATE_LOG_JOINT_TORQUES )
{
for ( int i = 0 ; i < m_maxLogDof ; i + + )
{
m_structTypes . append ( " f " ) ;
char jointName [ 256 ] ;
2017-09-02 18:35:54 +00:00
sprintf ( jointName , " t%d " , i ) ;
2017-08-31 02:41:15 +00:00
structNames . push_back ( jointName ) ;
}
}
2017-02-18 01:41:57 +00:00
const char * fileNameC = fileName . c_str ( ) ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
m_logFileHandle = createMinitaurLogFile ( fileNameC , structNames , m_structTypes ) ;
}
virtual void stop ( )
{
if ( m_logFileHandle )
{
closeMinitaurLogFile ( m_logFileHandle ) ;
m_logFileHandle = 0 ;
}
}
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
virtual void logState ( btScalar timeStep )
{
if ( m_logFileHandle )
{
for ( int i = 0 ; i < m_dynamicsWorld - > getNumMultibodies ( ) ; i + + )
{
2017-09-22 14:53:21 +00:00
const btMultiBody * mb = m_dynamicsWorld - > getMultiBody ( i ) ;
2017-02-20 20:17:12 +00:00
int objectUniqueId = mb - > getUserIndex2 ( ) ;
if ( m_filterObjectUniqueId & & m_bodyIdList . findLinearSearch2 ( objectUniqueId ) < 0 )
{
continue ;
}
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
MinitaurLogRecord logData ;
2017-03-04 21:19:43 +00:00
int stepCount = m_loggingTimeStamp ;
float timeStamp = m_loggingTimeStamp * m_dynamicsWorld - > getSolverInfo ( ) . m_timeStep ;
logData . m_values . push_back ( stepCount ) ;
logData . m_values . push_back ( timeStamp ) ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
btVector3 pos = mb - > getBasePos ( ) ;
2017-02-22 21:30:28 +00:00
btQuaternion ori = mb - > getWorldToBaseRot ( ) . inverse ( ) ;
2017-02-18 01:41:57 +00:00
btVector3 vel = mb - > getBaseVel ( ) ;
btVector3 omega = mb - > getBaseOmega ( ) ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
float posX = pos [ 0 ] ;
float posY = pos [ 1 ] ;
float posZ = pos [ 2 ] ;
float oriX = ori . x ( ) ;
float oriY = ori . y ( ) ;
float oriZ = ori . z ( ) ;
float oriW = ori . w ( ) ;
float velX = vel [ 0 ] ;
float velY = vel [ 1 ] ;
float velZ = vel [ 2 ] ;
float omegaX = omega [ 0 ] ;
float omegaY = omega [ 1 ] ;
float omegaZ = omega [ 2 ] ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
logData . m_values . push_back ( objectUniqueId ) ;
logData . m_values . push_back ( posX ) ;
logData . m_values . push_back ( posY ) ;
logData . m_values . push_back ( posZ ) ;
logData . m_values . push_back ( oriX ) ;
logData . m_values . push_back ( oriY ) ;
logData . m_values . push_back ( oriZ ) ;
logData . m_values . push_back ( oriW ) ;
logData . m_values . push_back ( velX ) ;
logData . m_values . push_back ( velY ) ;
logData . m_values . push_back ( velZ ) ;
logData . m_values . push_back ( omegaX ) ;
logData . m_values . push_back ( omegaY ) ;
logData . m_values . push_back ( omegaZ ) ;
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
int numDofs = mb - > getNumDofs ( ) ;
logData . m_values . push_back ( numDofs ) ;
2017-03-02 07:37:01 +00:00
int numJoints = mb - > getNumLinks ( ) ;
2018-03-15 03:47:56 +00:00
2017-03-02 07:37:01 +00:00
for ( int j = 0 ; j < numJoints ; + + j )
2017-02-18 01:41:57 +00:00
{
2017-03-02 07:37:01 +00:00
if ( mb - > getLink ( j ) . m_jointType = = 0 | | mb - > getLink ( j ) . m_jointType = = 1 )
2017-02-18 01:41:57 +00:00
{
float q = mb - > getJointPos ( j ) ;
logData . m_values . push_back ( q ) ;
}
2017-03-02 07:37:01 +00:00
}
2017-03-06 05:49:20 +00:00
for ( int j = numDofs ; j < m_maxLogDof ; + + j )
2017-03-02 07:37:01 +00:00
{
float q = 0.0 ;
logData . m_values . push_back ( q ) ;
2017-02-18 01:41:57 +00:00
}
2018-03-15 03:47:56 +00:00
2017-03-02 07:37:01 +00:00
for ( int j = 0 ; j < numJoints ; + + j )
2017-02-18 01:41:57 +00:00
{
2017-03-02 07:37:01 +00:00
if ( mb - > getLink ( j ) . m_jointType = = 0 | | mb - > getLink ( j ) . m_jointType = = 1 )
2017-02-18 01:41:57 +00:00
{
2017-08-31 02:41:15 +00:00
float v = mb - > getJointVel ( j ) ;
logData . m_values . push_back ( v ) ;
2017-02-18 01:41:57 +00:00
}
}
2017-03-06 05:49:20 +00:00
for ( int j = numDofs ; j < m_maxLogDof ; + + j )
2017-03-02 07:37:01 +00:00
{
2017-08-31 02:41:15 +00:00
float v = 0.0 ;
logData . m_values . push_back ( v ) ;
2017-03-02 07:37:01 +00:00
}
2017-08-31 02:41:15 +00:00
2018-03-15 03:47:56 +00:00
2017-08-31 02:41:15 +00:00
if ( m_logFlags & STATE_LOG_JOINT_TORQUES )
{
for ( int j = 0 ; j < numJoints ; + + j )
{
if ( mb - > getLink ( j ) . m_jointType = = 0 | | mb - > getLink ( j ) . m_jointType = = 1 )
{
float jointTorque = 0 ;
if ( m_logFlags & STATE_LOG_JOINT_MOTOR_TORQUES )
{
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( j ) . m_userPtr ;
if ( motor )
{
jointTorque + = motor - > getAppliedImpulse ( 0 ) / timeStep ;
}
}
if ( m_logFlags & STATE_LOG_JOINT_USER_TORQUES )
{
if ( mb - > getLink ( j ) . m_jointType = = 0 | | mb - > getLink ( j ) . m_jointType = = 1 )
{
jointTorque + = mb - > getJointTorque ( j ) ; //these are the 'user' applied external torques
}
}
logData . m_values . push_back ( jointTorque ) ;
}
}
for ( int j = numDofs ; j < m_maxLogDof ; + + j )
{
float u = 0.0 ;
logData . m_values . push_back ( u ) ;
}
}
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
//at the moment, appendMinitaurLogData will directly write to disk (potential delay)
//better to fill a huge memory buffer and once in a while write it to disk
appendMinitaurLogData ( m_logFileHandle , m_structTypes , logData ) ;
fflush ( m_logFileHandle ) ;
}
2018-03-15 03:47:56 +00:00
2017-02-18 01:41:57 +00:00
m_loggingTimeStamp + + ;
}
}
} ;
2017-04-02 22:09:40 +00:00
struct ContactPointsStateLogger : public InternalStateLogger
{
int m_loggingTimeStamp ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
std : : string m_fileName ;
FILE * m_logFileHandle ;
std : : string m_structTypes ;
btMultiBodyDynamicsWorld * m_dynamicsWorld ;
bool m_filterLinkA ;
bool m_filterLinkB ;
int m_linkIndexA ;
int m_linkIndexB ;
2017-04-04 17:38:25 +00:00
int m_bodyUniqueIdA ;
int m_bodyUniqueIdB ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
ContactPointsStateLogger ( int loggingUniqueId , const std : : string & fileName , btMultiBodyDynamicsWorld * dynamicsWorld )
: m_loggingTimeStamp ( 0 ) ,
m_fileName ( fileName ) ,
m_logFileHandle ( 0 ) ,
m_dynamicsWorld ( dynamicsWorld ) ,
m_filterLinkA ( false ) ,
m_filterLinkB ( false ) ,
m_linkIndexA ( - 2 ) ,
m_linkIndexB ( - 2 ) ,
2017-04-04 17:38:25 +00:00
m_bodyUniqueIdA ( - 1 ) ,
m_bodyUniqueIdB ( - 1 )
2017-04-02 22:09:40 +00:00
{
m_loggingUniqueId = loggingUniqueId ;
m_loggingType = STATE_LOGGING_CONTACT_POINTS ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
btAlignedObjectArray < std : : string > structNames ;
structNames . push_back ( " stepCount " ) ;
structNames . push_back ( " timeStamp " ) ;
structNames . push_back ( " contactFlag " ) ;
structNames . push_back ( " bodyUniqueIdA " ) ;
structNames . push_back ( " bodyUniqueIdB " ) ;
structNames . push_back ( " linkIndexA " ) ;
structNames . push_back ( " linkIndexB " ) ;
structNames . push_back ( " positionOnAX " ) ;
structNames . push_back ( " positionOnAY " ) ;
structNames . push_back ( " positionOnAZ " ) ;
structNames . push_back ( " positionOnBX " ) ;
structNames . push_back ( " positionOnBY " ) ;
structNames . push_back ( " positionOnBZ " ) ;
structNames . push_back ( " contactNormalOnBX " ) ;
structNames . push_back ( " contactNormalOnBY " ) ;
structNames . push_back ( " contactNormalOnBZ " ) ;
structNames . push_back ( " contactDistance " ) ;
structNames . push_back ( " normalForce " ) ;
2017-04-12 22:02:47 +00:00
m_structTypes = " IfIiiiifffffffffff " ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
const char * fileNameC = fileName . c_str ( ) ;
m_logFileHandle = createMinitaurLogFile ( fileNameC , structNames , m_structTypes ) ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
}
virtual void stop ( )
{
if ( m_logFileHandle )
{
closeMinitaurLogFile ( m_logFileHandle ) ;
m_logFileHandle = 0 ;
}
}
virtual void logState ( btScalar timeStep )
{
if ( m_logFileHandle )
{
int numContactManifolds = m_dynamicsWorld - > getDispatcher ( ) - > getNumManifolds ( ) ;
for ( int i = 0 ; i < numContactManifolds ; i + + )
{
const btPersistentManifold * manifold = m_dynamicsWorld - > getDispatcher ( ) - > getInternalManifoldPointer ( ) [ i ] ;
int linkIndexA = - 1 ;
int linkIndexB = - 1 ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
int objectIndexB = - 1 ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
const btRigidBody * bodyB = btRigidBody : : upcast ( manifold - > getBody1 ( ) ) ;
if ( bodyB )
{
objectIndexB = bodyB - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
if ( mblB & & mblB - > m_multiBody )
{
linkIndexB = mblB - > m_link ;
objectIndexB = mblB - > m_multiBody - > getUserIndex2 ( ) ;
if ( m_filterLinkB & & ( m_linkIndexB ! = linkIndexB ) )
{
continue ;
}
}
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
int objectIndexA = - 1 ;
const btRigidBody * bodyA = btRigidBody : : upcast ( manifold - > getBody0 ( ) ) ;
if ( bodyA )
{
objectIndexA = bodyA - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
if ( mblA & & mblA - > m_multiBody )
{
linkIndexA = mblA - > m_link ;
objectIndexA = mblA - > m_multiBody - > getUserIndex2 ( ) ;
if ( m_filterLinkA & & ( m_linkIndexA ! = linkIndexA ) )
{
continue ;
}
}
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
btAssert ( bodyA | | mblA ) ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
//apply the filter, if the user provides it
2017-04-04 17:38:25 +00:00
if ( m_bodyUniqueIdA > = 0 )
2017-04-02 22:09:40 +00:00
{
2017-04-04 17:38:25 +00:00
if ( ( m_bodyUniqueIdA ! = objectIndexA ) & &
( m_bodyUniqueIdA ! = objectIndexB ) )
2017-04-02 22:09:40 +00:00
continue ;
}
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
//apply the second object filter, if the user provides it
2017-04-04 17:38:25 +00:00
if ( m_bodyUniqueIdB > = 0 )
2017-04-02 22:09:40 +00:00
{
2017-04-04 17:38:25 +00:00
if ( ( m_bodyUniqueIdB ! = objectIndexA ) & &
( m_bodyUniqueIdB ! = objectIndexB ) )
2017-04-02 22:09:40 +00:00
continue ;
}
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
for ( int p = 0 ; p < manifold - > getNumContacts ( ) ; p + + )
{
MinitaurLogRecord logData ;
int stepCount = m_loggingTimeStamp ;
float timeStamp = m_loggingTimeStamp * timeStep ;
logData . m_values . push_back ( stepCount ) ;
logData . m_values . push_back ( timeStamp ) ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
const btManifoldPoint & srcPt = manifold - > getContactPoint ( p ) ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
logData . m_values . push_back ( 0 ) ; // reserved contact flag
logData . m_values . push_back ( objectIndexA ) ;
logData . m_values . push_back ( objectIndexB ) ;
logData . m_values . push_back ( linkIndexA ) ;
logData . m_values . push_back ( linkIndexB ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnA ( ) [ 0 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnA ( ) [ 1 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnA ( ) [ 2 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnB ( ) [ 0 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnB ( ) [ 1 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getPositionWorldOnB ( ) [ 2 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . m_normalWorldOnB [ 0 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . m_normalWorldOnB [ 1 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . m_normalWorldOnB [ 2 ] ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getDistance ( ) ) ) ;
logData . m_values . push_back ( ( float ) ( srcPt . getAppliedImpulse ( ) / timeStep ) ) ;
2018-03-15 03:47:56 +00:00
2017-04-02 22:09:40 +00:00
appendMinitaurLogData ( m_logFileHandle , m_structTypes , logData ) ;
fflush ( m_logFileHandle ) ;
}
}
m_loggingTimeStamp + + ;
}
}
} ;
2017-12-31 23:37:16 +00:00
struct SaveStateData
{
bParse : : btBulletFile * m_bulletFile ;
btSerializer * m_serializer ;
} ;
2015-11-23 04:50:32 +00:00
struct PhysicsServerCommandProcessorInternalData
{
///handle management
2017-06-30 20:35:07 +00:00
b3ResizablePool < InternalTextureHandle > m_textureHandles ;
2017-05-03 17:49:04 +00:00
b3ResizablePool < InternalBodyHandle > m_bodyHandles ;
2017-06-03 17:57:56 +00:00
b3ResizablePool < InternalCollisionShapeHandle > m_userCollisionShapeHandles ;
2017-10-08 01:50:23 +00:00
b3ResizablePool < InternalVisualShapeHandle > m_userVisualShapeHandles ;
2018-07-04 12:25:48 +00:00
b3ResizablePool < b3PoolBodyHandle < SharedMemoryUserData > > m_userDataHandles ;
2018-07-03 15:45:19 +00:00
btHashMap < SharedMemoryUserDataHashKey , int > m_userDataHandleLookup ;
2016-06-24 14:31:17 +00:00
2017-09-23 02:17:57 +00:00
b3PluginManager m_pluginManager ;
2017-10-05 18:43:14 +00:00
bool m_useRealTimeSimulation ;
2018-03-15 03:47:56 +00:00
2016-06-24 14:31:17 +00:00
2017-04-08 05:53:36 +00:00
b3VRControllerEvents m_vrControllerEvents ;
2017-12-31 23:37:16 +00:00
btAlignedObjectArray < SaveStateData > m_savedStates ;
2017-04-08 05:53:36 +00:00
2017-03-02 20:33:22 +00:00
btAlignedObjectArray < b3KeyboardEvent > m_keyboardEvents ;
2017-06-17 20:29:14 +00:00
btAlignedObjectArray < b3MouseEvent > m_mouseEvents ;
2015-11-23 04:50:32 +00:00
CommandLogger * m_commandLogger ;
2018-06-12 23:56:45 +00:00
int m_commandLoggingUid ;
2016-06-24 14:31:17 +00:00
2018-06-12 23:56:45 +00:00
CommandLogPlayback * m_logPlayback ;
int m_logPlaybackUid ;
2015-11-23 04:50:32 +00:00
btScalar m_physicsDeltaTime ;
2016-08-24 21:25:06 +00:00
btScalar m_numSimulationSubSteps ;
2015-11-23 04:50:32 +00:00
btAlignedObjectArray < btMultiBodyJointFeedback * > m_multiBodyJointFeedbacks ;
2017-05-05 00:51:40 +00:00
b3HashMap < btHashPtr , btInverseDynamics : : MultiBodyTree * > m_inverseDynamicsBodies ;
b3HashMap < btHashPtr , IKTrajectoryHelper * > m_inverseKinematicsHelpers ;
2018-03-15 03:47:56 +00:00
2016-11-14 22:08:05 +00:00
int m_userConstraintUIDGenerator ;
2017-05-05 00:51:40 +00:00
b3HashMap < btHashInt , InteralUserConstraintData > m_userConstraints ;
2016-11-14 22:08:05 +00:00
2016-10-13 06:03:36 +00:00
b3AlignedObjectArray < SaveWorldObjectData > m_saveWorldBodyData ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
2017-11-23 02:12:02 +00:00
btAlignedObjectArray < btMultiBodyWorldImporter * > m_worldImporters ;
2018-06-05 22:59:01 +00:00
2015-11-23 04:50:32 +00:00
btAlignedObjectArray < std : : string * > m_strings ;
btAlignedObjectArray < btCollisionShape * > m_collisionShapes ;
2018-02-22 07:22:16 +00:00
btAlignedObjectArray < int > m_allocatedTextures ;
2018-01-08 20:25:56 +00:00
btHashMap < btHashPtr , UrdfCollision > m_bulletCollisionShape2UrdfCollision ;
2017-08-24 16:16:11 +00:00
btAlignedObjectArray < btStridingMeshInterface * > m_meshInterfaces ;
2017-01-16 16:23:49 +00:00
MyOverlapFilterCallback * m_broadphaseCollisionFilterCallback ;
btHashedOverlappingPairCache * m_pairCache ;
2015-11-23 04:50:32 +00:00
btBroadphaseInterface * m_broadphase ;
btCollisionDispatcher * m_dispatcher ;
btMultiBodyConstraintSolver * m_solver ;
btDefaultCollisionConfiguration * m_collisionConfiguration ;
2018-03-15 03:47:56 +00:00
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
btSoftMultiBodyDynamicsWorld * m_dynamicsWorld ;
2016-10-19 00:38:43 +00:00
btSoftBodySolver * m_softbodySolver ;
# else
btMultiBodyDynamicsWorld * m_dynamicsWorld ;
# endif
2018-03-15 03:47:56 +00:00
2018-08-24 06:04:17 +00:00
int m_constraintSolverType ;
2015-11-23 04:50:32 +00:00
SharedMemoryDebugDrawer * m_remoteDebugDrawer ;
2018-03-15 03:47:56 +00:00
2016-09-01 20:30:07 +00:00
btAlignedObjectArray < b3ContactPointData > m_cachedContactPoints ;
2016-11-10 19:22:22 +00:00
MyBroadphaseCallback m_cachedOverlappingObjects ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
btAlignedObjectArray < int > m_sdfRecentLoadedBodies ;
2018-03-15 03:47:56 +00:00
2017-02-17 22:25:53 +00:00
btAlignedObjectArray < InternalStateLogger * > m_stateLoggers ;
int m_stateLoggersUniqueId ;
2017-05-05 00:51:40 +00:00
int m_profileTimingLoggingUid ;
std : : string m_profileTimingFileName ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
struct GUIHelperInterface * m_guiHelper ;
int m_sharedMemoryKey ;
2017-10-06 20:46:24 +00:00
bool m_enableTinyRenderer ;
2015-11-23 04:50:32 +00:00
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 ;
2017-03-20 17:58:07 +00:00
int m_savedActivationState ;
2015-11-23 04:50:32 +00:00
class btTypedConstraint * m_pickedConstraint ;
class btMultiBodyPoint2Point * m_pickingMultiBodyPoint2Point ;
btVector3 m_oldPickingPos ;
btVector3 m_hitPos ;
btScalar m_oldPickingDist ;
bool m_prevCanSleep ;
2018-06-05 01:41:41 +00:00
int m_pdControlPlugin ;
2018-07-27 13:48:24 +00:00
int m_collisionFilterPlugin ;
2017-04-29 17:32:30 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
b3SoundEngine m_soundEngine ;
# endif
2015-11-23 04:50:32 +00:00
2017-05-05 00:51:40 +00:00
b3HashMap < b3HashString , char * > m_profileEvents ;
2018-05-23 03:26:00 +00:00
b3HashMap < b3HashString , UrdfVisualShapeCache > m_cachedVUrdfisualShapes ;
2017-05-05 00:51:40 +00:00
2018-06-19 16:28:48 +00:00
b3ThreadPool * m_threadPool ;
2018-06-16 16:37:53 +00:00
2017-09-23 16:25:00 +00:00
PhysicsServerCommandProcessorInternalData ( PhysicsCommandProcessorInterface * proc )
: m_pluginManager ( proc ) ,
2017-10-05 18:43:14 +00:00
m_useRealTimeSimulation ( false ) ,
2015-11-23 04:50:32 +00:00
m_commandLogger ( 0 ) ,
2018-06-12 23:56:45 +00:00
m_commandLoggingUid ( - 1 ) ,
2015-11-23 04:50:32 +00:00
m_logPlayback ( 0 ) ,
2018-06-12 23:56:45 +00:00
m_logPlaybackUid ( - 1 ) ,
2015-11-23 04:50:32 +00:00
m_physicsDeltaTime ( 1. / 240. ) ,
2016-08-24 21:25:06 +00:00
m_numSimulationSubSteps ( 0 ) ,
2016-11-14 22:08:05 +00:00
m_userConstraintUIDGenerator ( 1 ) ,
2017-01-16 16:23:49 +00:00
m_broadphaseCollisionFilterCallback ( 0 ) ,
m_pairCache ( 0 ) ,
m_broadphase ( 0 ) ,
m_dispatcher ( 0 ) ,
m_solver ( 0 ) ,
m_collisionConfiguration ( 0 ) ,
2015-11-23 04:50:32 +00:00
m_dynamicsWorld ( 0 ) ,
2018-08-24 06:04:17 +00:00
m_constraintSolverType ( - 1 ) ,
2015-11-23 04:50:32 +00:00
m_remoteDebugDrawer ( 0 ) ,
2017-02-17 22:25:53 +00:00
m_stateLoggersUniqueId ( 0 ) ,
2017-05-05 00:51:40 +00:00
m_profileTimingLoggingUid ( - 1 ) ,
2015-11-23 04:50:32 +00:00
m_guiHelper ( 0 ) ,
m_sharedMemoryKey ( SHARED_MEMORY_KEY ) ,
2017-10-06 20:46:24 +00:00
m_enableTinyRenderer ( true ) ,
2017-10-07 00:30:10 +00:00
m_verboseOutput ( false ) ,
2015-11-23 04:50:32 +00:00
m_pickedBody ( 0 ) ,
m_pickedConstraint ( 0 ) ,
2018-06-05 01:41:41 +00:00
m_pickingMultiBodyPoint2Point ( 0 ) ,
2018-06-16 16:37:53 +00:00
m_pdControlPlugin ( - 1 ) ,
2018-07-27 13:48:24 +00:00
m_collisionFilterPlugin ( - 1 ) ,
2018-06-19 16:28:48 +00:00
m_threadPool ( 0 )
2015-11-23 04:50:32 +00:00
{
2017-09-24 01:05:23 +00:00
{
//register static plugins:
2017-10-03 22:00:52 +00:00
# ifdef STATIC_LINK_VR_PLUGIN
2018-06-05 01:41:41 +00:00
m_pluginManager . registerStaticLinkedPlugin ( " vrSyncPlugin " , initPlugin_vrSyncPlugin , exitPlugin_vrSyncPlugin , executePluginCommand_vrSyncPlugin , preTickPluginCallback_vrSyncPlugin , 0 , 0 ) ;
2017-10-03 22:00:52 +00:00
# endif //STATIC_LINK_VR_PLUGIN
2018-06-05 22:59:01 +00:00
# ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
2018-06-05 01:41:41 +00:00
{
2018-06-05 22:59:01 +00:00
m_pdControlPlugin = m_pluginManager . registerStaticLinkedPlugin ( " pdControlPlugin " , initPlugin_pdControlPlugin , exitPlugin_pdControlPlugin , executePluginCommand_pdControlPlugin , preTickPluginCallback_pdControlPlugin , 0 , 0 ) ;
2018-06-05 01:41:41 +00:00
}
2018-06-05 22:59:01 +00:00
# endif //SKIP_STATIC_PD_CONTROL_PLUGIN
2018-06-05 01:41:41 +00:00
2018-07-27 13:48:24 +00:00
# ifndef SKIP_COLLISION_FILTER_PLUGIN
{
m_collisionFilterPlugin = m_pluginManager . registerStaticLinkedPlugin ( " collisionFilterPlugin " , initPlugin_collisionFilterPlugin , exitPlugin_collisionFilterPlugin , executePluginCommand_collisionFilterPlugin , 0 , 0 , 0 ) ;
}
# endif
2018-06-05 01:41:41 +00:00
2018-01-17 20:48:48 +00:00
# ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
int renderPluginId = m_pluginManager . registerStaticLinkedPlugin ( " tinyRendererPlugin " , initPlugin_tinyRendererPlugin , exitPlugin_tinyRendererPlugin , executePluginCommand_tinyRendererPlugin , 0 , 0 , getRenderInterface_tinyRendererPlugin ) ;
m_pluginManager . selectPluginRenderer ( renderPluginId ) ;
# endif
2017-09-24 01:05:23 +00:00
}
2017-04-08 05:53:36 +00:00
m_vrControllerEvents . init ( ) ;
2016-06-24 14:31:17 +00:00
2017-05-04 04:53:29 +00:00
m_bodyHandles . exitHandles ( ) ;
2017-05-03 17:49:04 +00:00
m_bodyHandles . initHandles ( ) ;
2017-06-03 17:57:56 +00:00
m_userCollisionShapeHandles . exitHandles ( ) ;
m_userCollisionShapeHandles . initHandles ( ) ;
2017-10-08 01:50:23 +00:00
m_userVisualShapeHandles . exitHandles ( ) ;
m_userVisualShapeHandles . initHandles ( ) ;
2015-11-23 04:50:32 +00:00
}
2016-09-13 22:37:46 +00:00
btInverseDynamics : : MultiBodyTree * findOrCreateTree ( btMultiBody * multiBody )
{
btInverseDynamics : : MultiBodyTree * tree = 0 ;
2018-03-15 03:47:56 +00:00
2016-09-13 22:37:46 +00:00
btInverseDynamics : : MultiBodyTree * * treePtrPtr =
m_inverseDynamicsBodies . find ( multiBody ) ;
2018-03-15 03:47:56 +00:00
2016-09-13 22:37:46 +00:00
if ( treePtrPtr )
{
tree = * treePtrPtr ;
}
else
{
btInverseDynamics : : btMultiBodyTreeCreator id_creator ;
if ( - 1 = = id_creator . createFromBtMultiBody ( multiBody , false ) )
{
2018-03-15 03:47:56 +00:00
2016-09-13 22:37:46 +00:00
}
else
{
tree = btInverseDynamics : : CreateMultiBodyTree ( id_creator ) ;
m_inverseDynamicsBodies . insert ( multiBody , tree ) ;
}
}
2018-03-15 03:47:56 +00:00
2016-09-13 22:37:46 +00:00
return tree ;
}
2015-11-23 04:50:32 +00:00
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
} ;
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 ;
2016-07-18 06:50:11 +00:00
2016-09-01 17:45:14 +00:00
2016-07-18 06:50:11 +00:00
2015-11-23 04:50:32 +00:00
}
PhysicsServerCommandProcessor : : PhysicsServerCommandProcessor ( )
2017-09-23 16:25:00 +00:00
: m_data ( 0 )
2015-11-23 04:50:32 +00:00
{
2017-09-23 16:25:00 +00:00
m_data = new PhysicsServerCommandProcessorInternalData ( this ) ;
2015-11-23 04:50:32 +00:00
createEmptyDynamicsWorld ( ) ;
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
}
PhysicsServerCommandProcessor : : ~ PhysicsServerCommandProcessor ( )
{
deleteDynamicsWorld ( ) ;
if ( m_data - > m_commandLogger )
{
delete m_data - > m_commandLogger ;
m_data - > m_commandLogger = 0 ;
}
2017-05-05 00:51:40 +00:00
for ( int i = 0 ; i < m_data - > m_profileEvents . size ( ) ; i + + )
{
char * event = * m_data - > m_profileEvents . getAtIndex ( i ) ;
delete [ ] event ;
}
2018-06-19 16:28:48 +00:00
if ( m_data - > m_threadPool )
delete m_data - > m_threadPool ;
2018-06-16 16:37:53 +00:00
2015-11-23 04:50:32 +00:00
delete m_data ;
}
2017-09-24 01:05:23 +00:00
void preTickCallback ( btDynamicsWorld * world , btScalar timeStep )
{
PhysicsServerCommandProcessor * proc = ( PhysicsServerCommandProcessor * ) world - > getWorldUserInfo ( ) ;
bool isPreTick = true ;
proc - > tickPlugins ( timeStep , isPreTick ) ;
}
2017-02-17 18:47:55 +00:00
void logCallback ( btDynamicsWorld * world , btScalar timeStep )
{
2017-04-29 17:32:30 +00:00
//handle the logging and playing sounds
2017-02-17 18:47:55 +00:00
PhysicsServerCommandProcessor * proc = ( PhysicsServerCommandProcessor * ) world - > getWorldUserInfo ( ) ;
2017-04-29 17:32:30 +00:00
proc - > processCollisionForces ( timeStep ) ;
2017-02-17 18:47:55 +00:00
proc - > logObjectStates ( timeStep ) ;
2018-03-15 03:47:56 +00:00
2017-09-24 01:05:23 +00:00
bool isPreTick = false ;
proc - > tickPlugins ( timeStep , isPreTick ) ;
2017-02-17 18:47:55 +00:00
}
2017-02-17 22:25:53 +00:00
2017-04-29 17:32:30 +00:00
bool MyContactAddedCallback ( btManifoldPoint & cp , const btCollisionObjectWrapper * colObj0Wrap , int partId0 , int index0 , const btCollisionObjectWrapper * colObj1Wrap , int partId1 , int index1 )
{
2018-02-17 03:44:33 +00:00
btAdjustInternalEdgeContacts ( cp , colObj1Wrap , colObj0Wrap , partId1 , index1 ) ;
2017-04-29 17:32:30 +00:00
return true ;
}
bool MyContactDestroyedCallback ( void * userPersistentData )
{
//printf("destroyed\n");
return false ;
}
bool MyContactProcessedCallback ( btManifoldPoint & cp , void * body0 , void * body1 )
{
//printf("processed\n");
return false ;
}
void MyContactStartedCallback ( btPersistentManifold * const & manifold )
{
//printf("started\n");
}
void MyContactEndedCallback ( btPersistentManifold * const & manifold )
{
// printf("ended\n");
}
void PhysicsServerCommandProcessor : : processCollisionForces ( btScalar timeStep )
{
# ifdef B3_ENABLE_TINY_AUDIO
//this is experimental at the moment: impulse thresholds, sound parameters will be exposed in C-API/pybullet.
//audio will go into a wav file, as well as real-time output to speakers/headphones using RtAudio/DAC.
int numContactManifolds = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getNumManifolds ( ) ;
for ( int i = 0 ; i < numContactManifolds ; i + + )
{
const btPersistentManifold * manifold = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getInternalManifoldPointer ( ) [ i ] ;
2017-05-01 18:14:09 +00:00
bool objHasSound [ 2 ] ;
objHasSound [ 0 ] = ( 0 ! = ( manifold - > getBody0 ( ) - > getCollisionFlags ( ) & btCollisionObject : : CF_HAS_COLLISION_SOUND_TRIGGER ) ) ;
objHasSound [ 1 ] = ( 0 ! = ( manifold - > getBody1 ( ) - > getCollisionFlags ( ) & btCollisionObject : : CF_HAS_COLLISION_SOUND_TRIGGER ) ) ;
const btCollisionObject * colObjs [ 2 ] = { manifold - > getBody0 ( ) , manifold - > getBody1 ( ) } ;
for ( int ob = 0 ; ob < 2 ; ob + + )
{
if ( objHasSound [ ob ] )
2017-04-29 17:32:30 +00:00
{
2017-05-01 18:14:09 +00:00
int uid0 = - 1 ;
int linkIndex = - 2 ;
const btMultiBodyLinkCollider * mblB = btMultiBodyLinkCollider : : upcast ( colObjs [ ob ] ) ;
if ( mblB & & mblB - > m_multiBody )
2017-04-29 17:32:30 +00:00
{
2017-05-01 18:14:09 +00:00
linkIndex = mblB - > m_link ;
uid0 = mblB - > m_multiBody - > getUserIndex2 ( ) ;
}
const btRigidBody * bodyB = btRigidBody : : upcast ( colObjs [ ob ] ) ;
if ( bodyB )
{
uid0 = bodyB - > getUserIndex2 ( ) ;
linkIndex = - 1 ;
}
2017-04-29 17:32:30 +00:00
2017-05-01 18:14:09 +00:00
if ( ( uid0 < 0 ) | | ( linkIndex < - 1 ) )
continue ;
2017-04-29 17:32:30 +00:00
2017-05-03 17:49:04 +00:00
InternalBodyHandle * bodyHandle0 = m_data - > m_bodyHandles . getHandle ( uid0 ) ;
2017-05-01 18:14:09 +00:00
SDFAudioSource * audioSrc = bodyHandle0 - > m_audioSources [ linkIndex ] ;
if ( audioSrc = = 0 )
continue ;
2017-04-29 17:32:30 +00:00
2017-05-01 18:14:09 +00:00
for ( int p = 0 ; p < manifold - > getNumContacts ( ) ; p + + )
{
double imp = manifold - > getContactPoint ( p ) . getAppliedImpulse ( ) ;
//printf ("manifold %d, contact %d, lifeTime:%d, appliedImpulse:%f\n",i,p, manifold->getContactPoint(p).getLifeTime(),imp);
2017-04-29 17:32:30 +00:00
2017-05-01 18:14:09 +00:00
if ( imp > audioSrc - > m_collisionForceThreshold & & manifold - > getContactPoint ( p ) . getLifeTime ( ) = = 1 )
{
int soundSourceIndex = m_data - > m_soundEngine . getAvailableSoundSource ( ) ;
if ( soundSourceIndex > = 0 )
{
b3SoundMessage msg ;
msg . m_attackRate = audioSrc - > m_attackRate ;
msg . m_decayRate = audioSrc - > m_decayRate ;
msg . m_sustainLevel = audioSrc - > m_sustainLevel ;
msg . m_releaseRate = audioSrc - > m_releaseRate ;
msg . m_amplitude = audioSrc - > m_gain ;
msg . m_frequency = audioSrc - > m_pitch ;
msg . m_type = B3_SOUND_SOURCE_WAV_FILE ;
msg . m_wavId = audioSrc - > m_userIndex ;
msg . m_autoKeyOff = true ;
m_data - > m_soundEngine . startSound ( soundSourceIndex , msg ) ;
}
}
2017-04-29 17:32:30 +00:00
}
}
}
}
# endif //B3_ENABLE_TINY_AUDIO
}
2017-02-17 18:47:55 +00:00
2018-07-25 12:47:34 +00:00
void PhysicsServerCommandProcessor : : reportNotifications ( ) {
m_data - > m_pluginManager . reportNotifications ( ) ;
2018-07-24 12:12:16 +00:00
}
2017-09-24 01:05:23 +00:00
void PhysicsServerCommandProcessor : : tickPlugins ( btScalar timeStep , bool isPreTick )
{
m_data - > m_pluginManager . tickPlugins ( timeStep , isPreTick ) ;
2017-10-03 22:00:52 +00:00
if ( ! isPreTick )
{
//clear events after each postTick, so we don't receive events multiple ticks
m_data - > m_pluginManager . clearEvents ( ) ;
}
2017-09-24 01:05:23 +00:00
}
2017-02-17 18:47:55 +00:00
void PhysicsServerCommandProcessor : : logObjectStates ( btScalar timeStep )
{
2017-02-17 22:25:53 +00:00
for ( int i = 0 ; i < m_data - > m_stateLoggers . size ( ) ; i + + )
2017-02-17 18:47:55 +00:00
{
2017-02-17 22:25:53 +00:00
m_data - > m_stateLoggers [ i ] - > logState ( timeStep ) ;
2017-02-17 18:47:55 +00:00
}
}
2015-11-23 04:50:32 +00:00
2017-06-03 17:57:56 +00:00
struct ProgrammaticUrdfInterface : public URDFImporterInterface
{
int m_bodyUniqueId ;
const b3CreateMultiBodyArgs & m_createBodyArgs ;
mutable b3AlignedObjectArray < btCollisionShape * > m_allocatedCollisionShapes ;
PhysicsServerCommandProcessorInternalData * m_data ;
ProgrammaticUrdfInterface ( const b3CreateMultiBodyArgs & bodyArgs , PhysicsServerCommandProcessorInternalData * data )
: m_bodyUniqueId ( - 1 ) ,
m_createBodyArgs ( bodyArgs ) ,
m_data ( data )
{
}
virtual ~ ProgrammaticUrdfInterface ( )
{
}
virtual bool loadURDF ( const char * fileName , bool forceFixedBase = false )
{
b3Assert ( 0 ) ;
return false ;
}
virtual const char * getPathPrefix ( )
{
return " " ;
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex ( ) const
{
return m_createBodyArgs . m_baseLinkIndex ;
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std : : string getLinkName ( int linkIndex ) const
{
2017-06-19 20:15:05 +00:00
std : : string linkName = " link " ;
char numstr [ 21 ] ; // enough to hold all numbers up to 64-bits
sprintf ( numstr , " %d " , linkIndex ) ;
linkName = linkName + numstr ;
return linkName ;
2017-06-03 17:57:56 +00:00
}
//various derived class in internal source code break with new pure virtual methods, so provide some default implementation
virtual std : : string getBodyName ( ) const
{
return m_createBodyArgs . m_bodyName ;
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
2018-03-15 03:47:56 +00:00
virtual bool getLinkColor ( int linkIndex , btVector4 & colorRGBA ) const
{
2017-06-03 17:57:56 +00:00
b3Assert ( 0 ) ;
return false ;
}
2018-03-15 03:47:56 +00:00
virtual bool getLinkColor2 ( int linkIndex , struct UrdfMaterialColor & matCol ) const
{
2017-10-08 01:50:23 +00:00
if ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ linkIndex ] > = 0 )
{
const InternalVisualShapeHandle * visHandle = m_data - > m_userVisualShapeHandles . getHandle ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ linkIndex ] ) ;
if ( visHandle )
{
2018-03-29 02:08:18 +00:00
for ( int i = 0 ; i < visHandle - > m_visualShapes . size ( ) ; i + + )
2017-10-08 01:50:23 +00:00
{
2018-03-29 02:08:18 +00:00
if ( visHandle - > m_visualShapes [ i ] . m_geometry . m_hasLocalMaterial )
{
matCol = visHandle - > m_visualShapes [ i ] . m_geometry . m_localMaterial . m_matColor ;
return true ;
}
2017-10-08 01:50:23 +00:00
}
}
}
2017-06-03 17:57:56 +00:00
return false ;
}
2018-03-15 03:47:56 +00:00
virtual int getCollisionGroupAndMask ( int linkIndex , int & colGroup , int & colMask ) const
{
2017-06-03 17:57:56 +00:00
return 0 ;
}
///this API will likely change, don't override it!
2018-03-15 03:47:56 +00:00
virtual bool getLinkContactInfo ( int linkIndex , URDFLinkContactInfo & contactInfo ) const
{
2017-06-03 17:57:56 +00:00
return false ;
}
2018-03-15 03:47:56 +00:00
virtual bool getLinkAudioSource ( int linkIndex , SDFAudioSource & audioSource ) const
2017-06-03 17:57:56 +00:00
{
b3Assert ( 0 ) ;
return false ;
}
virtual std : : string getJointName ( int linkIndex ) const
{
2017-06-19 20:15:05 +00:00
std : : string jointName = " joint " ;
char numstr [ 21 ] ; // enough to hold all numbers up to 64-bits
sprintf ( numstr , " %d " , linkIndex ) ;
jointName = jointName + numstr ;
return jointName ;
2017-06-03 17:57:56 +00:00
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia ( int urdfLinkIndex , btScalar & mass , btVector3 & localInertiaDiagonal , btTransform & inertialFrame ) const
{
if ( urdfLinkIndex > = 0 & & urdfLinkIndex < m_createBodyArgs . m_numLinks )
{
mass = m_createBodyArgs . m_linkMasses [ urdfLinkIndex ] ;
localInertiaDiagonal . setValue (
m_createBodyArgs . m_linkInertias [ urdfLinkIndex * 3 + 0 ] ,
m_createBodyArgs . m_linkInertias [ urdfLinkIndex * 3 + 1 ] ,
m_createBodyArgs . m_linkInertias [ urdfLinkIndex * 3 + 2 ] ) ;
inertialFrame . setOrigin ( btVector3 (
m_createBodyArgs . m_linkInertialFramePositions [ urdfLinkIndex * 3 + 0 ] ,
m_createBodyArgs . m_linkInertialFramePositions [ urdfLinkIndex * 3 + 1 ] ,
m_createBodyArgs . m_linkInertialFramePositions [ urdfLinkIndex * 3 + 2 ] ) ) ;
inertialFrame . setRotation ( btQuaternion (
m_createBodyArgs . m_linkInertialFrameOrientations [ urdfLinkIndex * 4 + 0 ] ,
m_createBodyArgs . m_linkInertialFrameOrientations [ urdfLinkIndex * 4 + 1 ] ,
m_createBodyArgs . m_linkInertialFrameOrientations [ urdfLinkIndex * 4 + 2 ] ,
m_createBodyArgs . m_linkInertialFrameOrientations [ urdfLinkIndex * 4 + 3 ] ) ) ;
} else
2018-03-15 03:47:56 +00:00
{
2017-06-03 17:57:56 +00:00
mass = 0 ;
localInertiaDiagonal . setValue ( 0 , 0 , 0 ) ;
inertialFrame . setIdentity ( ) ;
}
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices ( int urdfLinkIndex , btAlignedObjectArray < int > & childLinkIndices ) const
{
2017-06-19 17:14:26 +00:00
for ( int i = 0 ; i < m_createBodyArgs . m_numLinks ; i + + )
{
2017-06-19 20:15:05 +00:00
if ( m_createBodyArgs . m_linkParentIndices [ i ] = = urdfLinkIndex )
2017-06-19 17:14:26 +00:00
{
childLinkIndices . push_back ( i ) ;
}
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
}
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
virtual bool getJointInfo ( int urdfLinkIndex , btTransform & parent2joint , btTransform & linkTransformInWorld , btVector3 & jointAxisInJointSpace , int & jointType , btScalar & jointLowerLimit , btScalar & jointUpperLimit , btScalar & jointDamping , btScalar & jointFriction ) const
{
return false ;
} ;
2018-03-15 03:47:56 +00:00
virtual bool getJointInfo2 ( int urdfLinkIndex , btTransform & parent2joint , btTransform & linkTransformInWorld , btVector3 & jointAxisInJointSpace , int & jointType , btScalar & jointLowerLimit , btScalar & jointUpperLimit , btScalar & jointDamping , btScalar & jointFriction , btScalar & jointMaxForce , btScalar & jointMaxVelocity ) const
2017-06-03 17:57:56 +00:00
{
2017-06-19 20:15:05 +00:00
bool isValid = false ;
int jointTypeOrg = m_createBodyArgs . m_linkJointTypes [ urdfLinkIndex ] ;
switch ( jointTypeOrg )
{
case eRevoluteType :
{
isValid = true ;
jointType = URDFRevoluteJoint ;
break ;
}
case ePrismaticType :
{
isValid = true ;
jointType = URDFPrismaticJoint ;
break ;
}
case eFixedType :
{
isValid = true ;
jointType = URDFFixedJoint ;
break ;
}
//case eSphericalType:
//case ePlanarType:
//case eFixedType:
//case ePoint2PointType:
//case eGearType:
default :
{
}
} ;
if ( isValid )
{
//backwards compatibility for custom file importers
jointMaxForce = 0 ;
jointMaxVelocity = 0 ;
jointFriction = 0 ;
jointDamping = 0 ;
jointLowerLimit = 1 ;
jointUpperLimit = - 1 ;
parent2joint . setOrigin ( btVector3 (
m_createBodyArgs . m_linkPositions [ urdfLinkIndex * 3 + 0 ] ,
m_createBodyArgs . m_linkPositions [ urdfLinkIndex * 3 + 1 ] ,
m_createBodyArgs . m_linkPositions [ urdfLinkIndex * 3 + 2 ] ) ) ;
parent2joint . setRotation ( btQuaternion (
m_createBodyArgs . m_linkOrientations [ urdfLinkIndex * 4 + 0 ] ,
m_createBodyArgs . m_linkOrientations [ urdfLinkIndex * 4 + 1 ] ,
m_createBodyArgs . m_linkOrientations [ urdfLinkIndex * 4 + 2 ] ,
m_createBodyArgs . m_linkOrientations [ urdfLinkIndex * 4 + 3 ]
) ) ;
2018-03-15 03:47:56 +00:00
2017-06-19 20:15:05 +00:00
linkTransformInWorld . setIdentity ( ) ;
jointAxisInJointSpace . setValue (
m_createBodyArgs . m_linkJointAxis [ 3 * urdfLinkIndex + 0 ] ,
m_createBodyArgs . m_linkJointAxis [ 3 * urdfLinkIndex + 1 ] ,
m_createBodyArgs . m_linkJointAxis [ 3 * urdfLinkIndex + 2 ] ) ;
2018-03-15 03:47:56 +00:00
2017-06-19 20:15:05 +00:00
}
return isValid ;
2017-06-03 17:57:56 +00:00
} ;
2018-03-15 03:47:56 +00:00
2017-06-03 17:57:56 +00:00
virtual bool getRootTransformInWorld ( btTransform & rootTransformInWorld ) const
{
2017-06-19 17:14:26 +00:00
int baseLinkIndex = m_createBodyArgs . m_baseLinkIndex ;
2017-06-03 17:57:56 +00:00
rootTransformInWorld . setOrigin ( btVector3 (
2017-06-19 17:14:26 +00:00
m_createBodyArgs . m_linkPositions [ baseLinkIndex * 3 + 0 ] ,
m_createBodyArgs . m_linkPositions [ baseLinkIndex * 3 + 1 ] ,
m_createBodyArgs . m_linkPositions [ baseLinkIndex * 3 + 2 ] ) ) ;
2017-06-03 17:57:56 +00:00
rootTransformInWorld . setRotation ( btQuaternion (
2017-06-19 17:14:26 +00:00
m_createBodyArgs . m_linkOrientations [ baseLinkIndex * 4 + 0 ] ,
m_createBodyArgs . m_linkOrientations [ baseLinkIndex * 4 + 1 ] ,
m_createBodyArgs . m_linkOrientations [ baseLinkIndex * 4 + 2 ] ,
m_createBodyArgs . m_linkOrientations [ baseLinkIndex * 4 + 3 ] ) ) ;
2017-06-03 17:57:56 +00:00
return true ;
}
virtual void setRootTransformInWorld ( const btTransform & rootTransformInWorld )
{
b3Assert ( 0 ) ;
}
2018-03-29 02:08:18 +00:00
virtual int convertLinkVisualShapes ( int linkIndex , const char * pathPrefix , const btTransform & localInertiaFrame ) const
2017-06-03 17:57:56 +00:00
{
2018-03-29 02:08:18 +00:00
int graphicsIndex = - 1 ;
double globalScaling = 1.f ; //todo!
int flags = 0 ;
BulletURDFImporter u2b ( m_data - > m_guiHelper , m_data - > m_pluginManager . getRenderInterface ( ) , globalScaling , flags ) ;
u2b . setEnableTinyRenderer ( m_data - > m_enableTinyRenderer ) ;
btAlignedObjectArray < GLInstanceVertex > vertices ;
btAlignedObjectArray < int > indices ;
btTransform startTrans ; startTrans . setIdentity ( ) ;
btAlignedObjectArray < BulletURDFTexture > textures ;
2017-10-08 01:50:23 +00:00
if ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ linkIndex ] > = 0 )
{
2018-03-29 02:08:18 +00:00
InternalVisualShapeHandle * visHandle = m_data - > m_userVisualShapeHandles . getHandle ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ linkIndex ] ) ;
2017-10-08 01:50:23 +00:00
if ( visHandle )
{
2018-03-29 02:08:18 +00:00
if ( visHandle - > m_OpenGLGraphicsIndex > = 0 )
{
//instancing. assume the inertial frame is identical
graphicsIndex = visHandle - > m_OpenGLGraphicsIndex ;
} else
{
for ( int v = 0 ; v < visHandle - > m_visualShapes . size ( ) ; v + + )
{
u2b . convertURDFToVisualShapeInternal ( & visHandle - > m_visualShapes [ v ] , pathPrefix , localInertiaFrame . inverse ( ) * visHandle - > m_visualShapes [ v ] . m_linkLocalFrame , vertices , indices , textures ) ;
}
if ( vertices . size ( ) & & indices . size ( ) )
{
if ( 1 )
{
int textureIndex = - 1 ;
if ( textures . size ( ) )
{
textureIndex = m_data - > m_guiHelper - > registerTexture ( textures [ 0 ] . textureData1 , textures [ 0 ] . m_width , textures [ 0 ] . m_height ) ;
}
{
B3_PROFILE ( " registerGraphicsShape " ) ;
graphicsIndex = m_data - > m_guiHelper - > registerGraphicsShape ( & vertices [ 0 ] . xyzw [ 0 ] , vertices . size ( ) , & indices [ 0 ] , indices . size ( ) , B3_GL_TRIANGLES , textureIndex ) ;
visHandle - > m_OpenGLGraphicsIndex = graphicsIndex ;
}
}
}
}
2017-10-08 01:50:23 +00:00
}
2018-03-29 02:08:18 +00:00
2017-10-08 01:50:23 +00:00
}
2018-03-29 02:08:18 +00:00
return graphicsIndex ;
2017-06-03 17:57:56 +00:00
}
2018-03-15 03:47:56 +00:00
virtual void convertLinkVisualShapes2 ( int linkIndex , int urdfIndex , const char * pathPrefix , const btTransform & localInertiaFrame , class btCollisionObject * colObj , int bodyUniqueId ) const
2017-06-03 17:57:56 +00:00
{
2017-06-30 05:06:27 +00:00
//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
UrdfModel model ; // = m_data->m_urdfParser.getModel();
UrdfLink link ;
2018-03-29 02:08:18 +00:00
2018-05-09 17:28:12 +00:00
if ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ urdfIndex ] > = 0 )
2017-06-30 05:06:27 +00:00
{
2018-05-09 17:28:12 +00:00
const InternalVisualShapeHandle * visHandle = m_data - > m_userVisualShapeHandles . getHandle ( m_createBodyArgs . m_linkVisualShapeUniqueIds [ urdfIndex ] ) ;
2018-03-29 02:08:18 +00:00
if ( visHandle )
2017-06-19 17:14:26 +00:00
{
2018-03-29 02:08:18 +00:00
for ( int i = 0 ; i < visHandle - > m_visualShapes . size ( ) ; i + + )
2017-06-19 17:14:26 +00:00
{
2018-03-29 02:08:18 +00:00
link . m_visualArray . push_back ( visHandle - > m_visualShapes [ i ] ) ;
}
}
}
if ( link . m_visualArray . size ( ) = = 0 )
{
int colShapeUniqueId = m_createBodyArgs . m_linkCollisionShapeUniqueIds [ urdfIndex ] ;
if ( colShapeUniqueId > = 0 )
{
InternalCollisionShapeHandle * handle = m_data - > m_userCollisionShapeHandles . getHandle ( colShapeUniqueId ) ;
if ( handle )
{
for ( int i = 0 ; i < handle - > m_urdfCollisionObjects . size ( ) ; i + + )
{
link . m_collisionArray . push_back ( handle - > m_urdfCollisionObjects [ i ] ) ;
}
2017-06-19 17:14:26 +00:00
}
}
}
2017-06-30 05:06:27 +00:00
//UrdfVisual vis;
//link.m_visualArray.push_back(vis);
//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > convertVisualShapes ( linkIndex , pathPrefix , localInertiaFrame , & link , & model , colObj - > getBroadphaseHandle ( ) - > getUid ( ) , bodyUniqueId ) ;
2018-01-17 01:58:19 +00:00
}
2017-06-03 17:57:56 +00:00
}
2018-03-15 03:47:56 +00:00
virtual void setBodyUniqueId ( int bodyId )
2017-06-03 17:57:56 +00:00
{
m_bodyUniqueId = bodyId ;
}
2018-03-15 03:47:56 +00:00
virtual int getBodyUniqueId ( ) const
2017-06-03 17:57:56 +00:00
{
return m_bodyUniqueId ;
}
2018-03-15 03:47:56 +00:00
//default implementation for backward compatibility
2017-06-03 17:57:56 +00:00
virtual class btCompoundShape * convertLinkCollisionShapes ( int linkIndex , const char * pathPrefix , const btTransform & localInertiaFrame ) const
{
btCompoundShape * compound = new btCompoundShape ( ) ;
int colShapeUniqueId = m_createBodyArgs . m_linkCollisionShapeUniqueIds [ linkIndex ] ;
if ( colShapeUniqueId > = 0 )
{
InternalCollisionShapeHandle * handle = m_data - > m_userCollisionShapeHandles . getHandle ( colShapeUniqueId ) ;
2018-01-08 20:25:56 +00:00
if ( handle & & handle - > m_collisionShape )
2017-06-03 17:57:56 +00:00
{
2018-01-08 20:25:56 +00:00
if ( handle - > m_collisionShape - > getShapeType ( ) = = COMPOUND_SHAPE_PROXYTYPE )
{
btCompoundShape * childCompound = ( btCompoundShape * ) handle - > m_collisionShape ;
for ( int c = 0 ; c < childCompound - > getNumChildShapes ( ) ; c + + )
{
btTransform childTrans = childCompound - > getChildTransform ( c ) ;
btCollisionShape * childShape = childCompound - > getChildShape ( c ) ;
btTransform tr = localInertiaFrame . inverse ( ) * childTrans ;
compound - > addChildShape ( tr , childShape ) ;
}
}
else
{
btTransform childTrans ;
childTrans . setIdentity ( ) ;
compound - > addChildShape ( localInertiaFrame . inverse ( ) * childTrans , handle - > m_collisionShape ) ;
}
2017-06-03 17:57:56 +00:00
}
}
m_allocatedCollisionShapes . push_back ( compound ) ;
return compound ;
}
2018-03-15 03:47:56 +00:00
virtual int getNumAllocatedCollisionShapes ( ) const
{
2017-06-03 17:57:56 +00:00
return m_allocatedCollisionShapes . size ( ) ;
}
2018-03-15 03:47:56 +00:00
virtual class btCollisionShape * getAllocatedCollisionShape ( int index )
2017-06-03 17:57:56 +00:00
{
return m_allocatedCollisionShapes [ index ] ;
}
2018-03-15 03:47:56 +00:00
virtual int getNumModels ( ) const
2017-06-03 17:57:56 +00:00
{
return 1 ;
}
2018-03-15 03:47:56 +00:00
virtual void activateModel ( int /*modelIndex*/ )
2017-06-03 17:57:56 +00:00
{
}
} ;
2015-11-23 04:50:32 +00:00
void PhysicsServerCommandProcessor : : createEmptyDynamicsWorld ( )
{
2018-08-24 06:04:17 +00:00
m_data - > m_constraintSolverType = eConstraintSolverLCP_SI ;
2016-10-17 20:01:04 +00:00
///collision configuration contains default setup for memory, collision setup
//m_collisionConfiguration->setConvexConvexMultipointIterations();
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
m_data - > m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration ( ) ;
2016-10-19 00:38:43 +00:00
# else
m_data - > m_collisionConfiguration = new btDefaultCollisionConfiguration ( ) ;
# endif
2016-10-17 20:01:04 +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 ) ;
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
m_data - > m_broadphaseCollisionFilterCallback = new MyOverlapFilterCallback ( ) ;
m_data - > m_broadphaseCollisionFilterCallback - > m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA ;
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
m_data - > m_pairCache = new btHashedOverlappingPairCache ( ) ;
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
m_data - > m_pairCache - > setOverlapFilterCallback ( m_data - > m_broadphaseCollisionFilterCallback ) ;
2017-12-30 22:19:13 +00:00
2018-08-26 23:49:17 +00:00
int maxProxies = 32768 ;
2018-08-27 00:17:42 +00:00
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
btDbvtBroadphase * bv = new btDbvtBroadphase ( m_data - > m_pairCache ) ;
bv - > setVelocityPrediction ( 0 ) ;
m_data - > m_broadphase = bv ;
2018-03-15 03:47:56 +00:00
2017-12-30 22:19:13 +00:00
2016-10-17 20:01:04 +00:00
m_data - > m_solver = new btMultiBodyConstraintSolver ;
2018-03-15 03:47:56 +00:00
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-17 20:01:04 +00:00
m_data - > m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld ( m_data - > m_dispatcher , m_data - > m_broadphase , m_data - > m_solver , m_data - > m_collisionConfiguration ) ;
2016-10-19 00:38:43 +00:00
# else
m_data - > m_dynamicsWorld = new btMultiBodyDynamicsWorld ( m_data - > m_dispatcher , m_data - > m_broadphase , m_data - > m_solver , m_data - > m_collisionConfiguration ) ;
# endif
2018-03-15 03:47:56 +00:00
2017-12-30 22:19:13 +00:00
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
2016-11-17 00:12:59 +00:00
m_data - > m_dynamicsWorld - > getCollisionObjectArray ( ) . reserve ( 32768 ) ;
2018-03-15 03:47:56 +00:00
2016-10-17 20:01:04 +00:00
m_data - > m_remoteDebugDrawer = new SharedMemoryDebugDrawer ( ) ;
2018-03-15 03:47:56 +00:00
2016-10-17 20:01:04 +00:00
m_data - > m_dynamicsWorld - > setGravity ( btVector3 ( 0 , 0 , 0 ) ) ;
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp2 = 0.08 ;
2017-03-20 17:58:07 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_frictionERP = 0.2 ; //need to check if there are artifacts with frictionERP
2018-08-09 23:49:25 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_linearSlop = 0.00001 ;
2016-12-02 22:10:26 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_numIterations = 50 ;
2018-08-26 23:14:36 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_minimumSolverBatchSize = 0 ;
2018-08-27 00:17:42 +00:00
gDbvtMargin = btScalar ( 0 ) ;
2016-12-17 02:09:52 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_leastSquaresResidualThreshold = 1e-7 ;
2016-12-23 23:20:04 +00:00
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
2017-04-11 23:03:07 +00:00
if ( m_data - > m_guiHelper )
{
m_data - > m_guiHelper - > createPhysicsDebugDrawer ( m_data - > m_dynamicsWorld ) ;
}
2017-09-24 01:05:23 +00:00
bool isPreTick = false ;
m_data - > m_dynamicsWorld - > setInternalTickCallback ( logCallback , this , isPreTick ) ;
isPreTick = true ;
m_data - > m_dynamicsWorld - > setInternalTickCallback ( preTickCallback , this , isPreTick ) ;
2018-02-17 03:44:33 +00:00
gContactAddedCallback = MyContactAddedCallback ;
2017-04-29 17:32:30 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
m_data - > m_soundEngine . init ( 16 , true ) ;
//we don't use those callbacks (yet), experimental
2018-02-17 03:44:33 +00:00
2017-04-29 17:32:30 +00:00
// gContactDestroyedCallback = MyContactDestroyedCallback;
// gContactProcessedCallback = MyContactProcessedCallback;
// gContactStartedCallback = MyContactStartedCallback;
// gContactEndedCallback = MyContactEndedCallback;
# endif
2015-11-23 04:50:32 +00:00
}
2017-02-17 22:25:53 +00:00
void PhysicsServerCommandProcessor : : deleteStateLoggers ( )
{
for ( int i = 0 ; i < m_data - > m_stateLoggers . size ( ) ; i + + )
{
m_data - > m_stateLoggers [ i ] - > stop ( ) ;
delete m_data - > m_stateLoggers [ i ] ;
}
m_data - > m_stateLoggers . clear ( ) ;
}
2016-11-30 01:08:47 +00:00
void PhysicsServerCommandProcessor : : deleteCachedInverseKinematicsBodies ( )
{
for ( int i = 0 ; i < m_data - > m_inverseKinematicsHelpers . size ( ) ; i + + )
{
IKTrajectoryHelper * * ikHelperPtr = m_data - > m_inverseKinematicsHelpers . getAtIndex ( i ) ;
if ( ikHelperPtr )
{
IKTrajectoryHelper * ikHelper = * ikHelperPtr ;
delete ikHelper ;
}
}
m_data - > m_inverseKinematicsHelpers . clear ( ) ;
}
2016-08-10 01:40:12 +00:00
void PhysicsServerCommandProcessor : : deleteCachedInverseDynamicsBodies ( )
{
for ( int i = 0 ; i < m_data - > m_inverseDynamicsBodies . size ( ) ; i + + )
{
btInverseDynamics : : MultiBodyTree * * treePtrPtr = m_data - > m_inverseDynamicsBodies . getAtIndex ( i ) ;
if ( treePtrPtr )
{
btInverseDynamics : : MultiBodyTree * tree = * treePtrPtr ;
delete tree ;
}
}
m_data - > m_inverseDynamicsBodies . clear ( ) ;
}
2015-11-23 04:50:32 +00:00
void PhysicsServerCommandProcessor : : deleteDynamicsWorld ( )
{
2017-04-29 17:32:30 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
m_data - > m_soundEngine . exit ( ) ;
//gContactDestroyedCallback = 0;
//gContactProcessedCallback = 0;
//gContactStartedCallback = 0;
//gContactEndedCallback = 0;
# endif
2018-03-15 03:47:56 +00:00
2016-08-10 01:40:12 +00:00
deleteCachedInverseDynamicsBodies ( ) ;
2016-11-30 01:08:47 +00:00
deleteCachedInverseKinematicsBodies ( ) ;
2017-02-17 22:25:53 +00:00
deleteStateLoggers ( ) ;
2016-06-24 14:31:17 +00:00
2017-01-23 03:08:31 +00:00
m_data - > m_userConstraints . clear ( ) ;
m_data - > m_saveWorldBodyData . clear ( ) ;
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 + + )
{
2017-01-10 22:57:16 +00:00
m_data - > m_worldImporters [ i ] - > deleteAllData ( ) ;
2015-11-23 04:50:32 +00:00
delete m_data - > m_worldImporters [ i ] ;
}
m_data - > m_worldImporters . clear ( ) ;
2016-06-24 14:31:17 +00:00
2018-06-05 22:59:01 +00:00
# ifdef ENABLE_LINK_MAPPER
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 ( ) ;
2018-06-05 22:59:01 +00:00
# endif //ENABLE_LINK_MAPPER
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 ;
}
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for ( i = m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) . size ( ) - 1 ; i > = 0 ; i - - )
{
btSoftBody * sb = m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) [ i ] ;
m_data - > m_dynamicsWorld - > removeSoftBody ( sb ) ;
delete sb ;
}
# endif
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 ] ;
2018-02-17 03:44:33 +00:00
//check for internal edge utility, delete memory
if ( shape - > getShapeType ( ) = = TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
btBvhTriangleMeshShape * trimesh = ( btBvhTriangleMeshShape * ) shape ;
if ( trimesh - > getTriangleInfoMap ( ) )
{
delete trimesh - > getTriangleInfoMap ( ) ;
}
}
2015-11-23 04:50:32 +00:00
delete shape ;
}
2017-08-24 16:16:11 +00:00
for ( int j = 0 ; j < m_data - > m_meshInterfaces . size ( ) ; j + + )
{
delete m_data - > m_meshInterfaces [ j ] ;
}
2018-02-22 07:22:16 +00:00
if ( m_data - > m_guiHelper )
{
for ( int j = 0 ; j < m_data - > m_allocatedTextures . size ( ) ; j + + )
{
int texId = m_data - > m_allocatedTextures [ j ] ;
m_data - > m_guiHelper - > removeTexture ( texId ) ;
}
}
m_data - > m_allocatedTextures . clear ( ) ;
2017-08-24 16:16:11 +00:00
m_data - > m_meshInterfaces . clear ( ) ;
2015-11-23 04:50:32 +00:00
m_data - > m_collisionShapes . clear ( ) ;
2018-01-08 20:25:56 +00:00
m_data - > m_bulletCollisionShape2UrdfCollision . clear ( ) ;
2015-11-23 04:50:32 +00:00
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 ;
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
delete m_data - > m_broadphase ;
m_data - > m_broadphase = 0 ;
2017-01-16 16:23:49 +00:00
delete m_data - > m_pairCache ;
m_data - > m_pairCache = 0 ;
2018-03-15 03:47:56 +00:00
2017-01-16 16:23:49 +00:00
delete m_data - > m_broadphaseCollisionFilterCallback ;
m_data - > m_broadphaseCollisionFilterCallback = 0 ;
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
delete m_data - > m_dispatcher ;
m_data - > m_dispatcher = 0 ;
delete m_data - > m_collisionConfiguration ;
m_data - > m_collisionConfiguration = 0 ;
2017-05-04 20:52:02 +00:00
m_data - > m_userConstraintUIDGenerator = 1 ;
2015-11-23 04:50:32 +00:00
}
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 ) )
{
2016-09-23 06:21:24 +00:00
float maxMotorImpulse = 1.f ;
2015-11-23 04:50:32 +00:00
int dof = 0 ;
btScalar desiredVelocity = 0.f ;
btMultiBodyJointMotor * motor = new btMultiBodyJointMotor ( mb , mbLinkIndex , dof , desiredVelocity , maxMotorImpulse ) ;
2016-08-18 23:48:14 +00:00
motor - > setPositionTarget ( 0 , 0 ) ;
2016-08-18 20:10:28 +00:00
motor - > setVelocityTarget ( 0 , 1 ) ;
2016-09-28 23:07:55 +00:00
//motor->setRhsClamp(gRhsClamp);
2015-11-23 04:50:32 +00:00
//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-12-31 22:43:15 +00:00
bool PhysicsServerCommandProcessor : : processImportedObjects ( const char * fileName , char * bufferServerToClient , int bufferSizeInBytes , bool useMultiBody , int flags , URDFImporterInterface & u2b )
2016-06-13 17:11:28 +00:00
{
2016-12-31 22:43:15 +00:00
bool loadOk = true ;
2018-03-15 03:47:56 +00:00
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
btTransform rootTrans ;
rootTrans . setIdentity ( ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " loaded %s OK! " , fileName ) ;
}
SaveWorldObjectData sd ;
sd . m_fileName = fileName ;
2016-06-24 14:31:17 +00:00
2018-07-08 09:23:12 +00:00
int currentOpenGLTextureIndex = 0 ;
2016-12-31 22:43:15 +00:00
for ( int m = 0 ; m < u2b . getNumModels ( ) ; m + + )
2016-06-13 17:11:28 +00:00
{
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
u2b . activateModel ( m ) ;
btMultiBody * mb = 0 ;
btRigidBody * rb = 0 ;
2016-10-13 06:03:36 +00:00
2016-12-31 22:43:15 +00:00
//get a body index
2017-05-03 17:49:04 +00:00
int bodyUniqueId = m_data - > m_bodyHandles . allocHandle ( ) ;
2016-12-31 22:43:15 +00:00
2017-05-03 17:49:04 +00:00
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2018-03-15 03:47:56 +00:00
2016-12-31 22:43:15 +00:00
sd . m_bodyUniqueIds . push_back ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
u2b . setBodyUniqueId ( bodyUniqueId ) ;
2016-06-13 17:11:28 +00:00
{
2016-12-31 22:43:15 +00:00
btScalar mass = 0 ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
2017-03-30 18:01:33 +00:00
bodyHandle - > m_bodyName = u2b . getBodyName ( ) ;
2016-12-31 22:43:15 +00:00
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
int urdfLinkIndex = u2b . getRootLinkIndex ( ) ;
2018-05-03 21:24:16 +00:00
u2b . getMassAndInertia2 ( urdfLinkIndex , mass , localInertiaDiagonal , bodyHandle - > m_rootLocalInertialFrame , flags ) ;
2016-12-31 22:43:15 +00:00
}
2016-06-13 17:11:28 +00:00
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
2017-03-22 18:05:00 +00:00
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
2016-12-31 22:43:15 +00:00
MyMultiBodyCreator creation ( m_data - > m_guiHelper ) ;
2016-09-01 20:30:07 +00:00
2016-12-31 22:43:15 +00:00
u2b . getRootTransformInWorld ( rootTrans ) ;
2017-09-07 21:27:00 +00:00
//CUF_RESERVED is a temporary flag, for backward compatibility purposes
flags | = CUF_RESERVED ;
2018-05-23 03:26:00 +00:00
if ( flags & CUF_ENABLE_CACHED_GRAPHICS_SHAPES )
{
{
UrdfVisualShapeCache * tmpPtr = m_data - > m_cachedVUrdfisualShapes [ fileName ] ;
if ( tmpPtr = = 0 )
{
m_data - > m_cachedVUrdfisualShapes . insert ( fileName , UrdfVisualShapeCache ( ) ) ;
}
}
UrdfVisualShapeCache * cachedVisualShapesPtr = m_data - > m_cachedVUrdfisualShapes [ fileName ] ;
2018-05-23 04:22:22 +00:00
ConvertURDF2Bullet ( u2b , creation , rootTrans , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) , flags , cachedVisualShapesPtr ) ;
2018-05-23 03:26:00 +00:00
} else
{
ConvertURDF2Bullet ( u2b , creation , rootTrans , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) , flags ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
mb = creation . getBulletMultiBody ( ) ;
rb = creation . getRigidBody ( ) ;
if ( rb )
rb - > setUserIndex2 ( bodyUniqueId ) ;
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
if ( mb )
mb - > setUserIndex2 ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2018-03-15 03:47:56 +00:00
2016-12-31 22:43:15 +00:00
if ( mb )
{
bodyHandle - > m_multiBody = mb ;
2018-03-15 03:47:56 +00:00
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
m_data - > m_sdfRecentLoadedBodies . push_back ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
createJointMotors ( mb ) ;
2016-09-01 20:30:07 +00:00
2017-06-03 17:57:56 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource ;
int urdfRootLink = u2b . getRootLinkIndex ( ) ; //LinkIndex = creation.m_mb2urdfLink[-1];
if ( u2b . getLinkAudioSource ( urdfRootLink , audioSource ) )
{
int flags = mb - > getBaseCollider ( ) - > getCollisionFlags ( ) ;
mb - > getBaseCollider ( ) - > setCollisionFlags ( flags | btCollisionObject : : CF_HAS_COLLISION_SOUND_TRIGGER ) ;
audioSource . m_userIndex = m_data - > m_soundEngine . loadWavFile ( audioSource . m_uri . c_str ( ) ) ;
if ( audioSource . m_userIndex > = 0 )
{
bodyHandle - > m_audioSources . insert ( - 1 , audioSource ) ;
}
}
}
# endif
2016-12-31 22:43:15 +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 + + )
2016-06-13 17:11:28 +00:00
{
2016-12-31 22:43:15 +00:00
//disable serialization of the collision objects
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
btScalar mass ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
btTransform localInertialFrame ;
2018-05-03 21:24:16 +00:00
u2b . getMassAndInertia2 ( urdfLinkIndex , mass , localInertiaDiagonal , localInertialFrame , flags ) ;
2016-12-31 22:43:15 +00:00
bodyHandle - > m_linkLocalInertialFrames . push_back ( localInertialFrame ) ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
mb - > getLink ( i ) . m_linkName = linkName - > c_str ( ) ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
mb - > getLink ( i ) . m_jointName = jointName - > c_str ( ) ;
2017-06-03 17:57:56 +00:00
# ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource ;
int urdfLinkIndex = creation . m_mb2urdfLink [ link ] ;
if ( u2b . getLinkAudioSource ( urdfLinkIndex , audioSource ) )
{
int flags = mb - > getLink ( link ) . m_collider - > getCollisionFlags ( ) ;
mb - > getLink ( i ) . m_collider - > setCollisionFlags ( flags | btCollisionObject : : CF_HAS_COLLISION_SOUND_TRIGGER ) ;
audioSource . m_userIndex = m_data - > m_soundEngine . loadWavFile ( audioSource . m_uri . c_str ( ) ) ;
if ( audioSource . m_userIndex > = 0 )
{
bodyHandle - > m_audioSources . insert ( link , audioSource ) ;
}
}
}
# endif
2016-12-31 22:43:15 +00:00
}
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
mb - > setBaseName ( baseName - > c_str ( ) ) ;
} else
{
2017-06-03 17:57:56 +00:00
//b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
2016-12-31 22:43:15 +00:00
bodyHandle - > m_rigidBody = rb ;
2017-06-03 17:57:56 +00:00
rb - > setUserIndex2 ( bodyUniqueId ) ;
m_data - > m_sdfRecentLoadedBodies . push_back ( bodyUniqueId ) ;
2017-08-30 02:14:27 +00:00
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
bodyHandle - > m_bodyName = * baseName ;
int numJoints = creation . getNum6DofConstraints ( ) ;
bodyHandle - > m_rigidBodyJoints . reserve ( numJoints ) ;
bodyHandle - > m_rigidBodyJointNames . reserve ( numJoints ) ;
bodyHandle - > m_rigidBodyLinkNames . reserve ( numJoints ) ;
for ( int i = 0 ; i < numJoints ; i + + )
{
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
btGeneric6DofSpring2Constraint * con = creation . get6DofConstraint ( i ) ;
2018-03-15 03:47:56 +00:00
2017-08-30 02:14:27 +00:00
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
bodyHandle - > m_rigidBodyJointNames . push_back ( * jointName ) ;
bodyHandle - > m_rigidBodyLinkNames . push_back ( * linkName ) ;
bodyHandle - > m_rigidBodyJoints . push_back ( con ) ;
}
2016-12-31 22:43:15 +00:00
}
2016-06-24 14:31:17 +00:00
2018-07-08 09:23:12 +00:00
{
int startShapeIndex = 0 ;
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
{
int totalNumVisualShapes = m_data - > m_pluginManager . getRenderInterface ( ) - > getNumVisualShapes ( bodyUniqueId ) ;
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
b3VisualShapeData tmpShape ;
int remain = totalNumVisualShapes - startShapeIndex ;
int shapeIndex = startShapeIndex ;
int success = m_data - > m_pluginManager . getRenderInterface ( ) - > getVisualShapesData ( bodyUniqueId , shapeIndex , & tmpShape ) ;
if ( success )
{
if ( tmpShape . m_tinyRendererTextureId > = 0 )
{
int openglTextureUniqueId = - 1 ;
//find companion opengl texture unique id and create a 'textureUid'
if ( currentOpenGLTextureIndex < u2b . getNumAllocatedTextures ( ) )
{
openglTextureUniqueId = u2b . getAllocatedTexture ( currentOpenGLTextureIndex ) ;
currentOpenGLTextureIndex + + ;
}
int texHandle = m_data - > m_textureHandles . allocHandle ( ) ;
InternalTextureHandle * texH = m_data - > m_textureHandles . getHandle ( texHandle ) ;
if ( texH )
{
texH - > m_tinyRendererTextureId = tmpShape . m_tinyRendererTextureId ;
texH - > m_openglTextureId = openglTextureUniqueId ;
}
}
}
}
}
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = BODY_ADDED ;
notification . m_bodyArgs . m_bodyUniqueId = bodyUniqueId ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2016-12-31 22:43:15 +00:00
}
2017-08-24 16:16:11 +00:00
2018-03-15 03:47:56 +00:00
2018-02-22 07:22:16 +00:00
for ( int i = 0 ; i < u2b . getNumAllocatedTextures ( ) ; i + + )
{
int texId = u2b . getAllocatedTexture ( i ) ;
m_data - > m_allocatedTextures . push_back ( texId ) ;
}
2018-07-08 09:23:12 +00:00
/*
int texHandle = m_data - > m_textureHandles . allocHandle ( ) ;
InternalTextureHandle * texH = m_data - > m_textureHandles . getHandle ( texHandle ) ;
if ( texH )
{
texH - > m_tinyRendererTextureId = - 1 ;
texH - > m_openglTextureId = - 1 ;
*/
2018-02-22 07:22:16 +00:00
2017-08-24 16:16:11 +00:00
for ( int i = 0 ; i < u2b . getNumAllocatedMeshInterfaces ( ) ; i + + )
{
m_data - > m_meshInterfaces . push_back ( u2b . getAllocatedMeshInterface ( i ) ) ;
}
2016-12-31 22:43:15 +00:00
for ( int i = 0 ; i < u2b . getNumAllocatedCollisionShapes ( ) ; i + + )
{
btCollisionShape * shape = u2b . getAllocatedCollisionShape ( i ) ;
m_data - > m_collisionShapes . push_back ( shape ) ;
2018-01-08 20:25:56 +00:00
UrdfCollision urdfCollision ;
if ( u2b . getUrdfFromCollisionShape ( shape , urdfCollision ) )
{
m_data - > m_bulletCollisionShape2UrdfCollision . insert ( shape , urdfCollision ) ;
}
if ( shape - > getShapeType ( ) = = COMPOUND_SHAPE_PROXYTYPE )
{
btCompoundShape * compound = ( btCompoundShape * ) shape ;
for ( int c = 0 ; c < compound - > getNumChildShapes ( ) ; c + + )
{
btCollisionShape * childShape = compound - > getChildShape ( c ) ;
if ( u2b . getUrdfFromCollisionShape ( childShape , urdfCollision ) )
{
m_data - > m_bulletCollisionShape2UrdfCollision . insert ( childShape , urdfCollision ) ;
}
}
}
2016-12-31 22:43:15 +00:00
}
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
m_data - > m_saveWorldBodyData . push_back ( sd ) ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
return loadOk ;
}
2016-06-13 17:11:28 +00:00
2016-12-31 22:43:15 +00:00
struct MyMJCFLogger2 : public MJCFErrorLogger
{
virtual void reportError ( const char * error )
{
b3Error ( error ) ;
}
virtual void reportWarning ( const char * warning )
{
b3Warning ( warning ) ;
}
virtual void printMessage ( const char * msg )
{
b3Printf ( msg ) ;
}
} ;
2016-06-24 14:31:17 +00:00
2016-12-31 22:43:15 +00:00
bool PhysicsServerCommandProcessor : : loadMjcf ( const char * fileName , char * bufferServerToClient , int bufferSizeInBytes , bool useMultiBody , int flags )
{
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-12-31 22:43:15 +00:00
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
2018-01-17 20:48:48 +00:00
BulletMJCFImporter u2b ( m_data - > m_guiHelper , m_data - > m_pluginManager . getRenderInterface ( ) , flags ) ;
2016-12-31 22:43:15 +00:00
bool useFixedBase = false ;
MyMJCFLogger2 logger ;
bool loadOk = u2b . loadMJCF ( fileName , & logger , useFixedBase ) ;
if ( loadOk )
{
processImportedObjects ( fileName , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags , u2b ) ;
}
return loadOk ;
}
2016-10-13 06:03:36 +00:00
2017-08-14 21:59:41 +00:00
bool PhysicsServerCommandProcessor : : loadSdf ( const char * fileName , char * bufferServerToClient , int bufferSizeInBytes , bool useMultiBody , int flags , btScalar globalScaling )
2016-12-31 22:43:15 +00:00
{
btAssert ( m_data - > m_dynamicsWorld ) ;
if ( ! m_data - > m_dynamicsWorld )
{
b3Error ( " loadSdf: No valid m_dynamicsWorld " ) ;
return false ;
}
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
2018-01-17 20:48:48 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , m_data - > m_pluginManager . getRenderInterface ( ) , globalScaling , flags ) ;
2017-10-06 20:46:24 +00:00
u2b . setEnableTinyRenderer ( m_data - > m_enableTinyRenderer ) ;
2016-12-31 22:43:15 +00:00
bool forceFixedBase = false ;
bool loadOk = u2b . loadSDF ( fileName , forceFixedBase ) ;
2018-03-15 03:47:56 +00:00
2016-12-31 22:43:15 +00:00
if ( loadOk )
{
processImportedObjects ( fileName , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags , u2b ) ;
}
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
2016-09-13 22:37:46 +00:00
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : loadUrdf ( const char * fileName , const btVector3 & pos , const btQuaternion & orn ,
2017-08-14 21:59:41 +00:00
bool useMultiBody , bool useFixedBase , int * bodyUniqueIdPtr , char * bufferServerToClient , int bufferSizeInBytes , int flags , btScalar globalScaling )
2015-11-23 04:50:32 +00:00
{
2017-06-03 17:57:56 +00:00
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
* bodyUniqueIdPtr = - 1 ;
2016-12-29 05:51:54 +00:00
BT_PROFILE ( " loadURDF " ) ;
2015-11-23 04:50:32 +00:00
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
2018-01-17 20:48:48 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , m_data - > m_pluginManager . getRenderInterface ( ) , globalScaling , flags ) ;
2017-10-06 20:46:24 +00:00
u2b . setEnableTinyRenderer ( m_data - > m_enableTinyRenderer ) ;
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 )
{
2017-06-03 17:57:56 +00:00
btTransform rootTrans ;
rootTrans . setOrigin ( pos ) ;
rootTrans . setRotation ( orn ) ;
u2b . setRootTransformInWorld ( rootTrans ) ;
bool ok = processImportedObjects ( fileName , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags , u2b ) ;
if ( ok )
{
if ( m_data - > m_sdfRecentLoadedBodies . size ( ) = = 1 )
{
* bodyUniqueIdPtr = m_data - > m_sdfRecentLoadedBodies [ 0 ] ;
}
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
}
return ok ;
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
2017-05-03 17:49:04 +00:00
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2018-03-15 03:47:56 +00:00
btMultiBody * mb = bodyHandle ? bodyHandle - > m_multiBody : 0 ;
2016-06-15 01:41:19 +00:00
if ( mb )
{
2018-06-05 22:59:01 +00:00
UrdfLinkNameMapUtil utilBla ;
UrdfLinkNameMapUtil * util = & utilBla ;
btDefaultSerializer ser ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
2016-06-15 01:41:19 +00:00
util - > m_mb = mb ;
2018-06-05 22:59:01 +00:00
util - > m_memSerializer = & ser ;
2016-11-05 19:53:40 +00:00
util - > m_memSerializer - > startSerialization ( ) ;
2016-06-15 01:41:19 +00:00
//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-09-27 19:13:45 +00:00
if ( mb - > getBaseName ( ) )
{
util - > m_memSerializer - > registerNameForPointer ( mb - > getBaseName ( ) , mb - > getBaseName ( ) ) ;
}
2016-06-15 01:41:19 +00:00
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
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
2017-08-30 02:14:27 +00:00
} else
{
2018-03-15 03:47:56 +00:00
btRigidBody * rb = bodyHandle ? bodyHandle - > m_rigidBody : 0 ;
2017-08-30 02:14:27 +00:00
if ( rb )
{
2018-06-05 22:59:01 +00:00
UrdfLinkNameMapUtil utilBla ;
UrdfLinkNameMapUtil * util = & utilBla ;
btDefaultSerializer ser ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
util - > m_memSerializer = & ser ;
2017-08-30 02:14:27 +00:00
util - > m_memSerializer - > startSerialization ( ) ;
util - > m_memSerializer - > registerNameForPointer ( bodyHandle - > m_rigidBody , bodyHandle - > m_bodyName . c_str ( ) ) ;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
for ( int i = 0 ; i < bodyHandle - > m_rigidBodyJoints . size ( ) ; i + + )
{
const btGeneric6DofSpring2Constraint * con = bodyHandle - > m_rigidBodyJoints . at ( i ) ;
2017-11-18 21:21:20 +00:00
2017-08-30 02:14:27 +00:00
util - > m_memSerializer - > registerNameForPointer ( con , bodyHandle - > m_rigidBodyJointNames [ i ] . c_str ( ) ) ;
util - > m_memSerializer - > registerNameForPointer ( & con - > getRigidBodyB ( ) , bodyHandle - > m_rigidBodyLinkNames [ i ] . c_str ( ) ) ;
const btRigidBody & bodyA = con - > getRigidBodyA ( ) ;
int len = con - > calculateSerializeBufferSize ( ) ;
btChunk * chunk = util - > m_memSerializer - > allocate ( len , 1 ) ;
const char * structType = con - > serialize ( chunk - > m_oldPtr , util - > m_memSerializer ) ;
util - > m_memSerializer - > finalizeChunk ( chunk , structType , BT_CONSTRAINT_CODE , ( void * ) con ) ;
}
streamSizeInBytes = util - > m_memSerializer - > getCurrentBufferSize ( ) ;
2017-11-18 21:21:20 +00:00
2017-08-30 02:14:27 +00:00
}
}
2016-06-15 01:41:19 +00:00
return streamSizeInBytes ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processStateLoggingCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
2015-11-23 04:50:32 +00:00
{
2018-03-15 03:47:56 +00:00
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_STATE_LOGGING " ) ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_STATE_LOGGING_FAILED ;
bool hasStatus = true ;
2017-02-17 22:25:53 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & STATE_LOGGING_START_LOG )
{
2018-06-12 23:56:45 +00:00
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_ALL_COMMANDS )
{
if ( m_data - > m_commandLogger = = 0 )
{
enableCommandLogging ( true , clientCmd . m_stateLoggingArguments . m_fileName ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
m_data - > m_commandLoggingUid = loggerUid ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
}
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_REPLAY_ALL_COMMANDS )
{
if ( m_data - > m_logPlayback = = 0 )
{
replayFromLogFile ( clientCmd . m_stateLoggingArguments . m_fileName ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
m_data - > m_logPlaybackUid = loggerUid ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_PROFILE_TIMINGS )
{
if ( m_data - > m_profileTimingLoggingUid < 0 )
{
b3ChromeUtilsStartTimings ( ) ;
m_data - > m_profileTimingFileName = clientCmd . m_stateLoggingArguments . m_fileName ;
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
m_data - > m_profileTimingLoggingUid = loggerUid ;
}
}
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_VIDEO_MP4 )
{
//if (clientCmd.m_stateLoggingArguments.m_fileName)
{
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
VideoMP4Loggger * logger = new VideoMP4Loggger ( loggerUid , clientCmd . m_stateLoggingArguments . m_fileName , this - > m_data - > m_guiHelper ) ;
m_data - > m_stateLoggers . push_back ( logger ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_MINITAUR )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
std : : string fileName = clientCmd . m_stateLoggingArguments . m_fileName ;
//either provide the minitaur by object unique Id, or search for first multibody with 8 motors...
2017-02-17 22:25:53 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID ) & & ( clientCmd . m_stateLoggingArguments . m_numBodyUniqueIds > 0 ) )
{
int bodyUniqueId = clientCmd . m_stateLoggingArguments . m_bodyUniqueIds [ 0 ] ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( body )
{
if ( body - > m_multiBody )
{
btAlignedObjectArray < std : : string > motorNames ;
motorNames . push_back ( " motor_front_leftR_joint " ) ;
motorNames . push_back ( " motor_front_leftL_joint " ) ;
motorNames . push_back ( " motor_back_leftR_joint " ) ;
motorNames . push_back ( " motor_back_leftL_joint " ) ;
motorNames . push_back ( " motor_front_rightL_joint " ) ;
motorNames . push_back ( " motor_front_rightR_joint " ) ;
motorNames . push_back ( " motor_back_rightL_joint " ) ;
motorNames . push_back ( " motor_back_rightR_joint " ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btAlignedObjectArray < int > motorIdList ;
for ( int m = 0 ; m < motorNames . size ( ) ; m + + )
{
for ( int i = 0 ; i < body - > m_multiBody - > getNumLinks ( ) ; i + + )
2017-02-17 22:25:53 +00:00
{
2017-11-18 00:14:18 +00:00
std : : string jointName ;
if ( body - > m_multiBody - > getLink ( i ) . m_jointName )
2017-02-17 22:25:53 +00:00
{
2017-11-18 00:14:18 +00:00
jointName = body - > m_multiBody - > getLink ( i ) . m_jointName ;
}
if ( motorNames [ m ] = = jointName )
{
motorIdList . push_back ( i ) ;
2017-02-17 22:25:53 +00:00
}
}
}
2017-11-18 00:14:18 +00:00
if ( motorIdList . size ( ) = = 8 )
2017-04-02 22:45:48 +00:00
{
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
2017-11-18 00:14:18 +00:00
MinitaurStateLogger * logger = new MinitaurStateLogger ( loggerUid , fileName , body - > m_multiBody , motorIdList ) ;
2017-04-02 22:45:48 +00:00
m_data - > m_stateLoggers . push_back ( logger ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
2017-02-17 22:25:53 +00:00
}
}
2017-11-18 00:14:18 +00:00
}
}
2018-06-12 23:56:45 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_GENERIC_ROBOT )
{
std : : string fileName = clientCmd . m_stateLoggingArguments . m_fileName ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
int maxLogDof = 12 ;
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_MAX_LOG_DOF ) )
{
maxLogDof = clientCmd . m_stateLoggingArguments . m_maxLogDof ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int logFlags = 0 ;
if ( clientCmd . m_updateFlags & STATE_LOGGING_LOG_FLAGS )
{
logFlags = clientCmd . m_stateLoggingArguments . m_logFlags ;
}
GenericRobotStateLogger * logger = new GenericRobotStateLogger ( loggerUid , fileName , m_data - > m_dynamicsWorld , maxLogDof , logFlags ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID ) & & ( clientCmd . m_stateLoggingArguments . m_numBodyUniqueIds > 0 ) )
{
logger - > m_filterObjectUniqueId = true ;
for ( int i = 0 ; i < clientCmd . m_stateLoggingArguments . m_numBodyUniqueIds ; + + i )
{
int objectUniqueId = clientCmd . m_stateLoggingArguments . m_bodyUniqueIds [ i ] ;
logger - > m_bodyIdList . push_back ( objectUniqueId ) ;
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_stateLoggers . push_back ( logger ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_CONTACT_POINTS )
{
std : : string fileName = clientCmd . m_stateLoggingArguments . m_fileName ;
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
ContactPointsStateLogger * logger = new ContactPointsStateLogger ( loggerUid , fileName , m_data - > m_dynamicsWorld ) ;
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A ) & & clientCmd . m_stateLoggingArguments . m_linkIndexA > = - 1 )
{
logger - > m_filterLinkA = true ;
logger - > m_linkIndexA = clientCmd . m_stateLoggingArguments . m_linkIndexA ;
}
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B ) & & clientCmd . m_stateLoggingArguments . m_linkIndexB > = - 1 )
{
logger - > m_filterLinkB = true ;
logger - > m_linkIndexB = clientCmd . m_stateLoggingArguments . m_linkIndexB ;
}
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A ) & & clientCmd . m_stateLoggingArguments . m_bodyUniqueIdA > - 1 )
{
logger - > m_bodyUniqueIdA = clientCmd . m_stateLoggingArguments . m_bodyUniqueIdA ;
}
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B ) & & clientCmd . m_stateLoggingArguments . m_bodyUniqueIdB > - 1 )
{
logger - > m_bodyUniqueIdB = clientCmd . m_stateLoggingArguments . m_bodyUniqueIdB ;
}
m_data - > m_stateLoggers . push_back ( logger ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
}
if ( clientCmd . m_stateLoggingArguments . m_logType = = STATE_LOGGING_VR_CONTROLLERS )
{
std : : string fileName = clientCmd . m_stateLoggingArguments . m_fileName ;
int loggerUid = m_data - > m_stateLoggersUniqueId + + ;
int deviceFilterType = VR_DEVICE_CONTROLLER ;
if ( clientCmd . m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE )
{
deviceFilterType = clientCmd . m_stateLoggingArguments . m_deviceFilterType ;
}
VRControllerStateLogger * logger = new VRControllerStateLogger ( loggerUid , deviceFilterType , fileName ) ;
m_data - > m_stateLoggers . push_back ( logger ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_START_COMPLETED ;
serverStatusOut . m_stateLoggingResultArgs . m_loggingUniqueId = loggerUid ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
if ( ( clientCmd . m_updateFlags & STATE_LOGGING_STOP_LOG ) & & clientCmd . m_stateLoggingArguments . m_loggingUniqueId > = 0 )
{
2018-06-12 23:56:45 +00:00
if ( clientCmd . m_stateLoggingArguments . m_loggingUniqueId = = m_data - > m_logPlaybackUid )
{
if ( m_data - > m_logPlayback )
{
delete m_data - > m_logPlayback ;
m_data - > m_logPlayback = 0 ;
m_data - > m_logPlaybackUid = - 1 ;
}
}
if ( clientCmd . m_stateLoggingArguments . m_loggingUniqueId = = m_data - > m_commandLoggingUid )
{
if ( m_data - > m_commandLogger )
{
enableCommandLogging ( false , 0 ) ;
serverStatusOut . m_type = CMD_STATE_LOGGING_COMPLETED ;
m_data - > m_commandLoggingUid = - 1 ;
}
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_stateLoggingArguments . m_loggingUniqueId = = m_data - > m_profileTimingLoggingUid )
{
serverStatusOut . m_type = CMD_STATE_LOGGING_COMPLETED ;
b3ChromeUtilsStopTimingsAndWriteJsonFile ( m_data - > m_profileTimingFileName . c_str ( ) ) ;
m_data - > m_profileTimingLoggingUid = - 1 ;
}
else
{
serverStatusOut . m_type = CMD_STATE_LOGGING_COMPLETED ;
for ( int i = 0 ; i < m_data - > m_stateLoggers . size ( ) ; i + + )
{
if ( m_data - > m_stateLoggers [ i ] - > m_loggingUniqueId = = clientCmd . m_stateLoggingArguments . m_loggingUniqueId )
2017-01-06 01:41:58 +00:00
{
2017-11-18 00:14:18 +00:00
m_data - > m_stateLoggers [ i ] - > stop ( ) ;
delete m_data - > m_stateLoggers [ i ] ;
m_data - > m_stateLoggers . removeAtIndex ( i ) ;
}
}
}
}
return hasStatus ;
}
2017-01-06 01:41:58 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestCameraImageCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_CAMERA_IMAGE_DATA " ) ;
int startPixelIndex = clientCmd . m_requestPixelDataArguments . m_startPixelIndex ;
int width = clientCmd . m_requestPixelDataArguments . m_pixelWidth ;
int height = clientCmd . m_requestPixelDataArguments . m_pixelHeight ;
int numPixelsCopied = 0 ;
2017-01-06 01:41:58 +00:00
2017-05-18 02:29:12 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 ) & &
( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setWidthAndHeight ( clientCmd . m_requestPixelDataArguments . m_pixelWidth ,
2017-11-18 00:14:18 +00:00
clientCmd . m_requestPixelDataArguments . m_pixelHeight ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
}
2017-12-28 20:37:07 +00:00
int flags = 0 ;
if ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS )
{
flags = clientCmd . m_requestPixelDataArguments . m_flags ;
}
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setFlags ( flags ) ;
2018-01-17 01:58:19 +00:00
}
2017-03-29 23:29:34 +00:00
2017-11-18 00:14:18 +00:00
int numTotalPixels = width * height ;
int numRemainingPixels = numTotalPixels - startPixelIndex ;
2017-03-02 20:33:22 +00:00
2017-11-18 00:14:18 +00:00
if ( numRemainingPixels > 0 )
{
int totalBytesPerPixel = 4 + 4 + 4 ; //4 for rgb, 4 for depth, 4 for segmentation mask
int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1 ;
unsigned char * pixelRGBA = ( unsigned char * ) bufferServerToClient ;
int numRequestedPixels = btMin ( maxNumPixels , numRemainingPixels ) ;
float * depthBuffer = ( float * ) ( bufferServerToClient + numRequestedPixels * 4 ) ;
int * segmentationMaskBuffer = ( int * ) ( bufferServerToClient + numRequestedPixels * 8 ) ;
serverStatusOut . m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel ;
float viewMat [ 16 ] ;
float projMat [ 16 ] ;
2018-03-19 01:45:54 +00:00
float projTextureViewMat [ 16 ] ;
float projTextureProjMat [ 16 ] ;
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; i < 16 ; i + + )
{
viewMat [ i ] = clientCmd . m_requestPixelDataArguments . m_viewMatrix [ i ] ;
projMat [ i ] = clientCmd . m_requestPixelDataArguments . m_projectionMatrix [ i ] ;
}
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ) = = 0 )
{
b3OpenGLVisualizerCameraInfo tmpCamResult ;
bool result = this - > m_data - > m_guiHelper - > getCameraInfo (
& tmpCamResult . m_width ,
& tmpCamResult . m_height ,
tmpCamResult . m_viewMatrix ,
tmpCamResult . m_projectionMatrix ,
tmpCamResult . m_camUp ,
tmpCamResult . m_camForward ,
tmpCamResult . m_horizontal ,
tmpCamResult . m_vertical ,
& tmpCamResult . m_yaw ,
& tmpCamResult . m_pitch ,
& tmpCamResult . m_dist ,
tmpCamResult . m_target ) ;
if ( result )
{
for ( int i = 0 ; i < 16 ; i + + )
2017-06-17 20:29:14 +00:00
{
2017-11-18 00:14:18 +00:00
viewMat [ i ] = tmpCamResult . m_viewMatrix [ i ] ;
projMat [ i ] = tmpCamResult . m_projectionMatrix [ i ] ;
}
}
2018-03-19 01:45:54 +00:00
}
2017-11-18 00:14:18 +00:00
bool handled = false ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_updateFlags & ER_BULLET_HARDWARE_OPENGL ) ! = 0 )
{
2018-03-19 00:01:23 +00:00
if ( ( flags & ER_USE_PROJECTIVE_TEXTURE ) ! = 0 )
{
2018-03-21 04:28:47 +00:00
this - > m_data - > m_guiHelper - > setProjectiveTexture ( true ) ;
2018-03-19 01:45:54 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES ) ! = 0 )
{
for ( int i = 0 ; i < 16 ; i + + )
{
projTextureViewMat [ i ] = clientCmd . m_requestPixelDataArguments . m_projectiveTextureViewMatrix [ i ] ;
projTextureProjMat [ i ] = clientCmd . m_requestPixelDataArguments . m_projectiveTextureProjectionMatrix [ i ] ;
}
}
else // If no specified matrices for projective texture, then use the camera matrices.
{
for ( int i = 0 ; i < 16 ; i + + )
{
projTextureViewMat [ i ] = viewMat [ i ] ;
projTextureProjMat [ i ] = projMat [ i ] ;
}
}
this - > m_data - > m_guiHelper - > setProjectiveTextureMatrices ( projTextureViewMat , projTextureProjMat ) ;
2018-03-19 00:01:23 +00:00
}
else
{
2018-03-21 04:28:47 +00:00
this - > m_data - > m_guiHelper - > setProjectiveTexture ( false ) ;
2018-03-19 00:01:23 +00:00
}
2017-06-17 20:29:14 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > copyCameraImageData ( viewMat ,
projMat , pixelRGBA , numRequestedPixels ,
depthBuffer , numRequestedPixels ,
segmentationMaskBuffer , numRequestedPixels ,
startPixelIndex , width , height , & numPixelsCopied ) ;
2017-06-17 20:29:14 +00:00
2017-11-18 00:14:18 +00:00
if ( numPixelsCopied > 0 )
{
handled = true ;
m_data - > m_guiHelper - > debugDisplayCameraImageData ( viewMat ,
projMat , pixelRGBA , numRequestedPixels ,
depthBuffer , numRequestedPixels ,
0 , numRequestedPixels ,
startPixelIndex , width , height , & numPixelsCopied ) ;
}
2017-06-17 20:29:14 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
if ( ! handled )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2017-11-18 00:14:18 +00:00
{
2018-01-17 01:58:19 +00:00
if ( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 )
2017-11-18 00:14:18 +00:00
{
2018-01-17 01:58:19 +00:00
// printf("-------------------------------\nRendering\n");
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightDirection ( clientCmd . m_requestPixelDataArguments . m_lightDirection [ 0 ] , clientCmd . m_requestPixelDataArguments . m_lightDirection [ 1 ] , clientCmd . m_requestPixelDataArguments . m_lightDirection [ 2 ] ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightColor ( clientCmd . m_requestPixelDataArguments . m_lightColor [ 0 ] , clientCmd . m_requestPixelDataArguments . m_lightColor [ 1 ] , clientCmd . m_requestPixelDataArguments . m_lightColor [ 2 ] ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightDistance ( clientCmd . m_requestPixelDataArguments . m_lightDistance ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setShadow ( ( clientCmd . m_requestPixelDataArguments . m_hasShadow ! = 0 ) ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightAmbientCoeff ( clientCmd . m_requestPixelDataArguments . m_lightAmbientCoeff ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightDiffuseCoeff ( clientCmd . m_requestPixelDataArguments . m_lightDiffuseCoeff ) ;
2018-01-17 01:58:19 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-17 01:58:19 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > setLightSpecularCoeff ( clientCmd . m_requestPixelDataArguments . m_lightSpecularCoeff ) ;
2018-01-17 01:58:19 +00:00
}
2017-03-26 20:06:46 +00:00
2018-01-17 01:58:19 +00:00
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getNumCollisionObjects ( ) ; i + + )
2017-03-02 20:33:22 +00:00
{
2018-01-17 01:58:19 +00:00
const btCollisionObject * colObj = m_data - > m_dynamicsWorld - > getCollisionObjectArray ( ) [ i ] ;
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > syncTransform ( colObj - > getBroadphaseHandle ( ) - > getUid ( ) , colObj - > getWorldTransform ( ) , colObj - > getCollisionShape ( ) - > getLocalScaling ( ) ) ;
2018-01-17 01:58:19 +00:00
}
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ) ! = 0 )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > render (
2018-01-17 01:58:19 +00:00
clientCmd . m_requestPixelDataArguments . m_viewMatrix ,
clientCmd . m_requestPixelDataArguments . m_projectionMatrix ) ;
2017-11-18 00:14:18 +00:00
} else
2017-03-02 20:33:22 +00:00
{
2018-01-17 01:58:19 +00:00
b3OpenGLVisualizerCameraInfo tmpCamResult ;
bool result = this - > m_data - > m_guiHelper - > getCameraInfo (
& tmpCamResult . m_width ,
& tmpCamResult . m_height ,
tmpCamResult . m_viewMatrix ,
tmpCamResult . m_projectionMatrix ,
tmpCamResult . m_camUp ,
tmpCamResult . m_camForward ,
tmpCamResult . m_horizontal ,
tmpCamResult . m_vertical ,
& tmpCamResult . m_yaw ,
& tmpCamResult . m_pitch ,
& tmpCamResult . m_dist ,
tmpCamResult . m_target ) ;
if ( result )
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > render ( tmpCamResult . m_viewMatrix , tmpCamResult . m_projectionMatrix ) ;
2018-01-17 01:58:19 +00:00
} else
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > render ( ) ;
2018-01-17 01:58:19 +00:00
}
2017-03-02 20:33:22 +00:00
}
2017-11-18 00:14:18 +00:00
}
}
2017-03-02 20:33:22 +00:00
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > copyCameraImageData ( pixelRGBA , numRequestedPixels ,
2017-11-18 00:14:18 +00:00
depthBuffer , numRequestedPixels ,
segmentationMaskBuffer , numRequestedPixels ,
startPixelIndex , & width , & height , & numPixelsCopied ) ;
2018-01-17 01:58:19 +00:00
}
2017-03-02 20:33:22 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > debugDisplayCameraImageData ( clientCmd . m_requestPixelDataArguments . m_viewMatrix ,
clientCmd . m_requestPixelDataArguments . m_projectionMatrix , pixelRGBA , numRequestedPixels ,
depthBuffer , numRequestedPixels ,
segmentationMaskBuffer , numRequestedPixels ,
startPixelIndex , width , height , & numPixelsCopied ) ;
2017-03-02 20:33:22 +00:00
2017-11-18 00:14:18 +00:00
}
2016-12-27 03:40:09 +00:00
2017-11-18 00:14:18 +00:00
//each pixel takes 4 RGBA values and 1 float = 8 bytes
2016-12-27 03:40:09 +00:00
2017-11-18 00:14:18 +00:00
} else
{
2016-12-27 03:40:09 +00:00
2017-11-18 00:14:18 +00:00
}
2016-12-27 03:40:09 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_CAMERA_IMAGE_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
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 ;
return hasStatus ;
}
2016-12-27 03:40:09 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processSaveWorldCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = false ;
BT_PROFILE ( " CMD_SAVE_WORLD " ) ;
serverStatusOut . m_type = CMD_SAVE_WORLD_FAILED ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
{
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
int constraintCount = 0 ;
FILE * f = fopen ( clientCmd . m_sdfArguments . m_sdfFileName , " w " ) ;
if ( f )
{
char line [ 1024 ] ;
{
sprintf ( line , " import pybullet as p \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " cin = p.connect(p.SHARED_MEMORY) \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " if (cin < 0): \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " cin = p.connect(p.GUI) \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
//for each objects ...
for ( int i = 0 ; i < m_data - > m_saveWorldBodyData . size ( ) ; i + + )
{
SaveWorldObjectData & sd = m_data - > m_saveWorldBodyData [ i ] ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; i < sd . m_bodyUniqueIds . size ( ) ; i + + )
{
{
int bodyUniqueId = sd . m_bodyUniqueIds [ i ] ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( body )
{
if ( body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btTransform comTr = mb - > getBaseWorldTransform ( ) ;
btTransform tr = comTr * body - > m_rootLocalInertialFrame . inverse ( ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( strstr ( sd . m_fileName . c_str ( ) , " .urdf " ) )
{
sprintf ( line , " objects = [p.loadURDF( \" %s \" , %f,%f,%f,%f,%f,%f,%f)] \n " , sd . m_fileName . c_str ( ) ,
tr . getOrigin ( ) [ 0 ] , tr . getOrigin ( ) [ 1 ] , tr . getOrigin ( ) [ 2 ] ,
tr . getRotation ( ) [ 0 ] , tr . getRotation ( ) [ 1 ] , tr . getRotation ( ) [ 2 ] , tr . getRotation ( ) [ 3 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) & & i = = 0 )
{
sprintf ( line , " objects = p.loadSDF( \" %s \" ) \n " , sd . m_fileName . c_str ( ) ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
if ( strstr ( sd . m_fileName . c_str ( ) , " .xml " ) & & i = = 0 )
{
sprintf ( line , " objects = p.loadMJCF( \" %s \" ) \n " , sd . m_fileName . c_str ( ) ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) | | strstr ( sd . m_fileName . c_str ( ) , " .xml " ) | | ( ( strstr ( sd . m_fileName . c_str ( ) , " .urdf " ) ) & & mb - > getNumLinks ( ) ) )
{
sprintf ( line , " ob = objects[%d] \n " , i ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) | | strstr ( sd . m_fileName . c_str ( ) , " .xml " ) )
{
sprintf ( line , " p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f]) \n " ,
comTr . getOrigin ( ) [ 0 ] , comTr . getOrigin ( ) [ 1 ] , comTr . getOrigin ( ) [ 2 ] ,
comTr . getRotation ( ) [ 0 ] , comTr . getRotation ( ) [ 1 ] , comTr . getRotation ( ) [ 2 ] , comTr . getRotation ( ) [ 3 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( mb - > getNumLinks ( ) )
{
{
sprintf ( line , " jointPositions=[ " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
btScalar jointPos = mb - > getJointPosMultiDof ( i ) [ 0 ] ;
if ( i < mb - > getNumLinks ( ) - 1 )
{
sprintf ( line , " %f, " , jointPos ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
} else
{
sprintf ( line , " %f " , jointPos ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
{
sprintf ( line , " ] \n for jointIndex in range (p.getNumJoints(ob)): \n \t p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) \n \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
}
} else
{
//todo: btRigidBody/btSoftBody etc case
}
2015-11-23 04:50:32 +00:00
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
//for URDF, load at origin, then reposition...
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
struct SaveWorldObjectData
2016-05-18 06:57:19 +00:00
{
2017-11-18 00:14:18 +00:00
b3AlignedObjectArray < int > m_bodyUniqueIds ;
std : : string m_fileName ;
} ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
//user constraints
{
for ( int i = 0 ; i < m_data - > m_userConstraints . size ( ) ; i + + )
{
InteralUserConstraintData * ucptr = m_data - > m_userConstraints . getAtIndex ( i ) ;
b3UserConstraint & uc = ucptr - > m_userConstraintData ;
int parentBodyIndex = uc . m_parentBodyIndex ;
int parentJointIndex = uc . m_parentJointIndex ;
int childBodyIndex = uc . m_childBodyIndex ;
int childJointIndex = uc . m_childJointIndex ;
btVector3 jointAxis ( uc . m_jointAxis [ 0 ] , uc . m_jointAxis [ 1 ] , uc . m_jointAxis [ 2 ] ) ;
btVector3 pivotParent ( uc . m_parentFrame [ 0 ] , uc . m_parentFrame [ 1 ] , uc . m_parentFrame [ 2 ] ) ;
btVector3 pivotChild ( uc . m_childFrame [ 0 ] , uc . m_childFrame [ 1 ] , uc . m_childFrame [ 2 ] ) ;
btQuaternion ornFrameParent ( uc . m_parentFrame [ 3 ] , uc . m_parentFrame [ 4 ] , uc . m_parentFrame [ 5 ] , uc . m_parentFrame [ 6 ] ) ;
btQuaternion ornFrameChild ( uc . m_childFrame [ 3 ] , uc . m_childFrame [ 4 ] , uc . m_childFrame [ 5 ] , uc . m_childFrame [ 6 ] ) ;
{
char jointTypeStr [ 1024 ] = " FIXED " ;
bool hasKnownJointType = true ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
switch ( uc . m_jointType )
2017-09-13 05:14:00 +00:00
{
2017-11-18 00:14:18 +00:00
case eRevoluteType :
2017-09-13 05:14:00 +00:00
{
2017-11-18 00:14:18 +00:00
sprintf ( jointTypeStr , " p.JOINT_REVOLUTE " ) ;
break ;
2017-09-13 05:14:00 +00:00
}
2017-11-18 00:14:18 +00:00
case ePrismaticType :
{
sprintf ( jointTypeStr , " p.JOINT_PRISMATIC " ) ;
break ;
}
case eSphericalType :
{
sprintf ( jointTypeStr , " p.JOINT_SPHERICAL " ) ;
break ;
}
case ePlanarType :
{
sprintf ( jointTypeStr , " p.JOINT_PLANAR " ) ;
break ;
}
case eFixedType :
{
sprintf ( jointTypeStr , " p.JOINT_FIXED " ) ;
break ;
}
case ePoint2PointType :
{
sprintf ( jointTypeStr , " p.JOINT_POINT2POINT " ) ;
break ;
}
case eGearType :
2017-09-13 16:56:39 +00:00
{
2017-11-18 00:14:18 +00:00
sprintf ( jointTypeStr , " p.JOINT_GEAR " ) ;
break ;
2017-09-13 16:56:39 +00:00
}
2017-11-18 00:14:18 +00:00
default :
{
hasKnownJointType = false ;
b3Warning ( " unknown constraint type in SAVE_WORLD " ) ;
}
} ;
if ( hasKnownJointType )
2016-06-01 18:04:10 +00:00
{
2017-11-18 00:14:18 +00:00
{
sprintf ( line , " cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f]) \n " ,
constraintCount ,
parentBodyIndex ,
parentJointIndex ,
childBodyIndex ,
childJointIndex ,
jointTypeStr ,
jointAxis [ 0 ] , jointAxis [ 1 ] , jointAxis [ 2 ] ,
pivotParent [ 0 ] , pivotParent [ 1 ] , pivotParent [ 2 ] ,
pivotChild [ 0 ] , pivotChild [ 1 ] , pivotChild [ 2 ] ,
ornFrameParent [ 0 ] , ornFrameParent [ 1 ] , ornFrameParent [ 2 ] , ornFrameParent [ 3 ] ,
ornFrameChild [ 0 ] , ornFrameChild [ 1 ] , ornFrameChild [ 2 ] , ornFrameChild [ 3 ]
) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " p.changeConstraint(cid%d,maxForce=%f) \n " , constraintCount , uc . m_maxAppliedForce ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
constraintCount + + ;
}
}
}
}
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
{
btVector3 grav = this - > m_data - > m_dynamicsWorld - > getGravity ( ) ;
sprintf ( line , " p.setGravity(%f,%f,%f) \n " , grav [ 0 ] , grav [ 1 ] , grav [ 2 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
2018-03-15 03:47:56 +00:00
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
{
sprintf ( line , " p.stepSimulation() \n p.disconnect() \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
fclose ( f ) ;
}
2016-06-24 14:31:17 +00:00
2017-06-13 17:53:24 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_SAVE_WORLD_COMPLETED ;
hasStatus = true ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2017-06-13 17:53:24 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCreateCollisionShapeCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
serverStatusOut . m_type = CMD_CREATE_COLLISION_SHAPE_FAILED ;
2018-01-08 20:25:56 +00:00
btScalar defaultCollisionMargin = 0.001 ;
2017-11-23 02:12:02 +00:00
btMultiBodyWorldImporter * worldImporter = new btMultiBodyWorldImporter ( m_data - > m_dynamicsWorld ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
btCollisionShape * shape = 0 ;
b3AlignedObjectArray < UrdfCollision > urdfCollisionObjects ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
btCompoundShape * compound = 0 ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_createUserShapeArgs . m_numUserShapes > 1 )
{
compound = worldImporter - > createCompoundShape ( ) ;
}
2018-01-08 20:25:56 +00:00
for ( int i = 0 ; i < clientCmd . m_createUserShapeArgs . m_numUserShapes ; i + + )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
GLInstanceGraphicsShape * glmesh = 0 ;
char pathPrefix [ 1024 ] = " " ;
char relativeFileName [ 1024 ] = " " ;
2017-11-18 00:14:18 +00:00
UrdfCollision urdfColObj ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
btTransform childTransform ;
childTransform . setIdentity ( ) ;
if ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_hasChildTransform )
{
childTransform . setOrigin ( btVector3 ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childPosition [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childPosition [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childPosition [ 2 ] ) ) ;
childTransform . setRotation ( btQuaternion (
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childOrientation [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childOrientation [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childOrientation [ 2 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_childOrientation [ 3 ]
) ) ;
2018-01-08 20:25:56 +00:00
if ( compound = = 0 )
2017-11-18 00:14:18 +00:00
{
compound = worldImporter - > createCompoundShape ( ) ;
}
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
urdfColObj . m_linkLocalFrame = childTransform ;
urdfColObj . m_sourceFileLocation = " memory " ;
urdfColObj . m_name = " memory " ;
urdfColObj . m_geometry . m_type = URDF_GEOM_UNKNOWN ;
switch ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_type )
{
2018-01-08 20:25:56 +00:00
case GEOM_SPHERE :
{
double radius = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_sphereRadius ;
shape = worldImporter - > createSphereShape ( radius ) ;
if ( compound )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
compound - > addChildShape ( childTransform , shape ) ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 20:25:56 +00:00
urdfColObj . m_geometry . m_type = URDF_GEOM_SPHERE ;
urdfColObj . m_geometry . m_sphereRadius = radius ;
break ;
}
case GEOM_BOX :
{
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents (
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_boxHalfExtents [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_boxHalfExtents [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_boxHalfExtents [ 2 ] ) ;
shape = worldImporter - > createBoxShape ( halfExtents ) ;
if ( compound )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
compound - > addChildShape ( childTransform , shape ) ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 20:25:56 +00:00
urdfColObj . m_geometry . m_type = URDF_GEOM_BOX ;
urdfColObj . m_geometry . m_boxSize = 2. * halfExtents ;
break ;
}
case GEOM_CAPSULE :
{
shape = worldImporter - > createCapsuleShapeZ ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleRadius ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleHeight ) ;
if ( compound )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
compound - > addChildShape ( childTransform , shape ) ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 20:25:56 +00:00
urdfColObj . m_geometry . m_type = URDF_GEOM_CAPSULE ;
urdfColObj . m_geometry . m_capsuleRadius = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleRadius ;
urdfColObj . m_geometry . m_capsuleHeight = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleHeight ;
break ;
}
case GEOM_CYLINDER :
{
shape = worldImporter - > createCylinderShapeZ ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleRadius ,
0.5 * clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleHeight ) ;
if ( compound )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
compound - > addChildShape ( childTransform , shape ) ;
}
urdfColObj . m_geometry . m_type = URDF_GEOM_CYLINDER ;
urdfColObj . m_geometry . m_capsuleRadius = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleRadius ;
urdfColObj . m_geometry . m_capsuleHeight = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_capsuleHeight ;
2017-03-26 20:06:46 +00:00
2018-01-08 20:25:56 +00:00
break ;
}
case GEOM_PLANE :
{
btVector3 planeNormal ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 2 ] ) ;
shape = worldImporter - > createPlaneShape ( planeNormal , 0 ) ;
if ( compound )
{
compound - > addChildShape ( childTransform , shape ) ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 20:25:56 +00:00
urdfColObj . m_geometry . m_type = URDF_GEOM_PLANE ;
urdfColObj . m_geometry . m_planeNormal . setValue (
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_planeNormal [ 2 ] ) ;
break ;
}
case GEOM_MESH :
{
btVector3 meshScale ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 2 ] ) ;
const std : : string & urdf_path = " " ;
std : : string fileName = clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshFileName ;
urdfColObj . m_geometry . m_type = URDF_GEOM_MESH ;
urdfColObj . m_geometry . m_meshFileName = fileName ;
urdfColObj . m_geometry . m_meshScale = meshScale ;
2018-03-15 03:47:56 +00:00
2018-01-08 20:25:56 +00:00
pathPrefix [ 0 ] = 0 ;
if ( b3ResourcePath : : findResourcePath ( fileName . c_str ( ) , relativeFileName , 1024 ) )
2017-11-18 00:14:18 +00:00
{
2017-01-23 03:08:31 +00:00
2018-01-08 20:25:56 +00:00
b3FileUtils : : extractPath ( relativeFileName , pathPrefix , 1024 ) ;
2017-11-18 00:14:18 +00:00
}
2017-01-23 03:08:31 +00:00
2018-01-08 20:25:56 +00:00
const std : : string & error_message_prefix = " " ;
std : : string out_found_filename ;
int out_type ;
2017-01-23 03:08:31 +00:00
2018-01-08 20:25:56 +00:00
bool foundFile = findExistingMeshFile ( pathPrefix , relativeFileName , error_message_prefix , & out_found_filename , & out_type ) ;
if ( foundFile )
{
urdfColObj . m_geometry . m_meshFileType = out_type ;
2017-03-26 20:06:46 +00:00
2018-01-08 20:25:56 +00:00
if ( out_type = = UrdfGeometry : : FILE_STL )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
glmesh = LoadMeshFromSTL ( relativeFileName ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
}
2018-01-08 20:25:56 +00:00
if ( out_type = = UrdfGeometry : : FILE_OBJ )
2016-10-13 06:03:36 +00:00
{
2018-01-08 20:25:56 +00:00
//create a convex hull for each shape, and store it in a btCompoundShape
2017-03-26 20:06:46 +00:00
2018-01-08 20:25:56 +00:00
if ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH )
2016-10-13 06:03:36 +00:00
{
2018-01-08 20:25:56 +00:00
glmesh = LoadMeshFromObj ( relativeFileName , pathPrefix ) ;
2017-11-18 00:14:18 +00:00
2018-01-08 20:25:56 +00:00
}
else
{
2017-11-18 00:14:18 +00:00
2018-01-08 20:25:56 +00:00
std : : vector < tinyobj : : shape_t > shapes ;
std : : string err = tinyobj : : LoadObj ( shapes , out_found_filename . c_str ( ) ) ;
2017-11-18 00:14:18 +00:00
2018-01-08 20:25:56 +00:00
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
B3_PROFILE ( " createConvexHullFromShapes " ) ;
if ( compound = = 0 )
2017-11-18 00:14:18 +00:00
{
2018-01-08 20:25:56 +00:00
compound = worldImporter - > createCompoundShape ( ) ;
}
compound - > setMargin ( defaultCollisionMargin ) ;
2017-04-21 17:28:20 +00:00
2018-01-08 20:25:56 +00:00
for ( int s = 0 ; s < ( int ) shapes . size ( ) ; s + + )
{
btConvexHullShape * convexHull = worldImporter - > createConvexHullShape ( ) ;
convexHull - > setMargin ( defaultCollisionMargin ) ;
tinyobj : : shape_t & shape = shapes [ s ] ;
int faceCount = shape . mesh . indices . size ( ) ;
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
for ( int f = 0 ; f < faceCount ; f + = 3 )
2017-11-18 00:14:18 +00:00
{
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
btVector3 pt ;
pt . setValue ( shape . mesh . positions [ shape . mesh . indices [ f ] * 3 + 0 ] ,
shape . mesh . positions [ shape . mesh . indices [ f ] * 3 + 1 ] ,
shape . mesh . positions [ shape . mesh . indices [ f ] * 3 + 2 ] ) ;
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
convexHull - > addPoint ( pt * meshScale , false ) ;
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
pt . setValue ( shape . mesh . positions [ shape . mesh . indices [ f + 1 ] * 3 + 0 ] ,
shape . mesh . positions [ shape . mesh . indices [ f + 1 ] * 3 + 1 ] ,
shape . mesh . positions [ shape . mesh . indices [ f + 1 ] * 3 + 2 ] ) ;
convexHull - > addPoint ( pt * meshScale , false ) ;
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
pt . setValue ( shape . mesh . positions [ shape . mesh . indices [ f + 2 ] * 3 + 0 ] ,
shape . mesh . positions [ shape . mesh . indices [ f + 2 ] * 3 + 1 ] ,
shape . mesh . positions [ shape . mesh . indices [ f + 2 ] * 3 + 2 ] ) ;
convexHull - > addPoint ( pt * meshScale , false ) ;
2016-10-13 06:03:36 +00:00
}
2018-01-08 20:25:56 +00:00
convexHull - > recalcLocalAabb ( ) ;
convexHull - > optimizeConvexHull ( ) ;
compound - > addChildShape ( childTransform , convexHull ) ;
2017-11-18 00:14:18 +00:00
}
}
}
}
2018-01-08 20:25:56 +00:00
break ;
}
default :
{
}
2017-11-18 00:14:18 +00:00
}
if ( urdfColObj . m_geometry . m_type ! = URDF_GEOM_UNKNOWN )
{
urdfCollisionObjects . push_back ( urdfColObj ) ;
}
2016-10-13 06:03:36 +00:00
2018-01-08 20:25:56 +00:00
if ( glmesh )
{
btVector3 meshScale ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_meshScale [ 2 ] ) ;
if ( ! glmesh | | glmesh - > m_numvertices < = 0 )
{
b3Warning ( " %s: cannot extract mesh from '%s' \n " , pathPrefix , relativeFileName ) ;
delete glmesh ;
}
else
{
btAlignedObjectArray < btVector3 > convertedVerts ;
convertedVerts . reserve ( glmesh - > m_numvertices ) ;
2018-01-09 18:10:36 +00:00
for ( int v = 0 ; v < glmesh - > m_numvertices ; v + + )
2018-01-08 20:25:56 +00:00
{
convertedVerts . push_back ( btVector3 (
2018-01-09 18:10:36 +00:00
glmesh - > m_vertices - > at ( v ) . xyzw [ 0 ] * meshScale [ 0 ] ,
glmesh - > m_vertices - > at ( v ) . xyzw [ 1 ] * meshScale [ 1 ] ,
glmesh - > m_vertices - > at ( v ) . xyzw [ 2 ] * meshScale [ 2 ] ) ) ;
2018-01-08 20:25:56 +00:00
}
if ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH )
{
2018-03-15 03:47:56 +00:00
2018-01-08 20:25:56 +00:00
BT_PROFILE ( " convert trimesh " ) ;
btTriangleMesh * meshInterface = new btTriangleMesh ( ) ;
this - > m_data - > m_meshInterfaces . push_back ( meshInterface ) ;
{
BT_PROFILE ( " convert vertices " ) ;
for ( int i = 0 ; i < glmesh - > m_numIndices / 3 ; i + + )
{
const btVector3 & v0 = convertedVerts [ glmesh - > m_indices - > at ( i * 3 ) ] ;
const btVector3 & v1 = convertedVerts [ glmesh - > m_indices - > at ( i * 3 + 1 ) ] ;
const btVector3 & v2 = convertedVerts [ glmesh - > m_indices - > at ( i * 3 + 2 ) ] ;
meshInterface - > addTriangle ( v0 , v1 , v2 ) ;
}
}
2018-02-17 03:44:33 +00:00
2018-01-08 20:25:56 +00:00
{
BT_PROFILE ( " create btBvhTriangleMeshShape " ) ;
btBvhTriangleMeshShape * trimesh = new btBvhTriangleMeshShape ( meshInterface , true , true ) ;
m_data - > m_collisionShapes . push_back ( trimesh ) ;
2018-02-17 03:44:33 +00:00
if ( clientCmd . m_createUserShapeArgs . m_shapes [ i ] . m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE )
{
btTriangleInfoMap * triangleInfoMap = new btTriangleInfoMap ( ) ;
btGenerateInternalEdgeInfo ( trimesh , triangleInfoMap ) ;
}
2018-01-08 20:25:56 +00:00
//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
shape = trimesh ;
if ( compound )
{
compound - > addChildShape ( childTransform , shape ) ;
}
}
delete glmesh ;
}
else
{
//convex mesh
if ( compound = = 0 )
{
compound = worldImporter - > createCompoundShape ( ) ;
}
compound - > setMargin ( defaultCollisionMargin ) ;
{
btConvexHullShape * convexHull = worldImporter - > createConvexHullShape ( ) ;
convexHull - > setMargin ( defaultCollisionMargin ) ;
for ( int v = 0 ; v < convertedVerts . size ( ) ; v + + )
{
btVector3 pt = convertedVerts [ v ] ;
convexHull - > addPoint ( pt , false ) ;
}
convexHull - > recalcLocalAabb ( ) ;
convexHull - > optimizeConvexHull ( ) ;
compound - > addChildShape ( childTransform , convexHull ) ;
}
}
}
2018-03-15 03:47:56 +00:00
}
2018-01-08 20:25:56 +00:00
}
2017-11-18 00:14:18 +00:00
if ( compound & & compound - > getNumChildShapes ( ) )
{
shape = compound ;
}
2017-01-23 03:08:31 +00:00
2017-11-18 00:14:18 +00:00
if ( shape )
{
int collisionShapeUid = m_data - > m_userCollisionShapeHandles . allocHandle ( ) ;
InternalCollisionShapeHandle * handle = m_data - > m_userCollisionShapeHandles . getHandle ( collisionShapeUid ) ;
handle - > m_collisionShape = shape ;
for ( int i = 0 ; i < urdfCollisionObjects . size ( ) ; i + + )
{
handle - > m_urdfCollisionObjects . push_back ( urdfCollisionObjects [ i ] ) ;
}
serverStatusOut . m_createUserShapeResultArgs . m_userShapeUniqueId = collisionShapeUid ;
m_data - > m_worldImporters . push_back ( worldImporter ) ;
serverStatusOut . m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED ;
} else
{
delete worldImporter ;
}
2016-10-13 06:03:36 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processCreateVisualShapeCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
serverStatusOut . m_type = CMD_CREATE_VISUAL_SHAPE_FAILED ;
double globalScaling = 1.f ;
2018-01-09 18:10:36 +00:00
int flags = 0 ;
2018-01-17 20:48:48 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , m_data - > m_pluginManager . getRenderInterface ( ) , globalScaling , flags ) ;
2017-11-18 00:14:18 +00:00
u2b . setEnableTinyRenderer ( m_data - > m_enableTinyRenderer ) ;
btTransform localInertiaFrame ;
localInertiaFrame . setIdentity ( ) ;
2018-03-29 02:08:18 +00:00
2017-11-18 00:14:18 +00:00
const char * pathPrefix = " " ;
2018-03-29 02:08:18 +00:00
int visualShapeUniqueId = - 1 ;
2018-03-15 03:47:56 +00:00
UrdfVisual visualShape ;
2018-03-10 02:02:06 +00:00
for ( int userShapeIndex = 0 ; userShapeIndex < clientCmd . m_createUserShapeArgs . m_numUserShapes ; userShapeIndex + + )
2017-11-18 00:14:18 +00:00
{
2018-03-29 02:08:18 +00:00
btTransform childTrans ;
childTrans . setIdentity ( ) ;
2017-11-18 00:14:18 +00:00
visualShape . m_geometry . m_type = ( UrdfGeomTypes ) clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_type ;
char relativeFileName [ 1024 ] ;
char pathPrefix [ 1024 ] ;
pathPrefix [ 0 ] = 0 ;
2017-12-31 01:42:13 +00:00
const b3CreateUserShapeData & visShape = clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] ;
switch ( visualShape . m_geometry . m_type )
2017-11-18 00:14:18 +00:00
{
2018-03-10 02:02:06 +00:00
case URDF_GEOM_CYLINDER :
{
visualShape . m_geometry . m_capsuleHeight = visShape . m_capsuleHeight ;
visualShape . m_geometry . m_capsuleRadius = visShape . m_capsuleRadius ;
break ;
}
case URDF_GEOM_BOX :
{
visualShape . m_geometry . m_boxSize . setValue ( 2. * visShape . m_boxHalfExtents [ 0 ] ,
2. * visShape . m_boxHalfExtents [ 1 ] ,
2. * visShape . m_boxHalfExtents [ 2 ] ) ;
break ;
}
case URDF_GEOM_SPHERE :
{
visualShape . m_geometry . m_sphereRadius = visShape . m_sphereRadius ;
break ;
}
case URDF_GEOM_CAPSULE :
{
visualShape . m_geometry . m_hasFromTo = visShape . m_hasFromTo ;
if ( visualShape . m_geometry . m_hasFromTo )
2017-11-18 00:14:18 +00:00
{
2018-03-10 02:02:06 +00:00
visualShape . m_geometry . m_capsuleFrom . setValue ( visShape . m_capsuleFrom [ 0 ] ,
visShape . m_capsuleFrom [ 1 ] ,
visShape . m_capsuleFrom [ 2 ] ) ;
visualShape . m_geometry . m_capsuleTo . setValue ( visShape . m_capsuleTo [ 0 ] ,
visShape . m_capsuleTo [ 1 ] ,
visShape . m_capsuleTo [ 2 ] ) ;
2017-11-18 00:14:18 +00:00
}
2018-03-10 02:02:06 +00:00
else
2017-12-31 01:42:13 +00:00
{
2018-03-10 02:02:06 +00:00
visualShape . m_geometry . m_capsuleHeight = visShape . m_capsuleHeight ;
visualShape . m_geometry . m_capsuleRadius = visShape . m_capsuleRadius ;
2017-12-31 01:42:13 +00:00
}
2018-03-10 02:02:06 +00:00
break ;
}
case URDF_GEOM_MESH :
{
2017-11-18 00:14:18 +00:00
2018-03-10 02:02:06 +00:00
std : : string fileName = clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_meshFileName ;
const std : : string & error_message_prefix = " " ;
std : : string out_found_filename ;
int out_type ;
if ( b3ResourcePath : : findResourcePath ( fileName . c_str ( ) , relativeFileName , 1024 ) )
2017-12-31 01:42:13 +00:00
{
2018-03-10 02:02:06 +00:00
b3FileUtils : : extractPath ( relativeFileName , pathPrefix , 1024 ) ;
2017-12-31 01:42:13 +00:00
}
2018-03-10 02:02:06 +00:00
bool foundFile = findExistingMeshFile ( pathPrefix , relativeFileName , error_message_prefix , & out_found_filename , & out_type ) ;
visualShape . m_geometry . m_meshFileType = out_type ;
visualShape . m_geometry . m_meshFileName = fileName ;
2017-12-31 01:42:13 +00:00
2018-03-10 02:02:06 +00:00
visualShape . m_geometry . m_meshScale . setValue ( clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_meshScale [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_meshScale [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_meshScale [ 2 ] ) ;
break ;
2017-12-31 01:42:13 +00:00
2018-03-10 02:02:06 +00:00
}
2017-12-31 01:42:13 +00:00
2018-03-10 02:02:06 +00:00
default :
{
}
2017-12-31 01:42:13 +00:00
} ;
visualShape . m_name = " in_memory " ;
2018-03-10 02:02:06 +00:00
visualShape . m_materialName = " " ;
visualShape . m_sourceFileLocation = " in_memory_unknown_line " ;
2017-11-18 00:14:18 +00:00
visualShape . m_linkLocalFrame . setIdentity ( ) ;
visualShape . m_geometry . m_hasLocalMaterial = false ;
2018-03-10 02:02:06 +00:00
bool hasRGBA = ( clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_visualFlags & GEOM_VISUAL_HAS_RGBA_COLOR ) ! = 0 ; ;
bool hasSpecular = ( clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_visualFlags & GEOM_VISUAL_HAS_SPECULAR_COLOR ) ! = 0 ; ;
visualShape . m_geometry . m_hasLocalMaterial = hasRGBA | hasSpecular ;
2017-11-18 00:14:18 +00:00
if ( visualShape . m_geometry . m_hasLocalMaterial )
{
if ( hasRGBA )
{
2018-03-10 02:02:06 +00:00
visualShape . m_geometry . m_localMaterial . m_matColor . m_rgbaColor . setValue (
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_rgbaColor [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_rgbaColor [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_rgbaColor [ 2 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_rgbaColor [ 3 ] ) ;
}
else
2017-11-18 00:14:18 +00:00
{
2018-03-10 02:02:06 +00:00
2017-11-18 00:14:18 +00:00
}
if ( hasSpecular )
{
visualShape . m_geometry . m_localMaterial . m_matColor . m_specularColor . setValue (
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_specularColor [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_specularColor [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_specularColor [ 2 ] ) ;
2018-03-10 02:02:06 +00:00
}
else
2017-11-18 00:14:18 +00:00
{
2018-03-10 02:02:06 +00:00
visualShape . m_geometry . m_localMaterial . m_matColor . m_specularColor . setValue ( 0.4 , 0.4 , 0.4 ) ;
2017-11-18 00:14:18 +00:00
}
}
2018-03-10 02:02:06 +00:00
if ( clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_hasChildTransform ! = 0 )
2017-11-18 00:14:18 +00:00
{
childTrans . setOrigin ( btVector3 ( clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childPosition [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childPosition [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childPosition [ 2 ] ) ) ;
childTrans . setRotation ( btQuaternion (
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childOrientation [ 0 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childOrientation [ 1 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childOrientation [ 2 ] ,
clientCmd . m_createUserShapeArgs . m_shapes [ userShapeIndex ] . m_childOrientation [ 3 ] ) ) ;
}
2016-10-13 06:03:36 +00:00
2018-03-10 02:02:06 +00:00
2018-03-29 02:08:18 +00:00
if ( visualShapeUniqueId < 0 )
2017-11-18 00:14:18 +00:00
{
2018-03-29 02:08:18 +00:00
visualShapeUniqueId = m_data - > m_userVisualShapeHandles . allocHandle ( ) ;
2017-11-18 00:14:18 +00:00
}
2018-03-29 02:08:18 +00:00
InternalVisualShapeHandle * visualHandle = m_data - > m_userVisualShapeHandles . getHandle ( visualShapeUniqueId ) ;
visualHandle - > m_OpenGLGraphicsIndex = - 1 ;
visualHandle - > m_tinyRendererVisualShapeIndex = - 1 ;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualShape . m_linkLocalFrame = childTrans ;
visualHandle - > m_visualShapes . push_back ( visualShape ) ;
visualHandle - > m_pathPrefixes . push_back ( pathPrefix [ 0 ] ? pathPrefix : " " ) ;
serverStatusOut . m_createUserShapeResultArgs . m_userShapeUniqueId = visualShapeUniqueId ;
serverStatusOut . m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED ;
2017-11-18 00:14:18 +00:00
}
2018-03-10 02:02:06 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCustomCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CUSTOM_COMMAND_FAILED ;
serverCmd . m_customCommandResultArgs . m_pluginUniqueId = - 1 ;
2016-12-31 22:43:15 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN )
{
//pluginPath could be registered or load from disk
const char * postFix = " " ;
if ( clientCmd . m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX )
{
postFix = clientCmd . m_customCommandArgs . m_postFix ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
int pluginUniqueId = m_data - > m_pluginManager . loadPlugin ( clientCmd . m_customCommandArgs . m_pluginPath , postFix ) ;
if ( pluginUniqueId > = 0 )
{
serverCmd . m_customCommandResultArgs . m_pluginUniqueId = pluginUniqueId ;
serverCmd . m_type = CMD_CUSTOM_COMMAND_COMPLETED ;
}
}
if ( clientCmd . m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN )
{
m_data - > m_pluginManager . unloadPlugin ( clientCmd . m_customCommandArgs . m_pluginUniqueId ) ;
serverCmd . m_type = CMD_CUSTOM_COMMAND_COMPLETED ;
}
if ( clientCmd . m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int result = m_data - > m_pluginManager . executePluginCommand ( clientCmd . m_customCommandArgs . m_pluginUniqueId , & clientCmd . m_customCommandArgs . m_arguments ) ;
serverCmd . m_customCommandResultArgs . m_executeCommandResult = result ;
serverCmd . m_type = CMD_CUSTOM_COMMAND_COMPLETED ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processUserDebugDrawCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_USER_DEBUG_DRAW " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_FAILED ;
2018-03-15 03:47:56 +00:00
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
int trackingVisualShapeIndex = - 1 ;
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_userDebugDrawArgs . m_parentObjectUniqueId > = 0 )
{
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( clientCmd . m_userDebugDrawArgs . m_parentObjectUniqueId ) ;
if ( bodyHandle )
{
int linkIndex = - 1 ;
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
if ( bodyHandle - > m_multiBody )
{
int linkIndex = clientCmd . m_userDebugDrawArgs . m_parentLinkIndex ;
if ( linkIndex = = - 1 )
{
if ( bodyHandle - > m_multiBody - > getBaseCollider ( ) )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
trackingVisualShapeIndex = bodyHandle - > m_multiBody - > getBaseCollider ( ) - > getUserIndex ( ) ;
2017-06-03 17:57:56 +00:00
}
2017-11-18 00:14:18 +00:00
} else
{
if ( linkIndex > = 0 & & linkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
if ( bodyHandle - > m_multiBody - > getLink ( linkIndex ) . m_collider )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
trackingVisualShapeIndex = bodyHandle - > m_multiBody - > getLink ( linkIndex ) . m_collider - > getUserIndex ( ) ;
2017-06-03 17:57:56 +00:00
}
2017-11-18 00:14:18 +00:00
}
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
if ( bodyHandle - > m_rigidBody )
{
trackingVisualShapeIndex = bodyHandle - > m_rigidBody - > getUserIndex ( ) ;
}
}
}
2017-06-30 05:06:27 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & USER_DEBUG_ADD_PARAMETER )
{
int uid = m_data - > m_guiHelper - > addUserDebugParameter (
clientCmd . m_userDebugDrawArgs . m_text ,
clientCmd . m_userDebugDrawArgs . m_rangeMin ,
clientCmd . m_userDebugDrawArgs . m_rangeMax ,
clientCmd . m_userDebugDrawArgs . m_startValue
) ;
serverCmd . m_userDebugDrawArgs . m_debugItemUniqueId = uid ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
}
if ( clientCmd . m_updateFlags & USER_DEBUG_READ_PARAMETER )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int ok = m_data - > m_guiHelper - > readUserDebugParameter (
clientCmd . m_userDebugDrawArgs . m_itemUniqueId ,
& serverCmd . m_userDebugDrawArgs . m_parameterValue ) ;
if ( ok )
{
serverCmd . m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED ;
2018-03-15 03:47:56 +00:00
}
2017-11-18 00:14:18 +00:00
}
if ( ( clientCmd . m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR ) | | ( clientCmd . m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR ) )
{
int bodyUniqueId = clientCmd . m_userDebugDrawArgs . m_objectUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( body )
{
btCollisionObject * destColObj = 0 ;
2017-06-30 05:06:27 +00:00
2017-11-18 00:14:18 +00:00
if ( body - > m_multiBody )
{
if ( clientCmd . m_userDebugDrawArgs . m_linkIndex = = - 1 )
{
destColObj = body - > m_multiBody - > getBaseCollider ( ) ;
}
else
{
if ( clientCmd . m_userDebugDrawArgs . m_linkIndex > = 0 & & clientCmd . m_userDebugDrawArgs . m_linkIndex < body - > m_multiBody - > getNumLinks ( ) )
{
destColObj = body - > m_multiBody - > getLink ( clientCmd . m_userDebugDrawArgs . m_linkIndex ) . m_collider ;
}
}
2017-06-30 05:06:27 +00:00
2017-11-18 00:14:18 +00:00
}
if ( body - > m_rigidBody )
{
destColObj = body - > m_rigidBody ;
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( destColObj )
{
if ( clientCmd . m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR )
{
destColObj - > removeCustomDebugColor ( ) ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
}
if ( clientCmd . m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR )
{
btVector3 objectColorRGB ;
objectColorRGB . setValue ( clientCmd . m_userDebugDrawArgs . m_objectDebugColorRGB [ 0 ] ,
clientCmd . m_userDebugDrawArgs . m_objectDebugColorRGB [ 1 ] ,
clientCmd . m_userDebugDrawArgs . m_objectDebugColorRGB [ 2 ] ) ;
destColObj - > setCustomDebugColor ( objectColorRGB ) ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
}
}
}
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & USER_DEBUG_HAS_TEXT )
{
//addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
int optionFlags = clientCmd . m_userDebugDrawArgs . m_optionFlags ;
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION ) = = 0 )
{
optionFlags | = DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA ;
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
int replaceItemUniqueId = - 1 ;
if ( ( clientCmd . m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID ) ! = 0 )
{
replaceItemUniqueId = clientCmd . m_userDebugDrawArgs . m_replaceItemUniqueId ;
}
2017-06-30 05:06:27 +00:00
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
int uid = m_data - > m_guiHelper - > addUserDebugText3D ( clientCmd . m_userDebugDrawArgs . m_text ,
clientCmd . m_userDebugDrawArgs . m_textPositionXYZ ,
clientCmd . m_userDebugDrawArgs . m_textOrientation ,
clientCmd . m_userDebugDrawArgs . m_textColorRGB ,
clientCmd . m_userDebugDrawArgs . m_textSize ,
clientCmd . m_userDebugDrawArgs . m_lifeTime ,
trackingVisualShapeIndex ,
optionFlags ,
replaceItemUniqueId ) ;
2017-06-23 21:43:28 +00:00
2017-11-18 00:14:18 +00:00
if ( uid > = 0 )
{
serverCmd . m_userDebugDrawArgs . m_debugItemUniqueId = uid ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
}
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & USER_DEBUG_HAS_LINE )
{
2018-06-22 23:47:20 +00:00
int replaceItemUid = - 1 ;
if ( clientCmd . m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID )
{
replaceItemUid = clientCmd . m_userDebugDrawArgs . m_replaceItemUniqueId ;
}
2017-11-18 00:14:18 +00:00
int uid = m_data - > m_guiHelper - > addUserDebugLine (
clientCmd . m_userDebugDrawArgs . m_debugLineFromXYZ ,
clientCmd . m_userDebugDrawArgs . m_debugLineToXYZ ,
clientCmd . m_userDebugDrawArgs . m_debugLineColorRGB ,
clientCmd . m_userDebugDrawArgs . m_lineWidth ,
clientCmd . m_userDebugDrawArgs . m_lifeTime ,
2018-06-22 23:47:20 +00:00
trackingVisualShapeIndex ,
replaceItemUid
) ;
2017-11-18 00:14:18 +00:00
if ( uid > = 0 )
{
serverCmd . m_userDebugDrawArgs . m_debugItemUniqueId = uid ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
}
}
2018-03-15 03:47:56 +00:00
2017-06-23 21:43:28 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & USER_DEBUG_REMOVE_ALL )
{
m_data - > m_guiHelper - > removeAllUserDebugItems ( ) ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
}
if ( clientCmd . m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM )
{
m_data - > m_guiHelper - > removeUserDebugItem ( clientCmd . m_userDebugDrawArgs . m_itemUniqueId ) ;
serverCmd . m_type = CMD_USER_DEBUG_DRAW_COMPLETED ;
2017-06-23 21:43:28 +00:00
2017-11-18 00:14:18 +00:00
}
return hasStatus ;
}
2017-06-23 21:43:28 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processSetVRCameraStateCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SET_VR_CAMERA_STATE " ) ;
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & VR_CAMERA_ROOT_POSITION )
{
gVRTeleportPos1 [ 0 ] = clientCmd . m_vrCameraStateArguments . m_rootPosition [ 0 ] ;
gVRTeleportPos1 [ 1 ] = clientCmd . m_vrCameraStateArguments . m_rootPosition [ 1 ] ;
gVRTeleportPos1 [ 2 ] = clientCmd . m_vrCameraStateArguments . m_rootPosition [ 2 ] ;
}
if ( clientCmd . m_updateFlags & VR_CAMERA_ROOT_ORIENTATION )
{
gVRTeleportOrn [ 0 ] = clientCmd . m_vrCameraStateArguments . m_rootOrientation [ 0 ] ;
gVRTeleportOrn [ 1 ] = clientCmd . m_vrCameraStateArguments . m_rootOrientation [ 1 ] ;
gVRTeleportOrn [ 2 ] = clientCmd . m_vrCameraStateArguments . m_rootOrientation [ 2 ] ;
gVRTeleportOrn [ 3 ] = clientCmd . m_vrCameraStateArguments . m_rootOrientation [ 3 ] ;
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT )
{
gVRTrackingObjectUniqueId = clientCmd . m_vrCameraStateArguments . m_trackingObjectUniqueId ;
}
2017-06-19 17:14:26 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & VR_CAMERA_FLAG )
{
gVRTrackingObjectFlag = clientCmd . m_vrCameraStateArguments . m_trackingObjectFlag ;
}
2017-06-30 05:06:27 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestVREventsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
//BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
serverStatusOut . m_sendVREvents . m_numVRControllerEvents = 0 ;
for ( int i = 0 ; i < MAX_VR_CONTROLLERS ; i + + )
{
b3VRControllerEvent & event = m_data - > m_vrControllerEvents . m_vrEvents [ i ] ;
if ( clientCmd . m_updateFlags & event . m_deviceType )
{
if ( event . m_numButtonEvents + event . m_numMoveEvents )
{
serverStatusOut . m_sendVREvents . m_controllerEvents [ serverStatusOut . m_sendVREvents . m_numVRControllerEvents + + ] = event ;
event . m_numButtonEvents = 0 ;
event . m_numMoveEvents = 0 ;
for ( int b = 0 ; b < MAX_VR_BUTTONS ; b + + )
{
event . m_buttons [ b ] = 0 ;
}
}
}
}
serverStatusOut . m_type = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestMouseEventsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
serverStatusOut . m_sendMouseEvents . m_numMouseEvents = m_data - > m_mouseEvents . size ( ) ;
if ( serverStatusOut . m_sendMouseEvents . m_numMouseEvents > MAX_MOUSE_EVENTS )
{
serverStatusOut . m_sendMouseEvents . m_numMouseEvents = MAX_MOUSE_EVENTS ;
}
for ( int i = 0 ; i < serverStatusOut . m_sendMouseEvents . m_numMouseEvents ; i + + )
{
serverStatusOut . m_sendMouseEvents . m_mouseEvents [ i ] = m_data - > m_mouseEvents [ i ] ;
}
m_data - > m_mouseEvents . resize ( 0 ) ;
serverStatusOut . m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestKeyboardEventsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
//BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
bool hasStatus = true ;
serverStatusOut . m_sendKeyboardEvents . m_numKeyboardEvents = m_data - > m_keyboardEvents . size ( ) ;
if ( serverStatusOut . m_sendKeyboardEvents . m_numKeyboardEvents > MAX_KEYBOARD_EVENTS )
{
serverStatusOut . m_sendKeyboardEvents . m_numKeyboardEvents = MAX_KEYBOARD_EVENTS ;
}
for ( int i = 0 ; i < serverStatusOut . m_sendKeyboardEvents . m_numKeyboardEvents ; i + + )
{
serverStatusOut . m_sendKeyboardEvents . m_keyboardEvents [ i ] = m_data - > m_keyboardEvents [ i ] ;
}
btAlignedObjectArray < b3KeyboardEvent > events ;
//remove out-of-date events
for ( int i = 0 ; i < m_data - > m_keyboardEvents . size ( ) ; i + + )
{
b3KeyboardEvent event = m_data - > m_keyboardEvents [ i ] ;
if ( event . m_keyState & eButtonIsDown )
{
event . m_keyState = eButtonIsDown ;
events . push_back ( event ) ;
}
}
m_data - > m_keyboardEvents . resize ( events . size ( ) ) ;
for ( int i = 0 ; i < events . size ( ) ; i + + )
{
m_data - > m_keyboardEvents [ i ] = events [ i ] ;
}
serverStatusOut . m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED ;
return hasStatus ;
}
2018-06-15 14:47:04 +00:00
# if __cplusplus >= 201103L
# include <atomic>
struct CastSyncInfo {
std : : atomic < int > m_nextTaskNumber ;
CastSyncInfo ( ) : m_nextTaskNumber ( 0 ) { }
inline int getNextTask ( ) {
return m_nextTaskNumber + + ;
}
} ;
# else // __cplusplus >= 201103L
struct CastSyncInfo {
volatile int m_nextTaskNumber ;
btSpinMutex m_taskLock ;
CastSyncInfo ( ) : m_nextTaskNumber ( 0 ) { }
inline int getNextTask ( ) {
m_taskLock . lock ( ) ;
const int taskNr = m_nextTaskNumber + + ;
m_taskLock . unlock ( ) ;
return taskNr ;
}
} ;
# endif // __cplusplus >= 201103L
2018-06-19 16:28:48 +00:00
struct BatchRayCaster
2017-11-18 00:14:18 +00:00
{
2018-06-19 16:28:48 +00:00
b3ThreadPool * m_threadPool ;
2018-06-15 14:47:04 +00:00
CastSyncInfo * m_syncInfo ;
const btCollisionWorld * m_world ;
const b3RayData * m_rayInputBuffer ;
b3RayHitInfo * m_hitInfoOutputBuffer ;
int m_numRays ;
2018-06-19 16:28:48 +00:00
BatchRayCaster ( b3ThreadPool * threadPool , const btCollisionWorld * world , const b3RayData * rayInputBuffer , b3RayHitInfo * hitInfoOutputBuffer , int numRays )
: m_threadPool ( threadPool ) , m_world ( world ) , m_rayInputBuffer ( rayInputBuffer ) , m_hitInfoOutputBuffer ( hitInfoOutputBuffer ) , m_numRays ( numRays ) {
2018-06-15 14:47:04 +00:00
m_syncInfo = new CastSyncInfo ;
}
2017-11-18 00:14:18 +00:00
2018-06-15 14:47:04 +00:00
~ BatchRayCaster ( ) {
delete m_syncInfo ;
}
void castRays ( int numWorkers ) {
# if BT_THREADSAFE
2018-06-19 16:28:48 +00:00
if ( numWorkers < = 1 ) {
castSequentially ( ) ;
}
else {
{
BT_PROFILE ( " BatchRayCaster_startingWorkerThreads " ) ;
int numTasks = btMin ( m_threadPool - > numWorkers ( ) , numWorkers - 1 ) ;
for ( int i = 0 ; i < numTasks ; i + + ) {
m_threadPool - > runTask ( i , BatchRayCaster : : rayCastWorker , this ) ;
}
}
rayCastWorker ( this ) ;
m_threadPool - > waitForAllTasks ( ) ;
2018-06-15 14:47:04 +00:00
}
2018-06-19 16:28:48 +00:00
# else // BT_THREADSAFE
castSequentially ( ) ;
2018-06-15 14:47:04 +00:00
# endif // BT_THREADSAFE
}
2018-06-19 16:28:48 +00:00
static void rayCastWorker ( void * arg ) {
BT_PROFILE ( " BatchRayCaster_raycastWorker " ) ;
BatchRayCaster * const obj = ( BatchRayCaster * ) arg ;
const int numRays = obj - > m_numRays ;
int taskNr ;
2018-06-15 14:47:04 +00:00
while ( true ) {
2018-06-19 16:28:48 +00:00
{
BT_PROFILE ( " CastSyncInfo_getNextTask " ) ;
taskNr = obj - > m_syncInfo - > getNextTask ( ) ;
}
if ( taskNr > = numRays )
2018-06-15 14:47:04 +00:00
return ;
2018-06-19 16:28:48 +00:00
obj - > processRay ( taskNr ) ;
2018-06-15 14:47:04 +00:00
}
}
2018-06-19 16:28:48 +00:00
void castSequentially ( ) {
for ( int i = 0 ; i < m_numRays ; i + + ) {
processRay ( i ) ;
}
}
void processRay ( int ray ) {
BT_PROFILE ( " BatchRayCaster_processRay " ) ;
2018-06-15 14:47:04 +00:00
const double * from = m_rayInputBuffer [ ray ] . m_rayFromPosition ;
const double * to = m_rayInputBuffer [ ray ] . m_rayToPosition ;
btVector3 rayFromWorld ( from [ 0 ] , from [ 1 ] , from [ 2 ] ) ;
btVector3 rayToWorld ( to [ 0 ] , to [ 1 ] , to [ 2 ] ) ;
2017-11-18 00:14:18 +00:00
btCollisionWorld : : ClosestRayResultCallback rayResultCallback ( rayFromWorld , rayToWorld ) ;
2018-03-07 22:51:51 +00:00
rayResultCallback . m_flags | = btTriangleRaycastCallback : : kF_UseGjkConvexCastRaytest ;
2018-06-15 14:47:04 +00:00
m_world - > rayTest ( rayFromWorld , rayToWorld , rayResultCallback ) ;
2017-11-18 00:14:18 +00:00
2018-06-15 14:47:04 +00:00
b3RayHitInfo & hit = m_hitInfoOutputBuffer [ ray ] ;
2017-11-18 00:14:18 +00:00
if ( rayResultCallback . hasHit ( ) )
{
2018-06-15 14:47:04 +00:00
hit . m_hitFraction = rayResultCallback . m_closestHitFraction ;
2017-11-18 00:14:18 +00:00
int objectUniqueId = - 1 ;
int linkIndex = - 1 ;
const btRigidBody * body = btRigidBody : : upcast ( rayResultCallback . m_collisionObject ) ;
if ( body )
{
objectUniqueId = rayResultCallback . m_collisionObject - > getUserIndex2 ( ) ;
} else
{
const btMultiBodyLinkCollider * mblB = btMultiBodyLinkCollider : : upcast ( rayResultCallback . m_collisionObject ) ;
if ( mblB & & mblB - > m_multiBody )
{
linkIndex = mblB - > m_link ;
objectUniqueId = mblB - > m_multiBody - > getUserIndex2 ( ) ;
}
}
2018-06-15 14:47:04 +00:00
hit . m_hitObjectUniqueId = objectUniqueId ;
hit . m_hitObjectLinkIndex = linkIndex ;
hit . m_hitPositionWorld [ 0 ] = rayResultCallback . m_hitPointWorld [ 0 ] ;
hit . m_hitPositionWorld [ 1 ] = rayResultCallback . m_hitPointWorld [ 1 ] ;
hit . m_hitPositionWorld [ 2 ] = rayResultCallback . m_hitPointWorld [ 2 ] ;
hit . m_hitNormalWorld [ 0 ] = rayResultCallback . m_hitNormalWorld [ 0 ] ;
hit . m_hitNormalWorld [ 1 ] = rayResultCallback . m_hitNormalWorld [ 1 ] ;
hit . m_hitNormalWorld [ 2 ] = rayResultCallback . m_hitNormalWorld [ 2 ] ;
2017-11-18 00:14:18 +00:00
} else
{
2018-06-15 14:47:04 +00:00
hit . m_hitFraction = 1 ;
hit . m_hitObjectUniqueId = - 1 ;
hit . m_hitObjectLinkIndex = - 1 ;
hit . m_hitPositionWorld [ 0 ] = 0 ;
hit . m_hitPositionWorld [ 1 ] = 0 ;
hit . m_hitPositionWorld [ 2 ] = 0 ;
hit . m_hitNormalWorld [ 0 ] = 0 ;
hit . m_hitNormalWorld [ 1 ] = 0 ;
hit . m_hitNormalWorld [ 2 ] = 0 ;
2017-11-18 00:14:18 +00:00
}
}
2018-06-15 14:47:04 +00:00
} ;
2018-06-19 16:28:48 +00:00
void PhysicsServerCommandProcessor : : createThreadPool ( )
2018-06-16 20:59:03 +00:00
{
# ifdef BT_THREADSAFE
2018-06-19 16:28:48 +00:00
if ( m_data - > m_threadPool = = 0 ) {
m_data - > m_threadPool = new b3ThreadPool ( " PhysicsServerCommandProcessorThreadPool " ) ;
2018-06-16 20:59:03 +00:00
}
# endif //BT_THREADSAFE
}
2018-06-15 14:47:04 +00:00
bool PhysicsServerCommandProcessor : : processRequestRaycastIntersectionsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_RAY_CAST_INTERSECTIONS " ) ;
serverStatusOut . m_raycastHits . m_numRaycastHits = 0 ;
2018-06-16 19:28:21 +00:00
const int numCommandRays = clientCmd . m_requestRaycastIntersections . m_numCommandRays ;
const int numStreamingRays = clientCmd . m_requestRaycastIntersections . m_numStreamingRays ;
2018-06-19 16:28:48 +00:00
const int totalRays = numCommandRays + numStreamingRays ;
int numThreads = clientCmd . m_requestRaycastIntersections . m_numThreads ;
if ( numThreads = = 0 ) {
// When 0 is specified, Bullet can decide how many threads to use.
// About 16 rays per thread seems to work reasonably well.
numThreads = btMax ( 1 , totalRays / 16 ) ;
}
if ( numThreads > 1 ) {
createThreadPool ( ) ;
}
2018-06-15 14:47:04 +00:00
2018-06-16 13:19:49 +00:00
btAlignedObjectArray < b3RayData > rays ;
2018-06-16 19:28:21 +00:00
rays . resize ( totalRays ) ;
if ( numCommandRays )
{
memcpy ( & rays [ 0 ] , & clientCmd . m_requestRaycastIntersections . m_fromToRays [ 0 ] , numCommandRays * sizeof ( b3RayData ) ) ;
}
if ( numStreamingRays )
{
memcpy ( & rays [ numCommandRays ] , bufferServerToClient , numStreamingRays * sizeof ( b3RayData ) ) ;
}
2018-06-16 13:19:49 +00:00
2018-06-19 16:28:48 +00:00
BatchRayCaster batchRayCaster ( m_data - > m_threadPool , m_data - > m_dynamicsWorld , & rays [ 0 ] , ( b3RayHitInfo * ) bufferServerToClient , totalRays ) ;
batchRayCaster . castRays ( numThreads ) ;
2018-06-15 14:47:04 +00:00
2018-06-16 19:28:21 +00:00
serverStatusOut . m_raycastHits . m_numRaycastHits = totalRays ;
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestDebugLinesCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_DEBUG_LINES " ) ;
int curFlags = m_data - > m_remoteDebugDrawer - > getDebugMode ( ) ;
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 ;
}
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 bytesPerLine = ( sizeof ( float ) * 9 ) ;
int maxNumLines = bufferSizeInBytes / bytesPerLine - 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 ( ) ;
}
int numLines = btMin ( maxNumLines , m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) - startingLineIndex ) ;
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 ( ) ;
}
}
serverStatusOut . m_type = CMD_DEBUG_LINES_COMPLETED ;
serverStatusOut . m_numDataStreamBytes = numLines * bytesPerLine ;
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 ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processSyncBodyInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SYNC_BODY_INFO " ) ;
b3AlignedObjectArray < int > usedHandles ;
m_data - > m_bodyHandles . getUsedHandles ( usedHandles ) ;
int actualNumBodies = 0 ;
for ( int i = 0 ; i < usedHandles . size ( ) ; i + + )
{
int usedHandle = usedHandles [ i ] ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( usedHandle ) ;
if ( body & & ( body - > m_multiBody | | body - > m_rigidBody ) )
{
serverStatusOut . m_sdfLoadedArgs . m_bodyUniqueIds [ actualNumBodies + + ] = usedHandle ;
}
}
serverStatusOut . m_sdfLoadedArgs . m_numBodies = actualNumBodies ;
int usz = m_data - > m_userConstraints . size ( ) ;
serverStatusOut . m_sdfLoadedArgs . m_numUserConstraints = usz ;
for ( int i = 0 ; i < usz ; i + + )
{
int key = m_data - > m_userConstraints . getKeyAtIndex ( i ) . getUid1 ( ) ;
// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId;
serverStatusOut . m_sdfLoadedArgs . m_userConstraintUniqueIds [ i ] = key ;
}
serverStatusOut . m_type = CMD_SYNC_BODY_INFO_COMPLETED ;
return hasStatus ;
}
2018-06-02 20:40:08 +00:00
bool PhysicsServerCommandProcessor : : processSyncUserDataCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SYNC_USER_DATA " ) ;
2018-07-03 15:45:19 +00:00
b3AlignedObjectArray < int > userDataHandles ;
m_data - > m_userDataHandles . getUsedHandles ( userDataHandles ) ;
2018-07-25 17:59:29 +00:00
if ( userDataHandles . size ( ) )
{
memcpy ( bufferServerToClient , & userDataHandles [ 0 ] , sizeof ( int ) * userDataHandles . size ( ) ) ;
}
2018-07-03 15:45:19 +00:00
serverStatusOut . m_syncUserDataArgs . m_numUserDataIdentifiers = userDataHandles . size ( ) ;
2018-06-02 20:40:08 +00:00
serverStatusOut . m_type = CMD_SYNC_USER_DATA_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestUserDataCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_USER_DATA " ) ;
serverStatusOut . m_type = CMD_REQUEST_USER_DATA_FAILED ;
2018-07-03 15:45:19 +00:00
SharedMemoryUserData * userData = m_data - > m_userDataHandles . getHandle ( clientCmd . m_userDataRequestArgs . m_userDataId ) ;
2018-06-02 20:40:08 +00:00
if ( ! userData ) {
return hasStatus ;
}
2018-07-03 15:45:19 +00:00
btAssert ( bufferSizeInBytes > = userData - > m_bytes . size ( ) ) ;
serverStatusOut . m_userDataResponseArgs . m_userDataId = clientCmd . m_userDataRequestArgs . m_userDataId ;
serverStatusOut . m_userDataResponseArgs . m_bodyUniqueId = userData - > m_bodyUniqueId ;
serverStatusOut . m_userDataResponseArgs . m_linkIndex = userData - > m_linkIndex ;
serverStatusOut . m_userDataResponseArgs . m_visualShapeIndex = userData - > m_visualShapeIndex ;
2018-06-02 20:40:08 +00:00
serverStatusOut . m_userDataResponseArgs . m_valueType = userData - > m_type ;
serverStatusOut . m_userDataResponseArgs . m_valueLength = userData - > m_bytes . size ( ) ;
serverStatusOut . m_type = CMD_REQUEST_USER_DATA_COMPLETED ;
strcpy ( serverStatusOut . m_userDataResponseArgs . m_key , userData - > m_key . c_str ( ) ) ;
if ( userData - > m_bytes . size ( ) )
{
memcpy ( bufferServerToClient , & userData - > m_bytes [ 0 ] , userData - > m_bytes . size ( ) ) ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processAddUserDataCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_ADD_USER_DATA " ) ;
serverStatusOut . m_type = CMD_ADD_USER_DATA_FAILED ;
if ( clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId < 0 | | clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId > = m_data - > m_bodyHandles . getNumHandles ( ) )
{
return hasStatus ;
}
2018-07-03 15:45:19 +00:00
2018-06-02 20:40:08 +00:00
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId ) ;
if ( ! body ) {
return hasStatus ;
}
2018-07-03 15:45:19 +00:00
SharedMemoryUserDataHashKey userDataIdentifier (
clientCmd . m_addUserDataRequestArgs . m_key ,
clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId ,
clientCmd . m_addUserDataRequestArgs . m_linkIndex ,
clientCmd . m_addUserDataRequestArgs . m_visualShapeIndex ) ;
int * userDataHandlePtr = m_data - > m_userDataHandleLookup . find ( userDataIdentifier ) ;
int userDataHandle = userDataHandlePtr ? * userDataHandlePtr : m_data - > m_userDataHandles . allocHandle ( ) ;
SharedMemoryUserData * userData = m_data - > m_userDataHandles . getHandle ( userDataHandle ) ;
if ( ! userData ) {
return hasStatus ;
2018-06-02 20:40:08 +00:00
}
2018-07-03 15:45:19 +00:00
if ( ! userDataHandlePtr ) {
userData - > m_key = clientCmd . m_addUserDataRequestArgs . m_key ;
userData - > m_bodyUniqueId = clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId ;
userData - > m_linkIndex = clientCmd . m_addUserDataRequestArgs . m_linkIndex ;
userData - > m_visualShapeIndex = clientCmd . m_addUserDataRequestArgs . m_visualShapeIndex ;
m_data - > m_userDataHandleLookup . insert ( userDataIdentifier , userDataHandle ) ;
body - > m_userDataHandles . push_back ( userDataHandle ) ;
}
userData - > replaceValue ( bufferServerToClient ,
clientCmd . m_addUserDataRequestArgs . m_valueLength ,
2018-06-02 20:40:08 +00:00
clientCmd . m_addUserDataRequestArgs . m_valueType ) ;
serverStatusOut . m_type = CMD_ADD_USER_DATA_COMPLETED ;
2018-07-03 15:45:19 +00:00
serverStatusOut . m_userDataResponseArgs . m_userDataId = userDataHandle ;
serverStatusOut . m_userDataResponseArgs . m_bodyUniqueId = clientCmd . m_addUserDataRequestArgs . m_bodyUniqueId ;
serverStatusOut . m_userDataResponseArgs . m_linkIndex = clientCmd . m_addUserDataRequestArgs . m_linkIndex ;
serverStatusOut . m_userDataResponseArgs . m_visualShapeIndex = clientCmd . m_addUserDataRequestArgs . m_visualShapeIndex ;
2018-06-02 20:40:08 +00:00
serverStatusOut . m_userDataResponseArgs . m_valueLength = clientCmd . m_addUserDataRequestArgs . m_valueLength ;
serverStatusOut . m_userDataResponseArgs . m_valueType = clientCmd . m_addUserDataRequestArgs . m_valueType ;
strcpy ( serverStatusOut . m_userDataResponseArgs . m_key , clientCmd . m_addUserDataRequestArgs . m_key ) ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = USER_DATA_ADDED ;
notification . m_userDataArgs . m_userDataId = userDataHandle ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-06-02 20:40:08 +00:00
// Keep bufferServerToClient as-is.
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRemoveUserDataCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REMOVE_USER_DATA " ) ;
serverStatusOut . m_type = CMD_REMOVE_USER_DATA_FAILED ;
2018-07-03 15:45:19 +00:00
SharedMemoryUserData * userData = m_data - > m_userDataHandles . getHandle ( clientCmd . m_removeUserDataRequestArgs . m_userDataId ) ;
if ( ! userData ) {
2018-06-02 20:40:08 +00:00
return hasStatus ;
}
2018-07-03 15:45:19 +00:00
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( userData - > m_bodyUniqueId ) ;
if ( ! body ) {
2018-06-02 20:40:08 +00:00
return hasStatus ;
}
2018-07-03 15:45:19 +00:00
body - > m_userDataHandles . remove ( clientCmd . m_removeUserDataRequestArgs . m_userDataId ) ;
m_data - > m_userDataHandleLookup . remove ( SharedMemoryUserDataHashKey ( userData ) ) ;
m_data - > m_userDataHandles . freeHandle ( clientCmd . m_removeUserDataRequestArgs . m_userDataId ) ;
2018-06-02 20:40:08 +00:00
serverStatusOut . m_removeUserDataResponseArgs = clientCmd . m_removeUserDataRequestArgs ;
serverStatusOut . m_type = CMD_REMOVE_USER_DATA_COMPLETED ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = USER_DATA_REMOVED ;
notification . m_userDataArgs . m_userDataId = clientCmd . m_removeUserDataRequestArgs . m_userDataId ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-06-02 20:40:08 +00:00
return hasStatus ;
}
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processSendDesiredStateCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SEND_DESIRED_STATE " ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_SEND_DESIRED_STATE " ) ;
}
int bodyUniqueId = clientCmd . m_sendDesiredStateCommandArgument . m_bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btAssert ( mb ) ;
switch ( clientCmd . m_sendDesiredStateCommandArgument . m_controlMode )
{
2018-06-16 00:59:26 +00:00
case CONTROL_MODE_PD :
2018-06-05 01:41:41 +00:00
{
2018-06-16 00:59:26 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_PD " ) ;
}
2018-06-05 01:41:41 +00:00
b3PluginArguments args ;
2018-06-16 00:59:26 +00:00
args . m_ints [ 1 ] = bodyUniqueId ;
//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 ) )
{
bool hasDesiredPosOrVel = false ;
btScalar desiredVelocity = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
hasDesiredPosOrVel = true ;
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ velIndex ] ;
args . m_floats [ 2 ] = 0.1 ; // kd
}
btScalar desiredPosition = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ posIndex ] & SIM_DESIRED_STATE_HAS_Q ) ! = 0 )
{
hasDesiredPosOrVel = true ;
desiredPosition = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] ;
args . m_floats [ 3 ] = 0.1 ; // kp
}
if ( hasDesiredPosOrVel )
{
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KP ) ! = 0 )
{
args . m_floats [ 3 ] = clientCmd . m_sendDesiredStateCommandArgument . m_Kp [ velIndex ] ;
}
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
args . m_floats [ 2 ] = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ velIndex ] ;
}
args . m_floats [ 1 ] = desiredVelocity ;
//clamp position
if ( mb - > getLink ( link ) . m_jointLowerLimit < = mb - > getLink ( link ) . m_jointUpperLimit )
{
btClamp ( desiredPosition , mb - > getLink ( link ) . m_jointLowerLimit , mb - > getLink ( link ) . m_jointUpperLimit ) ;
}
args . m_floats [ 0 ] = desiredPosition ;
btScalar maxImp = 1000000.f ;
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ velIndex ] ;
args . m_floats [ 4 ] = maxImp ;
args . m_ints [ 2 ] = link ;
args . m_numInts = 3 ;
args . m_numFloats = 5 ;
2018-06-25 00:06:16 +00:00
args . m_ints [ 0 ] = eSetPDControl ;
if ( args . m_floats [ 4 ] < B3_EPSILON ) {
args . m_ints [ 0 ] = eRemovePDControl ;
}
2018-06-16 00:59:26 +00:00
m_data - > m_pluginManager . executePluginCommand ( m_data - > m_pdControlPlugin , & args ) ;
}
}
velIndex + = mb - > getLink ( link ) . m_dofCount ;
posIndex + = mb - > getLink ( link ) . m_posVarCount ;
}
}
break ;
2018-06-05 01:41:41 +00:00
}
2017-11-18 00:14:18 +00:00
case CONTROL_MODE_TORQUE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_TORQUE " ) ;
}
// mb->clearForcesAndTorques();
int torqueIndex = 6 ;
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
for ( int dof = 0 ; dof < mb - > getLink ( link ) . m_dofCount ; dof + + )
2017-06-30 05:06:27 +00:00
{
2017-11-18 00:14:18 +00:00
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 + + ;
2017-06-30 05:06:27 +00:00
}
2017-06-03 17:57:56 +00:00
}
}
2017-11-18 00:14:18 +00:00
break ;
}
case CONTROL_MODE_VELOCITY :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_VELOCITY " ) ;
}
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
int dofIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom entries of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
if ( supportsJointMotor ( mb , link ) )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2017-10-08 01:50:23 +00:00
2017-11-18 00:14:18 +00:00
if ( motor )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
btScalar desiredVelocity = 0.f ;
bool hasDesiredVelocity = false ;
2017-10-08 01:50:23 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ dofIndex ] ;
btScalar kd = 0.1f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ dofIndex ] ;
2017-10-08 01:50:23 +00:00
}
2017-11-18 00:14:18 +00:00
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
btScalar kp = 0.f ;
motor - > setPositionTarget ( 0 , kp ) ;
hasDesiredVelocity = true ;
}
if ( hasDesiredVelocity )
{
2018-04-11 00:21:14 +00:00
//disable velocity clamp in velocity mode
motor - > setRhsClamp ( SIMD_INFINITY ) ;
2017-11-18 00:14:18 +00:00
btScalar maxImp = 1000000.f * m_data - > m_physicsDeltaTime ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
2017-10-08 01:50:23 +00:00
{
2017-11-18 00:14:18 +00:00
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ dofIndex ] * m_data - > m_physicsDeltaTime ;
2017-10-08 01:50:23 +00:00
}
2017-11-18 00:14:18 +00:00
motor - > setMaxAppliedImpulse ( maxImp ) ;
2017-10-08 01:50:23 +00:00
}
2017-11-18 00:14:18 +00:00
numMotors + + ;
2017-10-08 01:50:23 +00:00
}
}
2017-11-18 00:14:18 +00:00
dofIndex + = mb - > getLink ( link ) . m_dofCount ;
2017-10-08 01:50:23 +00:00
}
2017-11-18 00:14:18 +00:00
}
break ;
}
2017-10-08 01:50:23 +00:00
2017-11-18 00:14:18 +00:00
case CONTROL_MODE_POSITION_VELOCITY_PD :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_POSITION_VELOCITY_PD " ) ;
2017-06-03 17:57:56 +00:00
}
2017-11-18 00:14:18 +00:00
//compute the force base on PD control
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
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 + + )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
if ( supportsJointMotor ( mb , link ) )
2017-06-05 05:04:16 +00:00
{
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
if ( motor )
2017-06-03 17:57:56 +00:00
{
2017-11-22 01:05:28 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_RHS_CLAMP ) ! = 0 )
{
motor - > setRhsClamp ( clientCmd . m_sendDesiredStateCommandArgument . m_rhsClamp [ velIndex ] ) ;
}
2017-11-18 00:14:18 +00:00
bool hasDesiredPosOrVel = false ;
btScalar kp = 0.f ;
btScalar kd = 0.f ;
btScalar desiredVelocity = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
2017-06-03 17:57:56 +00:00
{
2017-11-18 00:14:18 +00:00
hasDesiredPosOrVel = true ;
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ velIndex ] ;
kd = 0.1 ;
2017-06-03 17:57:56 +00:00
}
2017-11-18 00:14:18 +00:00
btScalar desiredPosition = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ posIndex ] & SIM_DESIRED_STATE_HAS_Q ) ! = 0 )
{
hasDesiredPosOrVel = true ;
desiredPosition = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] ;
kp = 0.1 ;
}
if ( hasDesiredPosOrVel )
{
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KP ) ! = 0 )
{
kp = clientCmd . m_sendDesiredStateCommandArgument . m_Kp [ velIndex ] ;
}
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ velIndex ] ;
}
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
2018-01-06 23:00:20 +00:00
//todo: instead of clamping, combine the motor and limit
//and combine handling of limit force and motor force.
2018-03-15 03:47:56 +00:00
2018-01-04 21:14:11 +00:00
//clamp position
if ( mb - > getLink ( link ) . m_jointLowerLimit < = mb - > getLink ( link ) . m_jointUpperLimit )
{
btClamp ( desiredPosition , mb - > getLink ( link ) . m_jointLowerLimit , mb - > getLink ( link ) . m_jointUpperLimit ) ;
}
2017-11-18 00:14:18 +00:00
motor - > setPositionTarget ( desiredPosition , kp ) ;
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 ) ;
}
numMotors + + ;
}
}
velIndex + = mb - > getLink ( link ) . m_dofCount ;
posIndex + = mb - > getLink ( link ) . m_posVarCount ;
2017-06-03 17:57:56 +00:00
}
}
2017-11-18 00:14:18 +00:00
break ;
}
default :
{
b3Warning ( " m_controlMode not implemented yet " ) ;
break ;
}
}
} else
{
//support for non-btMultiBody, such as btRigidBody
if ( body & & body - > m_rigidBody )
{
btRigidBody * rb = body - > m_rigidBody ;
btAssert ( rb ) ;
//switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
//case CONTROL_MODE_TORQUE:
2017-08-28 02:34:00 +00:00
{
2015-11-23 04:50:32 +00:00
if ( m_data - > m_verboseOutput )
{
2017-11-18 00:14:18 +00:00
b3Printf ( " Using CONTROL_MODE_TORQUE " ) ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
// mb->clearForcesAndTorques();
///see addJointInfoFromConstraint
int velIndex = 6 ;
int posIndex = 7 ;
//if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
2017-08-14 21:59:41 +00:00
{
2017-11-18 00:14:18 +00:00
for ( int link = 0 ; link < body - > m_rigidBodyJoints . size ( ) ; link + + )
{
btGeneric6DofSpring2Constraint * con = body - > m_rigidBodyJoints [ link ] ;
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
btVector3 linearLowerLimit ;
btVector3 linearUpperLimit ;
btVector3 angularLowerLimit ;
btVector3 angularUpperLimit ;
2016-06-24 14:31:17 +00:00
2017-06-03 17:57:56 +00:00
2017-11-18 00:14:18 +00:00
//for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
{
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
int torqueIndex = velIndex ;
double torque = 100 ;
bool hasDesiredTorque = false ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
torque = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ velIndex ] ;
hasDesiredTorque = true ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
bool hasDesiredPosOrVel = false ;
btScalar qdotTarget = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
hasDesiredPosOrVel = true ;
qdotTarget = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ velIndex ] ;
}
btScalar qTarget = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ posIndex ] & SIM_DESIRED_STATE_HAS_Q ) ! = 0 )
{
hasDesiredPosOrVel = true ;
qTarget = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
con - > getLinearLowerLimit ( linearLowerLimit ) ;
con - > getLinearUpperLimit ( linearUpperLimit ) ;
con - > getAngularLowerLimit ( angularLowerLimit ) ;
con - > getAngularUpperLimit ( angularUpperLimit ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( linearLowerLimit . isZero ( ) & & linearUpperLimit . isZero ( ) & & angularLowerLimit . isZero ( ) & & angularUpperLimit . isZero ( ) )
2017-05-05 00:51:40 +00:00
{
2017-11-18 00:14:18 +00:00
//fixed, don't do anything
} else
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
con - > calculateTransforms ( ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( linearLowerLimit . isZero ( ) & & linearUpperLimit . isZero ( ) )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
//eRevoluteType;
btVector3 limitRange = angularLowerLimit . absolute ( ) + angularUpperLimit . absolute ( ) ;
int limitAxis = limitRange . maxAxis ( ) ;
const btTransform & transA = con - > getCalculatedTransformA ( ) ;
const btTransform & transB = con - > getCalculatedTransformB ( ) ;
btVector3 axisA = transA . getBasis ( ) . getColumn ( limitAxis ) ;
btVector3 axisB = transB . getBasis ( ) . getColumn ( limitAxis ) ;
switch ( clientCmd . m_sendDesiredStateCommandArgument . m_controlMode )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
case CONTROL_MODE_TORQUE :
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
if ( hasDesiredTorque )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
con - > getRigidBodyA ( ) . applyTorque ( torque * axisA ) ;
con - > getRigidBodyB ( ) . applyTorque ( - torque * axisB ) ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
break ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
case CONTROL_MODE_VELOCITY :
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
if ( hasDesiredPosOrVel )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
con - > enableMotor ( 3 + limitAxis , true ) ;
con - > setTargetVelocity ( 3 + limitAxis , qdotTarget ) ;
2018-03-05 22:05:22 +00:00
con - > setMaxMotorForce ( 3 + limitAxis , torque ) ;
2017-11-18 00:14:18 +00:00
}
break ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
case CONTROL_MODE_POSITION_VELOCITY_PD :
{
if ( hasDesiredPosOrVel )
{
con - > setServo ( 3 + limitAxis , true ) ;
con - > setServoTarget ( 3 + limitAxis , - qTarget ) ;
//next one is the maximum velocity to reach target position.
//the maximum velocity is limited by maxMotorForce
con - > setTargetVelocity ( 3 + limitAxis , 100 ) ;
2018-03-05 22:05:22 +00:00
con - > setMaxMotorForce ( 3 + limitAxis , torque ) ;
2017-11-18 00:14:18 +00:00
con - > enableMotor ( 3 + limitAxis , true ) ;
}
break ;
}
default :
{
}
} ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2017-08-30 02:14:27 +00:00
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
} else
{
//ePrismaticType; @todo
btVector3 limitRange = linearLowerLimit . absolute ( ) + linearUpperLimit . absolute ( ) ;
int limitAxis = limitRange . maxAxis ( ) ;
2017-06-17 01:10:10 +00:00
2017-11-18 00:14:18 +00:00
const btTransform & transA = con - > getCalculatedTransformA ( ) ;
const btTransform & transB = con - > getCalculatedTransformB ( ) ;
btVector3 axisA = transA . getBasis ( ) . getColumn ( limitAxis ) ;
btVector3 axisB = transB . getBasis ( ) . getColumn ( limitAxis ) ;
2017-06-17 01:10:10 +00:00
2017-11-18 00:14:18 +00:00
switch ( clientCmd . m_sendDesiredStateCommandArgument . m_controlMode )
{
case CONTROL_MODE_TORQUE :
{
con - > getRigidBodyA ( ) . applyForce ( - torque * axisA , btVector3 ( 0 , 0 , 0 ) ) ;
con - > getRigidBodyB ( ) . applyForce ( torque * axisB , btVector3 ( 0 , 0 , 0 ) ) ;
break ;
}
case CONTROL_MODE_VELOCITY :
{
con - > enableMotor ( limitAxis , true ) ;
con - > setTargetVelocity ( limitAxis , - qdotTarget ) ;
2018-03-05 22:05:22 +00:00
con - > setMaxMotorForce ( limitAxis , torque ) ;
2017-11-18 00:14:18 +00:00
break ;
}
case CONTROL_MODE_POSITION_VELOCITY_PD :
{
con - > setServo ( limitAxis , true ) ;
con - > setServoTarget ( limitAxis , qTarget ) ;
//next one is the maximum velocity to reach target position.
//the maximum velocity is limited by maxMotorForce
con - > setTargetVelocity ( limitAxis , 100 ) ;
2018-03-05 22:05:22 +00:00
con - > setMaxMotorForce ( limitAxis , torque ) ;
2017-11-18 00:14:18 +00:00
con - > enableMotor ( limitAxis , true ) ;
break ;
}
default :
{
}
} ;
2017-06-17 01:10:10 +00:00
2017-11-18 00:14:18 +00:00
}
}
} //fi
///see addJointInfoFromConstraint
velIndex + + ; //info.m_uIndex
posIndex + + ; //info.m_qIndex
2017-06-17 01:10:10 +00:00
}
}
2017-11-18 00:14:18 +00:00
} //fi
//break;
2017-06-17 01:10:10 +00:00
}
2017-11-18 00:14:18 +00:00
}
} //if (body && body->m_rigidBody)
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED ;
return hasStatus ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestActualStateCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_REQUEST_ACTUAL_STATE " ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Sending the actual state (Q,U) " ) ;
}
int bodyUniqueId = clientCmd . m_requestActualStateInformationCommandArgument . m_bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2018-03-15 03:47:56 +00:00
2017-06-16 02:46:27 +00:00
2017-11-18 00:14:18 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendActualStateArgs . m_bodyUniqueId = bodyUniqueId ;
serverCmd . m_sendActualStateArgs . m_numLinks = body - > m_multiBody - > getNumLinks ( ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
int totalDegreeOfFreedomQ = 0 ;
int totalDegreeOfFreedomU = 0 ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( mb - > getNumLinks ( ) > = MAX_DEGREE_OF_FREEDOM )
{
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_FAILED ;
hasStatus = true ;
return hasStatus ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +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 ( ) ) ;
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 ] ;
2017-03-26 20:06:46 +00:00
2018-03-15 03:47:56 +00:00
2017-09-26 17:05:17 +00:00
2017-11-18 00:14:18 +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
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
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 ] ;
//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
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
btAlignedObjectArray < btVector3 > omega ;
btAlignedObjectArray < btVector3 > linVel ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
bool computeForwardKinematics = ( ( clientCmd . m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS ) ! = 0 ) ;
if ( computeForwardKinematics )
{
B3_PROFILE ( " compForwardKinematics " ) ;
btAlignedObjectArray < btQuaternion > world_to_local ;
btAlignedObjectArray < btVector3 > local_origin ;
world_to_local . resize ( mb - > getNumLinks ( ) + 1 ) ;
local_origin . resize ( mb - > getNumLinks ( ) + 1 ) ;
mb - > forwardKinematics ( world_to_local , local_origin ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
bool computeLinkVelocities = ( ( clientCmd . m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY ) ! = 0 ) ;
if ( computeLinkVelocities )
{
omega . resize ( mb - > getNumLinks ( ) + 1 ) ;
linVel . resize ( mb - > getNumLinks ( ) + 1 ) ;
{
B3_PROFILE ( " compTreeLinkVelocities " ) ;
mb - > compTreeLinkVelocities ( & omega [ 0 ] , & linVel [ 0 ] ) ;
}
}
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
2017-11-18 00:14:18 +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
2017-11-18 00:14:18 +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 ] ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +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
2017-11-18 00:14:18 +00:00
serverCmd . m_sendActualStateArgs . m_jointMotorForce [ l ] = 0 ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( supportsJointMotor ( mb , l ) )
{
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) body - > m_multiBody - > getLink ( l ) . m_userPtr ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( motor & & m_data - > m_physicsDeltaTime > btScalar ( 0 ) )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +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);
//}
}
}
btVector3 linkLocalInertialOrigin = body - > m_linkLocalInertialFrames [ l ] . getOrigin ( ) ;
btQuaternion linkLocalInertialRotation = body - > m_linkLocalInertialFrames [ l ] . getRotation ( ) ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
btVector3 linkCOMOrigin = mb - > getLink ( l ) . m_cachedWorldTransform . getOrigin ( ) ;
btQuaternion linkCOMRotation = mb - > getLink ( l ) . m_cachedWorldTransform . getRotation ( ) ;
2016-09-01 17:45:14 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 0 ] = linkCOMOrigin . getX ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 1 ] = linkCOMOrigin . getY ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 2 ] = linkCOMOrigin . getZ ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 3 ] = linkCOMRotation . x ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 4 ] = linkCOMRotation . y ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 5 ] = linkCOMRotation . z ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 6 ] = linkCOMRotation . w ( ) ;
2016-09-24 18:25:05 +00:00
2018-03-15 03:47:56 +00:00
2016-09-22 15:50:28 +00:00
2017-11-18 00:14:18 +00:00
btVector3 worldLinVel ( 0 , 0 , 0 ) ;
btVector3 worldAngVel ( 0 , 0 , 0 ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( computeLinkVelocities )
{
const btMatrix3x3 & linkRotMat = mb - > getLink ( l ) . m_cachedWorldTransform . getBasis ( ) ;
worldLinVel = linkRotMat * linVel [ l + 1 ] ;
worldAngVel = linkRotMat * omega [ l + 1 ] ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 0 ] = worldLinVel [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 1 ] = worldLinVel [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 2 ] = worldLinVel [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 3 ] = worldAngVel [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 4 ] = worldAngVel [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_linkWorldVelocities [ l * 6 + 5 ] = worldAngVel [ 2 ] ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +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
2017-11-18 00:14:18 +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 ( ) ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
}
2016-11-05 00:06:55 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ ;
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomU = totalDegreeOfFreedomU ;
2017-05-27 01:14:38 +00:00
2017-11-18 00:14:18 +00:00
hasStatus = true ;
2017-10-19 02:15:35 +00:00
2017-11-18 00:14:18 +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 ;
serverCmd . m_sendActualStateArgs . m_numLinks = 0 ;
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
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
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 ] ;
//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 ;
}
}
return hasStatus ;
}
2017-10-19 02:15:35 +00:00
2017-06-05 05:04:16 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestContactpointInformationCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_CONTACT_POINT_INFORMATION " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_sendContactPointArgs . m_numContactPointsCopied = 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
//make a snapshot of the contact manifolds into individual contact points
if ( clientCmd . m_requestContactPointArguments . m_startingContactPointIndex = = 0 )
{
m_data - > m_cachedContactPoints . resize ( 0 ) ;
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS ;
2017-05-27 01:14:38 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE )
{
mode = clientCmd . m_requestContactPointArguments . m_mode ;
}
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
switch ( mode )
{
case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS :
{
int numContactManifolds = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getNumManifolds ( ) ;
m_data - > m_cachedContactPoints . reserve ( numContactManifolds * 4 ) ;
for ( int i = 0 ; i < numContactManifolds ; i + + )
{
const btPersistentManifold * manifold = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getInternalManifoldPointer ( ) [ i ] ;
int linkIndexA = - 1 ;
int linkIndexB = - 1 ;
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
int objectIndexB = - 1 ;
const btRigidBody * bodyB = btRigidBody : : upcast ( manifold - > getBody1 ( ) ) ;
if ( bodyB )
{
objectIndexB = bodyB - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
if ( mblB & & mblB - > m_multiBody )
{
linkIndexB = mblB - > m_link ;
objectIndexB = mblB - > m_multiBody - > getUserIndex2 ( ) ;
}
2017-06-07 15:37:42 +00:00
2017-11-18 00:14:18 +00:00
int objectIndexA = - 1 ;
const btRigidBody * bodyA = btRigidBody : : upcast ( manifold - > getBody0 ( ) ) ;
if ( bodyA )
{
objectIndexA = bodyA - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
if ( mblA & & mblA - > m_multiBody )
{
linkIndexA = mblA - > m_link ;
objectIndexA = mblA - > m_multiBody - > getUserIndex2 ( ) ;
}
btAssert ( bodyA | | mblA ) ;
//apply the filter, if the user provides it
bool swap = false ;
if ( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter > = 0 )
{
if ( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter = = objectIndexA )
{
swap = false ;
}
else if ( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter = = objectIndexB )
{
swap = true ;
}
else
{
continue ;
}
}
if ( swap )
{
std : : swap ( objectIndexA , objectIndexB ) ;
std : : swap ( linkIndexA , linkIndexB ) ;
std : : swap ( bodyA , bodyB ) ;
}
//apply the second object filter, if the user provides it
if ( clientCmd . m_requestContactPointArguments . m_objectBIndexFilter > = 0 )
{
if ( clientCmd . m_requestContactPointArguments . m_objectBIndexFilter ! = objectIndexB )
{
continue ;
}
}
if (
( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER ) & &
clientCmd . m_requestContactPointArguments . m_linkIndexAIndexFilter ! = linkIndexA )
{
continue ;
}
if (
( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER ) & &
clientCmd . m_requestContactPointArguments . m_linkIndexBIndexFilter ! = linkIndexB )
{
continue ;
}
for ( int p = 0 ; p < manifold - > getNumContacts ( ) ; p + + )
{
b3ContactPointData pt ;
pt . m_bodyUniqueIdA = objectIndexA ;
pt . m_bodyUniqueIdB = objectIndexB ;
const btManifoldPoint & srcPt = manifold - > getContactPoint ( p ) ;
pt . m_contactDistance = srcPt . getDistance ( ) ;
pt . m_contactFlags = 0 ;
pt . m_linkIndexA = linkIndexA ;
pt . m_linkIndexB = linkIndexB ;
for ( int j = 0 ; j < 3 ; j + + )
{
pt . m_contactNormalOnBInWS [ j ] = srcPt . m_normalWorldOnB [ j ] ;
pt . m_positionOnAInWS [ j ] = srcPt . getPositionWorldOnA ( ) [ j ] ;
pt . m_positionOnBInWS [ j ] = srcPt . getPositionWorldOnB ( ) [ j ] ;
}
pt . m_normalForce = srcPt . getAppliedImpulse ( ) / m_data - > m_physicsDeltaTime ;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_data - > m_cachedContactPoints . push_back ( pt ) ;
}
}
break ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS :
{
//todo(erwincoumans) compute closest points between all, and vs all, pair
btScalar closestDistanceThreshold = 0.f ;
if ( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD )
{
closestDistanceThreshold = clientCmd . m_requestContactPointArguments . m_closestDistanceThreshold ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int bodyUniqueIdA = clientCmd . m_requestContactPointArguments . m_objectAIndexFilter ;
int bodyUniqueIdB = clientCmd . m_requestContactPointArguments . m_objectBIndexFilter ;
bool hasLinkIndexAFilter = ( 0 ! = ( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER ) ) ;
bool hasLinkIndexBFilter = ( 0 ! = ( clientCmd . m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER ) ) ;
int linkIndexA = clientCmd . m_requestContactPointArguments . m_linkIndexAIndexFilter ;
int linkIndexB = clientCmd . m_requestContactPointArguments . m_linkIndexBIndexFilter ;
btAlignedObjectArray < btCollisionObject * > setA ;
btAlignedObjectArray < btCollisionObject * > setB ;
btAlignedObjectArray < int > setALinkIndex ;
btAlignedObjectArray < int > setBLinkIndex ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( bodyUniqueIdA > = 0 )
{
InternalBodyData * bodyA = m_data - > m_bodyHandles . getHandle ( bodyUniqueIdA ) ;
if ( bodyA )
{
if ( bodyA - > m_multiBody )
2017-05-04 04:47:53 +00:00
{
2017-11-18 00:14:18 +00:00
if ( bodyA - > m_multiBody - > getBaseCollider ( ) )
2017-05-04 04:47:53 +00:00
{
2017-11-18 00:14:18 +00:00
if ( ! hasLinkIndexAFilter | | ( linkIndexA = = - 1 ) )
2017-05-27 01:14:38 +00:00
{
2017-11-18 00:14:18 +00:00
setA . push_back ( bodyA - > m_multiBody - > getBaseCollider ( ) ) ;
setALinkIndex . push_back ( - 1 ) ;
2017-05-27 01:14:38 +00:00
}
2017-11-18 00:14:18 +00:00
}
for ( int i = 0 ; i < bodyA - > m_multiBody - > getNumLinks ( ) ; i + + )
{
if ( bodyA - > m_multiBody - > getLink ( i ) . m_collider )
2017-05-27 01:14:38 +00:00
{
2017-11-18 00:14:18 +00:00
if ( ! hasLinkIndexAFilter | | ( linkIndexA = = i ) )
2017-05-27 01:14:38 +00:00
{
2017-11-18 00:14:18 +00:00
setA . push_back ( bodyA - > m_multiBody - > getLink ( i ) . m_collider ) ;
setALinkIndex . push_back ( i ) ;
2017-05-27 01:14:38 +00:00
}
2017-05-04 04:47:53 +00:00
}
}
}
2017-11-18 00:14:18 +00:00
if ( bodyA - > m_rigidBody )
{
setA . push_back ( bodyA - > m_rigidBody ) ;
setALinkIndex . push_back ( - 1 ) ;
}
}
}
if ( bodyUniqueIdB > = 0 )
{
InternalBodyData * bodyB = m_data - > m_bodyHandles . getHandle ( bodyUniqueIdB ) ;
if ( bodyB )
2017-05-08 05:21:38 +00:00
{
2017-11-18 00:14:18 +00:00
if ( bodyB - > m_multiBody )
2017-05-08 05:21:38 +00:00
{
2017-11-18 00:14:18 +00:00
if ( bodyB - > m_multiBody - > getBaseCollider ( ) )
2017-05-08 05:21:38 +00:00
{
2017-11-18 00:14:18 +00:00
if ( ! hasLinkIndexBFilter | | ( linkIndexB = = - 1 ) )
2017-06-09 01:59:11 +00:00
{
2017-11-18 00:14:18 +00:00
setB . push_back ( bodyB - > m_multiBody - > getBaseCollider ( ) ) ;
setBLinkIndex . push_back ( - 1 ) ;
2017-06-09 01:59:11 +00:00
}
2017-11-18 00:14:18 +00:00
}
for ( int i = 0 ; i < bodyB - > m_multiBody - > getNumLinks ( ) ; i + + )
{
if ( bodyB - > m_multiBody - > getLink ( i ) . m_collider )
2017-06-09 01:59:11 +00:00
{
2017-11-18 00:14:18 +00:00
if ( ! hasLinkIndexBFilter | | ( linkIndexB = = i ) )
{
setB . push_back ( bodyB - > m_multiBody - > getLink ( i ) . m_collider ) ;
setBLinkIndex . push_back ( i ) ;
}
2017-06-09 01:59:11 +00:00
}
2017-05-08 05:21:38 +00:00
}
}
2017-11-18 00:14:18 +00:00
if ( bodyB - > m_rigidBody )
2017-05-08 05:21:38 +00:00
{
2017-11-18 00:14:18 +00:00
setB . push_back ( bodyB - > m_rigidBody ) ;
setBLinkIndex . push_back ( - 1 ) ;
2017-10-05 18:43:14 +00:00
2017-11-18 00:14:18 +00:00
}
2017-10-05 18:43:14 +00:00
}
2017-11-18 00:14:18 +00:00
}
2017-10-05 18:43:14 +00:00
2017-11-18 00:14:18 +00:00
{
///ContactResultCallback is used to report contact points
struct MyContactResultCallback : public btCollisionWorld : : ContactResultCallback
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
int m_bodyUniqueIdA ;
int m_bodyUniqueIdB ;
int m_linkIndexA ;
int m_linkIndexB ;
btScalar m_deltaTime ;
btAlignedObjectArray < b3ContactPointData > & m_cachedContactPoints ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
MyContactResultCallback ( btAlignedObjectArray < b3ContactPointData > & pointCache )
: m_cachedContactPoints ( pointCache )
2015-11-23 04:50:32 +00:00
{
}
2017-11-18 00:14:18 +00:00
virtual ~ MyContactResultCallback ( )
2016-07-18 06:50:11 +00:00
{
}
2017-11-18 00:14:18 +00:00
virtual bool needsCollision ( btBroadphaseProxy * proxy0 ) const
2016-10-23 14:14:50 +00:00
{
2017-11-18 00:14:18 +00:00
//bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
//collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
//return collides;
return true ;
2016-10-23 14:14:50 +00:00
}
2017-11-18 00:14:18 +00:00
virtual btScalar addSingleResult ( btManifoldPoint & cp , const btCollisionObjectWrapper * colObj0Wrap , int partId0 , int index0 , const btCollisionObjectWrapper * colObj1Wrap , int partId1 , int index1 )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
if ( cp . m_distance1 < = m_closestDistanceThreshold )
2015-11-23 04:50:32 +00:00
{
2017-11-18 00:14:18 +00:00
b3ContactPointData pt ;
pt . m_bodyUniqueIdA = m_bodyUniqueIdA ;
pt . m_bodyUniqueIdB = m_bodyUniqueIdB ;
const btManifoldPoint & srcPt = cp ;
pt . m_contactDistance = srcPt . getDistance ( ) ;
pt . m_contactFlags = 0 ;
pt . m_linkIndexA = m_linkIndexA ;
pt . m_linkIndexB = m_linkIndexB ;
for ( int j = 0 ; j < 3 ; j + + )
{
pt . m_contactNormalOnBInWS [ j ] = srcPt . m_normalWorldOnB [ j ] ;
pt . m_positionOnAInWS [ j ] = srcPt . getPositionWorldOnA ( ) [ j ] ;
pt . m_positionOnBInWS [ j ] = srcPt . getPositionWorldOnB ( ) [ j ] ;
}
pt . m_normalForce = srcPt . getAppliedImpulse ( ) / m_deltaTime ;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_cachedContactPoints . push_back ( pt ) ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
return 1 ;
2015-11-23 04:50:32 +00:00
}
2017-11-18 00:14:18 +00:00
} ;
2017-01-16 21:05:26 +00:00
2017-11-18 00:14:18 +00:00
MyContactResultCallback cb ( m_data - > m_cachedContactPoints ) ;
cb . m_bodyUniqueIdA = bodyUniqueIdA ;
cb . m_bodyUniqueIdB = bodyUniqueIdB ;
cb . m_deltaTime = m_data - > m_physicsDeltaTime ;
for ( int i = 0 ; i < setA . size ( ) ; i + + )
{
cb . m_linkIndexA = setALinkIndex [ i ] ;
for ( int j = 0 ; j < setB . size ( ) ; j + + )
2016-12-01 06:24:20 +00:00
{
2017-11-18 00:14:18 +00:00
cb . m_linkIndexB = setBLinkIndex [ j ] ;
cb . m_closestDistanceThreshold = closestDistanceThreshold ;
this - > m_data - > m_dynamicsWorld - > contactPairTest ( setA [ i ] , setB [ j ] , cb ) ;
2016-12-01 06:24:20 +00:00
}
2017-11-18 00:14:18 +00:00
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
break ;
}
default :
{
b3Warning ( " Unknown contact query mode: %d " , mode ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int numContactPoints = m_data - > m_cachedContactPoints . size ( ) ;
2018-03-15 03:47:56 +00:00
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
//b3ContactPoint
//struct b3ContactPointDynamics
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
int totalBytesPerContact = sizeof ( b3ContactPointData ) ;
int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1 ;
2017-06-07 16:37:28 +00:00
2017-11-18 00:14:18 +00:00
b3ContactPointData * contactData = ( b3ContactPointData * ) bufferServerToClient ;
2017-05-27 01:14:38 +00:00
2017-11-18 00:14:18 +00:00
int startContactPointIndex = clientCmd . m_requestContactPointArguments . m_startingContactPointIndex ;
int numContactPointBatch = btMin ( numContactPoints , contactPointStorage ) ;
2016-09-08 22:15:58 +00:00
2017-11-18 00:14:18 +00:00
int endContactPointIndex = startContactPointIndex + numContactPointBatch ;
2017-05-18 02:29:12 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = startContactPointIndex ; i < endContactPointIndex ; i + + )
{
const b3ContactPointData & srcPt = m_data - > m_cachedContactPoints [ i ] ;
b3ContactPointData & destPt = contactData [ serverCmd . m_sendContactPointArgs . m_numContactPointsCopied ] ;
destPt = srcPt ;
serverCmd . m_sendContactPointArgs . m_numContactPointsCopied + + ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendContactPointArgs . m_startingContactPointIndex = clientCmd . m_requestContactPointArguments . m_startingContactPointIndex ;
serverCmd . m_sendContactPointArgs . m_numRemainingContactPoints = numContactPoints - clientCmd . m_requestContactPointArguments . m_startingContactPointIndex - serverCmd . m_sendContactPointArgs . m_numContactPointsCopied ;
serverCmd . m_numDataStreamBytes = totalBytesPerContact * serverCmd . m_sendContactPointArgs . m_numContactPointsCopied ;
serverCmd . m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED ; //CMD_CONTACT_POINT_INFORMATION_FAILED,
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2017-05-18 02:29:12 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestBodyInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_BODY_INFO " ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
const SdfRequestInfoArgs & sdfInfoArgs = clientCmd . m_sdfRequestInfoArgs ;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream ( sdfInfoArgs . m_bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_BODY_INFO_COMPLETED ;
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = sdfInfoArgs . m_bodyUniqueId ;
serverStatusOut . m_dataStreamArguments . m_bodyName [ 0 ] = 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( sdfInfoArgs . m_bodyUniqueId ) ;
if ( bodyHandle )
{
strcpy ( serverStatusOut . m_dataStreamArguments . m_bodyName , bodyHandle - > m_bodyName . c_str ( ) ) ;
}
2016-11-28 23:11:34 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_numDataStreamBytes = streamSizeInBytes ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
2016-11-28 23:11:34 +00:00
2017-11-18 00:14:18 +00:00
}
2016-11-28 23:11:34 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processLoadSDFCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_LOAD_SDF " ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
const SdfArgs & sdfArgs = clientCmd . m_sdfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_SDF:%s " , sdfArgs . m_sdfFileName ) ;
}
bool useMultiBody = ( clientCmd . m_updateFlags & URDF_ARGS_USE_MULTIBODY ) ? ( sdfArgs . m_useMultiBody ! = 0 ) : true ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
int flags = CUF_USE_SDF ; //CUF_USE_URDF_INERTIA
btScalar globalScaling = 1.f ;
if ( clientCmd . m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING )
{
globalScaling = sdfArgs . m_globalScaling ;
}
bool completedOk = loadSdf ( sdfArgs . m_sdfFileName , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags , globalScaling ) ;
if ( completedOk )
{
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2016-09-01 17:45:14 +00:00
2017-11-18 00:14:18 +00:00
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut . m_sdfLoadedArgs . m_numBodies = m_data - > m_sdfRecentLoadedBodies . size ( ) ;
serverStatusOut . m_sdfLoadedArgs . m_numUserConstraints = 0 ;
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 ] ;
}
2017-03-23 17:29:16 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_SDF_LOADING_COMPLETED ;
} else
{
serverStatusOut . m_type = CMD_SDF_LOADING_FAILED ;
}
return hasStatus ;
}
2017-03-23 17:29:16 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCreateMultiBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2016-11-06 23:02:07 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_CREATE_MULTI_BODY_FAILED ;
if ( clientCmd . m_createMultiBodyArgs . m_baseLinkIndex > = 0 )
{
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
ProgrammaticUrdfInterface u2b ( clientCmd . m_createMultiBodyArgs , m_data ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
bool useMultiBody = true ;
if ( clientCmd . m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES )
{
useMultiBody = false ;
2018-03-15 03:47:56 +00:00
}
2017-11-18 00:14:18 +00:00
int flags = 0 ;
2018-06-02 18:37:14 +00:00
if ( clientCmd . m_updateFlags & MULT_BODY_HAS_FLAGS )
{
flags = clientCmd . m_createMultiBodyArgs . m_flags ;
}
2017-11-18 00:14:18 +00:00
bool ok = processImportedObjects ( " memory " , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags , u2b ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( ok )
{
int bodyUniqueId = - 1 ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( m_data - > m_sdfRecentLoadedBodies . size ( ) = = 1 )
{
bodyUniqueId = m_data - > m_sdfRecentLoadedBodies [ 0 ] ;
}
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
if ( bodyUniqueId > = 0 )
{
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
serverStatusOut . m_type = CMD_CREATE_MULTI_BODY_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int streamSizeInBytes = createBodyInfoStream ( bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2018-06-05 22:59:01 +00:00
serverStatusOut . m_numDataStreamBytes = streamSizeInBytes ;
2017-11-18 00:14:18 +00:00
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
strcpy ( serverStatusOut . m_dataStreamArguments . m_bodyName , body - > m_bodyName . c_str ( ) ) ;
}
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
//ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
}
return hasStatus ;
}
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processLoadURDFCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
serverStatusOut . m_type = CMD_URDF_LOADING_FAILED ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_LOAD_URDF " ) ;
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 ] ;
}
int urdfFlags = 0 ;
if ( clientCmd . m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS )
{
urdfFlags = urdfArgs . m_urdfFlags ;
}
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 ! = 0 ) : true ;
bool useFixedBase = ( clientCmd . m_updateFlags & URDF_ARGS_USE_FIXED_BASE ) ? ( urdfArgs . m_useFixedBase ! = 0 ) : false ;
int bodyUniqueId ;
btScalar globalScaling = 1.f ;
if ( clientCmd . m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING )
{
globalScaling = urdfArgs . m_globalScaling ;
}
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf ( urdfArgs . m_urdfFileName ,
initialPos , initialOrn ,
useMultiBody , useFixedBase , & bodyUniqueId , bufferServerToClient , bufferSizeInBytes , urdfFlags , globalScaling ) ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
if ( completedOk & & bodyUniqueId > = 0 )
{
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_URDF_LOADING_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int streamSizeInBytes = createBodyInfoStream ( bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2018-06-05 22:59:01 +00:00
serverStatusOut . m_numDataStreamBytes = streamSizeInBytes ;
2016-06-24 14:31:17 +00:00
2018-06-05 22:59:01 +00:00
# ifdef ENABLE_LINK_MAPPER
2017-11-18 00:14:18 +00:00
if ( m_data - > m_urdfLinkNameMapper . size ( ) )
{
serverStatusOut . m_numDataStreamBytes = m_data - > m_urdfLinkNameMapper . at ( m_data - > m_urdfLinkNameMapper . size ( ) - 1 ) - > m_memSerializer - > getCurrentBufferSize ( ) ;
}
2018-06-05 22:59:01 +00:00
# endif
2017-11-18 00:14:18 +00:00
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
strcpy ( serverStatusOut . m_dataStreamArguments . m_bodyName , body - > m_bodyName . c_str ( ) ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2016-06-24 14:31:17 +00:00
2018-01-08 03:24:37 +00:00
bool PhysicsServerCommandProcessor : : processLoadSoftBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
2017-11-18 00:14:18 +00:00
{
2018-01-10 06:47:56 +00:00
serverStatusOut . m_type = CMD_LOAD_SOFT_BODY_FAILED ;
2017-11-18 00:14:18 +00:00
bool hasStatus = true ;
2018-01-08 03:06:09 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2017-11-18 00:14:18 +00:00
double scale = 0.1 ;
double mass = 0.1 ;
double collisionMargin = 0.02 ;
2018-01-08 03:56:46 +00:00
const LoadSoftBodyArgs & loadSoftBodyArgs = clientCmd . m_loadSoftBodyArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_SOFT_BODY:%s " , loadSoftBodyArgs . m_fileName ) ;
}
btAssert ( ( clientCmd . m_updateFlags & LOAD_SOFT_BODY_FILE_NAME ) ! = 0 ) ;
btAssert ( loadSoftBodyArgs . m_fileName ) ;
2018-01-08 03:24:37 +00:00
if ( clientCmd . m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE )
2017-11-18 00:14:18 +00:00
{
2018-01-08 03:24:37 +00:00
scale = clientCmd . m_loadSoftBodyArguments . m_scale ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 03:24:37 +00:00
if ( clientCmd . m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS )
2017-11-18 00:14:18 +00:00
{
2018-01-08 03:24:37 +00:00
mass = clientCmd . m_loadSoftBodyArguments . m_mass ;
2017-11-18 00:14:18 +00:00
}
2018-01-08 03:24:37 +00:00
if ( clientCmd . m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN )
2017-11-18 00:14:18 +00:00
{
2018-01-08 03:24:37 +00:00
collisionMargin = clientCmd . m_loadSoftBodyArguments . m_collisionMargin ;
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2018-01-08 03:06:09 +00:00
{
char relativeFileName [ 1024 ] ;
char pathPrefix [ 1024 ] ;
pathPrefix [ 0 ] = 0 ;
2018-01-08 03:56:46 +00:00
if ( b3ResourcePath : : findResourcePath ( loadSoftBodyArgs . m_fileName , relativeFileName , 1024 ) )
2018-01-08 03:06:09 +00:00
{
b3FileUtils : : extractPath ( relativeFileName , pathPrefix , 1024 ) ;
}
const std : : string & error_message_prefix = " " ;
std : : string out_found_filename ;
int out_type ;
bool foundFile = findExistingMeshFile ( pathPrefix , relativeFileName , error_message_prefix , & out_found_filename , & out_type ) ;
std : : vector < tinyobj : : shape_t > shapes ;
std : : string err = tinyobj : : LoadObj ( shapes , out_found_filename . c_str ( ) ) ;
if ( shapes . size ( ) > 0 )
{
const tinyobj : : shape_t & shape = shapes [ 0 ] ;
btAlignedObjectArray < btScalar > vertices ;
btAlignedObjectArray < int > indices ;
for ( int i = 0 ; i < shape . mesh . positions . size ( ) ; i + + )
{
vertices . push_back ( shape . mesh . positions [ i ] ) ;
}
for ( int i = 0 ; i < shape . mesh . indices . size ( ) ; i + + )
{
indices . push_back ( shape . mesh . indices [ i ] ) ;
}
int numTris = indices . size ( ) / 3 ;
if ( numTris > 0 )
{
2018-02-18 19:09:42 +00:00
btSoftBody * psb = btSoftBodyHelpers : : CreateFromTriMesh ( m_data - > m_dynamicsWorld - > getWorldInfo ( ) , & vertices [ 0 ] , & indices [ 0 ] , numTris ) ;
2018-01-08 03:06:09 +00:00
btSoftBody : : Material * pm = psb - > appendMaterial ( ) ;
pm - > m_kLST = 0.5 ;
pm - > m_flags - = btSoftBody : : fMaterial : : DebugDraw ;
psb - > generateBendingConstraints ( 2 , pm ) ;
psb - > m_cfg . piterations = 20 ;
psb - > m_cfg . kDF = 0.5 ;
psb - > randomizeConstraints ( ) ;
psb - > rotate ( btQuaternion ( 0.70711 , 0 , 0 , 0.70711 ) ) ;
psb - > translate ( btVector3 ( - 0.05 , 0 , 1.0 ) ) ;
psb - > scale ( btVector3 ( scale , scale , scale ) ) ;
2018-03-15 03:47:56 +00:00
2018-01-08 03:06:09 +00:00
psb - > setTotalMass ( mass , true ) ;
psb - > getCollisionShape ( ) - > setMargin ( collisionMargin ) ;
2018-02-12 11:26:39 +00:00
psb - > getCollisionShape ( ) - > setUserPointer ( psb ) ;
2018-01-08 03:06:09 +00:00
m_data - > m_dynamicsWorld - > addSoftBody ( psb ) ;
2018-02-08 13:31:40 +00:00
m_data - > m_guiHelper - > createCollisionShapeGraphicsObject ( psb - > getCollisionShape ( ) ) ;
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2018-01-08 03:06:09 +00:00
int bodyUniqueId = m_data - > m_bodyHandles . allocHandle ( ) ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
bodyHandle - > m_softBody = psb ;
2018-01-08 03:24:37 +00:00
serverStatusOut . m_loadSoftBodyResultArguments . m_objectUniqueId = bodyUniqueId ;
2018-01-10 06:47:56 +00:00
serverStatusOut . m_type = CMD_LOAD_SOFT_BODY_COMPLETED ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = BODY_ADDED ;
notification . m_bodyArgs . m_bodyUniqueId = bodyUniqueId ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-01-08 03:06:09 +00:00
}
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
# endif
return hasStatus ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCreateSensorCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CREATE_SENSOR " ) ;
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_CREATE_SENSOR " ) ;
}
int bodyUniqueId = clientCmd . m_createSensorArguments . m_bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . 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 ) ;
} ;
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +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 ) ;
} ;
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
}
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
} else
{
b3Warning ( " No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet " ) ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
#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 ) ;
2015-11-23 04:50:32 +00:00
2016-11-10 19:22:22 +00:00
2017-11-18 00:14:18 +00:00
}
*/
# endif
2016-11-10 19:22:22 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
2016-11-10 19:22:22 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processProfileTimingCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
B3_PROFILE ( " custom " ) ; //clientCmd.m_profile.m_name);
{
B3_PROFILE ( " event " ) ; //clientCmd.m_profile.m_name);
char * * eventNamePtr = m_data - > m_profileEvents [ clientCmd . m_profile . m_name ] ;
char * eventName = 0 ;
if ( eventNamePtr )
{
B3_PROFILE ( " reuse " ) ;
eventName = * eventNamePtr ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
} else
{
B3_PROFILE ( " alloc " ) ;
int len = strlen ( clientCmd . m_profile . m_name ) ;
eventName = new char [ len + 1 ] ;
strcpy ( eventName , clientCmd . m_profile . m_name ) ;
eventName [ len ] = 0 ;
m_data - > m_profileEvents . insert ( eventName , eventName ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
2016-11-10 19:22:22 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
{
{
B3_PROFILE ( " with " ) ; //clientCmd.m_profile.m_name);
{
B3_PROFILE ( " some " ) ; //clientCmd.m_profile.m_name);
2016-11-10 19:22:22 +00:00
{
2017-11-18 00:14:18 +00:00
B3_PROFILE ( " deep " ) ; //clientCmd.m_profile.m_name);
2016-11-10 19:22:22 +00:00
{
2017-11-18 00:14:18 +00:00
B3_PROFILE ( " level " ) ; //clientCmd.m_profile.m_name);
{
B3_PROFILE ( eventName ) ;
b3Clock : : usleep ( clientCmd . m_profile . m_durationInMicroSeconds ) ;
}
2016-11-10 19:22:22 +00:00
}
}
2017-04-10 18:03:41 +00:00
}
2017-11-18 00:14:18 +00:00
}
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
return hasStatus ;
}
2017-04-10 18:03:41 +00:00
2017-10-06 20:46:24 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestCollisionInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverStatusOut . m_type = CMD_REQUEST_COLLISION_INFO_FAILED ;
hasStatus = true ;
int bodyUniqueId = clientCmd . m_requestCollisionInfoArgs . m_bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2018-03-15 03:47:56 +00:00
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
serverStatusOut . m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED ;
serverCmd . m_sendCollisionInfoArgs . m_numLinks = body - > m_multiBody - > getNumLinks ( ) ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 0 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 1 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 2 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 0 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 1 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 2 ] = - 1 ;
if ( body - > m_multiBody - > getBaseCollider ( ) )
{
btTransform tr ;
tr . setOrigin ( mb - > getBasePos ( ) ) ;
tr . setRotation ( mb - > getWorldToBaseRot ( ) . inverse ( ) ) ;
btVector3 aabbMin , aabbMax ;
body - > m_multiBody - > getBaseCollider ( ) - > getCollisionShape ( ) - > getAabb ( tr , aabbMin , aabbMax ) ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 0 ] = aabbMin [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 1 ] = aabbMin [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 2 ] = aabbMin [ 2 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 0 ] = aabbMax [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 1 ] = aabbMax [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 2 ] = aabbMax [ 2 ] ;
}
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + )
{
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 0 ] = 0 ;
2018-03-15 03:47:56 +00:00
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 1 ] = 0 ;
2017-11-18 00:14:18 +00:00
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 2 ] = 0 ;
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 0 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 1 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 2 ] = - 1 ;
2016-11-10 05:01:04 +00:00
2018-03-15 03:47:56 +00:00
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
if ( body - > m_multiBody - > getLink ( l ) . m_collider )
{
btVector3 aabbMin , aabbMax ;
body - > m_multiBody - > getLinkCollider ( l ) - > getCollisionShape ( ) - > getAabb ( mb - > getLink ( l ) . m_cachedWorldTransform , aabbMin , aabbMax ) ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 0 ] = aabbMin [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 1 ] = aabbMin [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMin [ 3 * l + 2 ] = aabbMin [ 2 ] ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 0 ] = aabbMax [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 1 ] = aabbMax [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_linkWorldAABBsMax [ 3 * l + 2 ] = aabbMax [ 2 ] ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
else
{
if ( body & & body - > m_rigidBody )
{
btRigidBody * rb = body - > m_rigidBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverStatusOut . m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED ;
serverCmd . m_sendCollisionInfoArgs . m_numLinks = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 0 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 1 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 2 ] = 0 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 0 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 1 ] = - 1 ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 2 ] = - 1 ;
if ( rb - > getCollisionShape ( ) )
{
btTransform tr = rb - > getWorldTransform ( ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btVector3 aabbMin , aabbMax ;
rb - > getCollisionShape ( ) - > getAabb ( tr , aabbMin , aabbMax ) ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 0 ] = aabbMin [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 1 ] = aabbMin [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMin [ 2 ] = aabbMin [ 2 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 0 ] = aabbMax [ 0 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 1 ] = aabbMax [ 1 ] ;
serverCmd . m_sendCollisionInfoArgs . m_rootWorldAABBMax [ 2 ] = aabbMax [ 2 ] ;
}
}
}
return hasStatus ;
}
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processForwardDynamicsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_STEP_FORWARD_SIMULATION " ) ;
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Step simulation request " ) ;
b3Printf ( " CMD_STEP_FORWARD_SIMULATION clientCmd = %d \n " , clientCmd . m_sequenceNumber ) ;
}
///todo(erwincoumans) move this damping inside Bullet
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getNumMultibodies ( ) ; i + + )
{
btMultiBody * mb = m_data - > m_dynamicsWorld - > getMultiBody ( i ) ;
2018-03-15 03:47:56 +00:00
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + )
2017-11-18 00:14:18 +00:00
{
2018-03-15 03:47:56 +00:00
for ( int d = 0 ; d < mb - > getLink ( l ) . m_dofCount ; d + + )
2017-11-18 00:14:18 +00:00
{
double damping_coefficient = mb - > getLink ( l ) . m_jointDamping ;
double damping = - damping_coefficient * mb - > getJointVelMultiDof ( l ) [ d ] ;
mb - > addJointTorqueMultiDof ( l , d , damping ) ;
}
}
}
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
btScalar deltaTimeScaled = m_data - > m_physicsDeltaTime * simTimeScalingFactor ;
2016-11-10 05:01:04 +00:00
2018-07-24 12:12:16 +00:00
int numSteps = 0 ;
2017-11-18 00:14:18 +00:00
if ( m_data - > m_numSimulationSubSteps > 0 )
{
2018-07-24 12:12:16 +00:00
numSteps = m_data - > m_dynamicsWorld - > stepSimulation ( deltaTimeScaled , m_data - > m_numSimulationSubSteps , m_data - > m_physicsDeltaTime / m_data - > m_numSimulationSubSteps ) ;
2017-11-18 00:14:18 +00:00
}
else
{
2018-07-24 12:12:16 +00:00
numSteps = m_data - > m_dynamicsWorld - > stepSimulation ( deltaTimeScaled , 0 ) ;
}
if ( numSteps > 0 )
{
addTransformChangedNotifications ( ) ;
2017-11-18 00:14:18 +00:00
}
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED ;
2018-07-24 12:12:16 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
2017-01-24 00:45:18 +00:00
2017-11-18 00:14:18 +00:00
}
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestInternalDataCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_INTERNAL_DATA " ) ;
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
//todo: also check version etc?
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_REQUEST_INTERNAL_DATA_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int sz = btDefaultSerializer : : getMemoryDnaSizeInBytes ( ) ;
const char * memDna = btDefaultSerializer : : getMemoryDna ( ) ;
if ( sz < bufferSizeInBytes )
{
for ( int i = 0 ; i < sz ; i + + )
{
bufferServerToClient [ i ] = memDna [ i ] ;
}
serverCmd . m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED ;
serverCmd . m_numDataStreamBytes = sz ;
}
return hasStatus ;
}
2016-11-10 05:01:04 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processChangeDynamicsInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CHANGE_DYNAMICS_INFO " ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int bodyUniqueId = clientCmd . m_changeDynamicsInfoArgs . m_bodyUniqueId ;
int linkIndex = clientCmd . m_changeDynamicsInfoArgs . m_linkIndex ;
double mass = clientCmd . m_changeDynamicsInfoArgs . m_mass ;
double lateralFriction = clientCmd . m_changeDynamicsInfoArgs . m_lateralFriction ;
double spinningFriction = clientCmd . m_changeDynamicsInfoArgs . m_spinningFriction ;
double rollingFriction = clientCmd . m_changeDynamicsInfoArgs . m_rollingFriction ;
double restitution = clientCmd . m_changeDynamicsInfoArgs . m_restitution ;
2017-12-21 00:56:31 +00:00
btVector3 newLocalInertiaDiagonal ( clientCmd . m_changeDynamicsInfoArgs . m_localInertiaDiagonal [ 0 ] ,
clientCmd . m_changeDynamicsInfoArgs . m_localInertiaDiagonal [ 1 ] ,
clientCmd . m_changeDynamicsInfoArgs . m_localInertiaDiagonal [ 2 ] ) ;
2017-11-18 00:14:18 +00:00
btAssert ( bodyUniqueId > = 0 ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
2016-09-01 20:30:07 +00:00
2018-06-16 04:26:26 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE )
{
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateWakeUp )
{
mb - > wakeUp ( ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateSleep )
{
mb - > goToSleep ( ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateEnableSleeping )
{
mb - > setCanSleep ( true ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateDisableSleeping )
{
mb - > setCanSleep ( false ) ;
}
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING )
{
mb - > setLinearDamping ( clientCmd . m_changeDynamicsInfoArgs . m_linearDamping ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING )
{
mb - > setAngularDamping ( clientCmd . m_changeDynamicsInfoArgs . m_angularDamping ) ;
}
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
if ( linkIndex = = - 1 )
{
if ( mb - > getBaseCollider ( ) )
{
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION )
{
mb - > getBaseCollider ( ) - > setRestitution ( restitution ) ;
}
2018-03-15 03:47:56 +00:00
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING )
{
mb - > getBaseCollider ( ) - > setContactStiffnessAndDamping ( clientCmd . m_changeDynamicsInfoArgs . m_contactStiffness , clientCmd . m_changeDynamicsInfoArgs . m_contactDamping ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION )
{
mb - > getBaseCollider ( ) - > setFriction ( lateralFriction ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION )
{
mb - > getBaseCollider ( ) - > setSpinningFriction ( spinningFriction ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION )
{
mb - > getBaseCollider ( ) - > setRollingFriction ( rollingFriction ) ;
2018-03-15 03:47:56 +00:00
}
2016-09-01 20:30:07 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR )
2016-08-10 01:40:12 +00:00
{
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_changeDynamicsInfoArgs . m_frictionAnchor )
2016-08-10 01:40:12 +00:00
{
2017-11-18 00:14:18 +00:00
mb - > getBaseCollider ( ) - > setCollisionFlags ( mb - > getBaseCollider ( ) - > getCollisionFlags ( ) | btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
} else
{
mb - > getBaseCollider ( ) - > setCollisionFlags ( mb - > getBaseCollider ( ) - > getCollisionFlags ( ) & ~ btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
}
2018-03-15 03:47:56 +00:00
}
2017-11-18 00:14:18 +00:00
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS )
{
2018-03-15 03:47:56 +00:00
mb - > setBaseMass ( mass ) ;
2017-11-18 00:14:18 +00:00
if ( mb - > getBaseCollider ( ) & & mb - > getBaseCollider ( ) - > getCollisionShape ( ) )
{
btVector3 localInertia ;
mb - > getBaseCollider ( ) - > getCollisionShape ( ) - > calculateLocalInertia ( mass , localInertia ) ;
mb - > setBaseInertia ( localInertia ) ;
}
}
2017-12-21 00:56:31 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL )
{
mb - > setBaseInertia ( newLocalInertiaDiagonal ) ;
}
2017-11-18 00:14:18 +00:00
}
else
{
if ( linkIndex > = 0 & & linkIndex < mb - > getNumLinks ( ) )
{
if ( mb - > getLinkCollider ( linkIndex ) )
{
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION )
{
mb - > getLinkCollider ( linkIndex ) - > setRestitution ( restitution ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION )
{
mb - > getLinkCollider ( linkIndex ) - > setSpinningFriction ( spinningFriction ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION )
{
mb - > getLinkCollider ( linkIndex ) - > setRollingFriction ( rollingFriction ) ;
}
2016-08-10 01:40:12 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR )
{
if ( clientCmd . m_changeDynamicsInfoArgs . m_frictionAnchor )
2016-08-10 01:40:12 +00:00
{
2017-11-18 00:14:18 +00:00
mb - > getLinkCollider ( linkIndex ) - > setCollisionFlags ( mb - > getLinkCollider ( linkIndex ) - > getCollisionFlags ( ) | btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
} else
{
mb - > getLinkCollider ( linkIndex ) - > setCollisionFlags ( mb - > getLinkCollider ( linkIndex ) - > getCollisionFlags ( ) & ~ btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
2016-08-10 01:40:12 +00:00
}
2018-03-15 03:47:56 +00:00
}
2016-09-01 17:45:14 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION )
2016-08-10 01:40:12 +00:00
{
2017-11-18 00:14:18 +00:00
mb - > getLinkCollider ( linkIndex ) - > setFriction ( lateralFriction ) ;
2016-08-10 01:40:12 +00:00
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING )
{
mb - > getLinkCollider ( linkIndex ) - > setContactStiffnessAndDamping ( clientCmd . m_changeDynamicsInfoArgs . m_contactStiffness , clientCmd . m_changeDynamicsInfoArgs . m_contactDamping ) ;
}
2018-03-15 03:47:56 +00:00
2016-08-10 01:40:12 +00:00
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS )
{
mb - > getLink ( linkIndex ) . m_mass = mass ;
if ( mb - > getLinkCollider ( linkIndex ) & & mb - > getLinkCollider ( linkIndex ) - > getCollisionShape ( ) )
{
btVector3 localInertia ;
mb - > getLinkCollider ( linkIndex ) - > getCollisionShape ( ) - > calculateLocalInertia ( mass , localInertia ) ;
mb - > getLink ( linkIndex ) . m_inertiaLocal = localInertia ;
}
}
2017-12-21 00:56:31 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL )
{
mb - > getLink ( linkIndex ) . m_inertiaLocal = newLocalInertiaDiagonal ;
}
2017-11-18 00:14:18 +00:00
}
}
} else
{
if ( body & & body - > m_rigidBody )
{
2018-06-16 04:26:26 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE )
{
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateEnableSleeping )
{
body - > m_rigidBody - > forceActivationState ( ACTIVE_TAG ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateDisableSleeping )
{
body - > m_rigidBody - > forceActivationState ( DISABLE_DEACTIVATION ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateWakeUp )
{
body - > m_rigidBody - > forceActivationState ( ACTIVE_TAG ) ;
}
if ( clientCmd . m_changeDynamicsInfoArgs . m_activationState & eActivationStateSleep )
{
body - > m_rigidBody - > forceActivationState ( ISLAND_SLEEPING ) ;
}
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING )
{
btScalar angDamping = body - > m_rigidBody - > getAngularDamping ( ) ;
body - > m_rigidBody - > setDamping ( clientCmd . m_changeDynamicsInfoArgs . m_linearDamping , angDamping ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING )
{
btScalar linDamping = body - > m_rigidBody - > getLinearDamping ( ) ;
body - > m_rigidBody - > setDamping ( linDamping , clientCmd . m_changeDynamicsInfoArgs . m_angularDamping ) ;
}
2017-03-26 20:06:46 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING )
{
body - > m_rigidBody - > setContactStiffnessAndDamping ( clientCmd . m_changeDynamicsInfoArgs . m_contactStiffness , clientCmd . m_changeDynamicsInfoArgs . m_contactDamping ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION )
{
body - > m_rigidBody - > setRestitution ( restitution ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION )
{
body - > m_rigidBody - > setFriction ( lateralFriction ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION )
{
body - > m_rigidBody - > setSpinningFriction ( spinningFriction ) ;
}
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION )
{
body - > m_rigidBody - > setRollingFriction ( rollingFriction ) ;
}
2017-09-30 20:56:40 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR )
{
if ( clientCmd . m_changeDynamicsInfoArgs . m_frictionAnchor )
{
body - > m_rigidBody - > setCollisionFlags ( body - > m_rigidBody - > getCollisionFlags ( ) | btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
} else
{
body - > m_rigidBody - > setCollisionFlags ( body - > m_rigidBody - > getCollisionFlags ( ) & ~ btCollisionObject : : CF_HAS_FRICTION_ANCHOR ) ;
}
2018-03-15 03:47:56 +00:00
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS )
{
btVector3 localInertia ;
if ( body - > m_rigidBody - > getCollisionShape ( ) )
{
body - > m_rigidBody - > getCollisionShape ( ) - > calculateLocalInertia ( mass , localInertia ) ;
}
body - > m_rigidBody - > setMassProps ( mass , localInertia ) ;
}
2017-12-21 00:56:31 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL )
{
btScalar orgMass = body - > m_rigidBody - > getInvMass ( ) ;
if ( orgMass > 0 )
{
body - > m_rigidBody - > setMassProps ( mass , newLocalInertiaDiagonal ) ;
}
}
2018-02-17 03:44:33 +00:00
2018-05-23 03:26:00 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD )
{
body - > m_rigidBody - > setContactProcessingThreshold ( clientCmd . m_changeDynamicsInfoArgs . m_contactProcessingThreshold ) ;
}
2018-02-17 03:44:33 +00:00
if ( clientCmd . m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS )
{
body - > m_rigidBody - > setCcdSweptSphereRadius ( clientCmd . m_changeDynamicsInfoArgs . m_ccdSweptSphereRadius ) ;
//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
body - > m_rigidBody - > setCcdMotionThreshold ( clientCmd . m_changeDynamicsInfoArgs . m_ccdSweptSphereRadius / 2. ) ;
}
2017-11-18 00:14:18 +00:00
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = LINK_DYNAMICS_CHANGED ;
notification . m_linkArgs . m_bodyUniqueId = bodyUniqueId ;
notification . m_linkArgs . m_linkIndex = linkIndex ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-03-15 03:47:56 +00:00
return hasStatus ;
2017-11-18 00:14:18 +00:00
}
bool PhysicsServerCommandProcessor : : processSetAdditionalSearchPathCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SET_ADDITIONAL_SEARCH_PATH " ) ;
b3ResourcePath : : setAdditionalSearchPath ( clientCmd . m_searchPathArgs . m_path ) ;
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processGetDynamicsInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_GET_DYNAMICS_INFO_FAILED ;
int bodyUniqueId = clientCmd . m_getDynamicsInfoArgs . m_bodyUniqueId ;
int linkIndex = clientCmd . m_getDynamicsInfoArgs . m_linkIndex ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_GET_DYNAMICS_INFO_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btMultiBody * mb = body - > m_multiBody ;
if ( linkIndex = = - 1 )
{
serverCmd . m_dynamicsInfo . m_mass = mb - > getBaseMass ( ) ;
2017-12-20 22:54:32 +00:00
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 0 ] = mb - > getBaseInertia ( ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 1 ] = mb - > getBaseInertia ( ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 2 ] = mb - > getBaseInertia ( ) [ 2 ] ;
2017-11-18 00:14:18 +00:00
serverCmd . m_dynamicsInfo . m_lateralFrictionCoeff = mb - > getBaseCollider ( ) - > getFriction ( ) ;
2018-01-04 03:17:28 +00:00
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 0 ] = body - > m_rootLocalInertialFrame . getOrigin ( ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 1 ] = body - > m_rootLocalInertialFrame . getOrigin ( ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 2 ] = body - > m_rootLocalInertialFrame . getOrigin ( ) [ 2 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 3 ] = body - > m_rootLocalInertialFrame . getRotation ( ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 4 ] = body - > m_rootLocalInertialFrame . getRotation ( ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 5 ] = body - > m_rootLocalInertialFrame . getRotation ( ) [ 2 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 6 ] = body - > m_rootLocalInertialFrame . getRotation ( ) [ 3 ] ;
2018-03-15 03:47:56 +00:00
2018-01-04 03:17:28 +00:00
serverCmd . m_dynamicsInfo . m_restitution = mb - > getBaseCollider ( ) - > getRestitution ( ) ;
serverCmd . m_dynamicsInfo . m_rollingFrictionCoeff = mb - > getBaseCollider ( ) - > getRollingFriction ( ) ;
serverCmd . m_dynamicsInfo . m_spinningFrictionCoeff = mb - > getBaseCollider ( ) - > getSpinningFriction ( ) ;
if ( mb - > getBaseCollider ( ) - > getCollisionFlags ( ) & btCollisionObject : : CF_HAS_CONTACT_STIFFNESS_DAMPING )
{
serverCmd . m_dynamicsInfo . m_contactStiffness = mb - > getBaseCollider ( ) - > getContactStiffness ( ) ;
serverCmd . m_dynamicsInfo . m_contactDamping = mb - > getBaseCollider ( ) - > getContactDamping ( ) ;
}
else
{
serverCmd . m_dynamicsInfo . m_contactStiffness = - 1 ;
serverCmd . m_dynamicsInfo . m_contactDamping = - 1 ;
}
2017-11-18 00:14:18 +00:00
}
else
{
serverCmd . m_dynamicsInfo . m_mass = mb - > getLinkMass ( linkIndex ) ;
2017-12-20 22:54:32 +00:00
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 0 ] = mb - > getLinkInertia ( linkIndex ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 1 ] = mb - > getLinkInertia ( linkIndex ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialDiagonal [ 2 ] = mb - > getLinkInertia ( linkIndex ) [ 2 ] ;
2018-01-04 03:17:28 +00:00
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 0 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getOrigin ( ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 1 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getOrigin ( ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 2 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getOrigin ( ) [ 2 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 3 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getRotation ( ) [ 0 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 4 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getRotation ( ) [ 1 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 5 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getRotation ( ) [ 2 ] ;
serverCmd . m_dynamicsInfo . m_localInertialFrame [ 6 ] = body - > m_linkLocalInertialFrames [ linkIndex ] . getRotation ( ) [ 3 ] ;
2017-11-18 00:14:18 +00:00
if ( mb - > getLinkCollider ( linkIndex ) )
{
serverCmd . m_dynamicsInfo . m_lateralFrictionCoeff = mb - > getLinkCollider ( linkIndex ) - > getFriction ( ) ;
2018-01-04 03:17:28 +00:00
serverCmd . m_dynamicsInfo . m_restitution = mb - > getLinkCollider ( linkIndex ) - > getRestitution ( ) ;
serverCmd . m_dynamicsInfo . m_rollingFrictionCoeff = mb - > getLinkCollider ( linkIndex ) - > getRollingFriction ( ) ;
serverCmd . m_dynamicsInfo . m_spinningFrictionCoeff = mb - > getLinkCollider ( linkIndex ) - > getSpinningFriction ( ) ;
if ( mb - > getLinkCollider ( linkIndex ) - > getCollisionFlags ( ) & btCollisionObject : : CF_HAS_CONTACT_STIFFNESS_DAMPING )
{
serverCmd . m_dynamicsInfo . m_contactStiffness = mb - > getLinkCollider ( linkIndex ) - > getContactStiffness ( ) ;
serverCmd . m_dynamicsInfo . m_contactDamping = mb - > getLinkCollider ( linkIndex ) - > getContactDamping ( ) ;
}
else
{
serverCmd . m_dynamicsInfo . m_contactStiffness = - 1 ;
serverCmd . m_dynamicsInfo . m_contactDamping = - 1 ;
}
2017-11-18 00:14:18 +00:00
}
else
{
b3Warning ( " The dynamic info requested is not available " ) ;
serverCmd . m_type = CMD_GET_DYNAMICS_INFO_FAILED ;
}
}
hasStatus = true ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestPhysicsSimulationParametersCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_allowedCcdPenetration = m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_allowedCcdPenetration ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_collisionFilterMode = m_data - > m_broadphaseCollisionFilterCallback - > m_filterMode ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_deltaTime = m_data - > m_physicsDeltaTime ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_contactBreakingThreshold = gContactBreakingThreshold ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_contactSlop = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_linearSlop ;
2018-06-12 23:08:46 +00:00
serverCmd . m_simulationParameterResultArgs . m_enableSAT = m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_enableSatConvex ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_defaultGlobalCFM = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_globalCfm ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_defaultContactERP = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp2 ;
serverCmd . m_simulationParameterResultArgs . m_defaultNonContactERP = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp ;
serverCmd . m_simulationParameterResultArgs . m_deltaTime = m_data - > m_physicsDeltaTime ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_deterministicOverlappingPairs = m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_deterministicOverlappingPairs ;
serverCmd . m_simulationParameterResultArgs . m_enableConeFriction = ( m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION ) ? 0 : 1 ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_enableFileCaching = b3IsFileCachingEnabled ( ) ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_frictionCFM = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_frictionCFM ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_frictionERP = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_frictionERP ;
btVector3 grav = m_data - > m_dynamicsWorld - > getGravity ( ) ;
serverCmd . m_simulationParameterResultArgs . m_gravityAcceleration [ 0 ] = grav [ 0 ] ;
serverCmd . m_simulationParameterResultArgs . m_gravityAcceleration [ 1 ] = grav [ 1 ] ;
serverCmd . m_simulationParameterResultArgs . m_gravityAcceleration [ 2 ] = grav [ 2 ] ;
serverCmd . m_simulationParameterResultArgs . m_internalSimFlags = gInternalSimFlags ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_jointFeedbackMode = 0 ;
if ( gJointFeedbackInWorldSpace )
{
serverCmd . m_simulationParameterResultArgs . m_jointFeedbackMode | = JOINT_FEEDBACK_IN_WORLD_SPACE ;
}
if ( gJointFeedbackInJointFrame )
{
serverCmd . m_simulationParameterResultArgs . m_jointFeedbackMode | = JOINT_FEEDBACK_IN_JOINT_FRAME ;
}
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_numSimulationSubSteps = m_data - > m_numSimulationSubSteps ;
serverCmd . m_simulationParameterResultArgs . m_numSolverIterations = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_numIterations ;
serverCmd . m_simulationParameterResultArgs . m_restitutionVelocityThreshold = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_restitutionVelocityThreshold ;
2018-06-10 02:40:12 +00:00
serverCmd . m_simulationParameterResultArgs . m_solverResidualThreshold = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_leastSquaresResidualThreshold ;
2017-11-18 00:14:18 +00:00
serverCmd . m_simulationParameterResultArgs . m_splitImpulsePenetrationThreshold = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_splitImpulsePenetrationThreshold ;
serverCmd . m_simulationParameterResultArgs . m_useRealTimeSimulation = m_data - > m_useRealTimeSimulation ;
serverCmd . m_simulationParameterResultArgs . m_useSplitImpulse = m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_splitImpulse ;
2018-06-10 02:40:12 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2018-05-12 02:52:06 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processSendPhysicsParametersCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_SEND_PHYSICS_SIMULATION_PARAMETERS " ) ;
2017-11-29 04:09:56 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION )
{
if ( clientCmd . m_physSimParamArgs . m_enableConeFriction )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_solverMode & = ~ SOLVER_DISABLE_IMPLICIT_CONE_FRICTION ;
} else
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_solverMode | = SOLVER_DISABLE_IMPLICIT_CONE_FRICTION ;
}
}
2018-01-09 18:10:36 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS )
{
2018-01-17 20:48:48 +00:00
m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_deterministicOverlappingPairs = ( clientCmd . m_physSimParamArgs . m_deterministicOverlappingPairs ! = 0 ) ;
2018-01-09 18:10:36 +00:00
}
2018-02-17 03:44:33 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION )
{
m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_allowedCcdPenetration = clientCmd . m_physSimParamArgs . m_allowedCcdPenetration ;
}
2018-05-12 02:52:06 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE )
{
gJointFeedbackInWorldSpace = ( clientCmd . m_physSimParamArgs . m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE ) ! = 0 ;
gJointFeedbackInJointFrame = ( clientCmd . m_physSimParamArgs . m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME ) ! = 0 ;
}
2018-02-17 03:44:33 +00:00
2017-11-18 00:14:18 +00:00
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_REAL_TIME_SIMULATION )
{
m_data - > m_useRealTimeSimulation = ( clientCmd . m_physSimParamArgs . m_useRealTimeSimulation ! = 0 ) ;
}
2018-03-15 03:47:56 +00:00
//see
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS )
{
//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
gInternalSimFlags = clientCmd . m_physSimParamArgs . m_internalSimFlags ;
}
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 ) ;
2018-02-21 03:44:02 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2018-02-18 19:09:42 +00:00
m_data - > m_dynamicsWorld - > getWorldInfo ( ) . m_gravity = grav ;
2018-03-15 03:47:56 +00:00
2018-02-21 03:44:02 +00:00
# endif
2017-11-18 00:14:18 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Updated Gravity: %f,%f,%f " , grav [ 0 ] , grav [ 1 ] , grav [ 2 ] ) ;
}
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_numIterations = clientCmd . m_physSimParamArgs . m_numSolverIterations ;
}
2018-05-23 03:26:00 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_leastSquaresResidualThreshold = clientCmd . m_physSimParamArgs . m_solverResidualThreshold ;
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD )
{
gContactBreakingThreshold = clientCmd . m_physSimParamArgs . m_contactBreakingThreshold ;
}
2018-05-23 03:26:00 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_CONTACT_SLOP )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_linearSlop = clientCmd . m_physSimParamArgs . m_contactSlop ;
}
2018-06-12 23:08:46 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_ENABLE_SAT )
{
2018-06-16 20:59:03 +00:00
m_data - > m_dynamicsWorld - > getDispatchInfo ( ) . m_enableSatConvex = clientCmd . m_physSimParamArgs . m_enableSAT ! = 0 ;
2018-06-12 23:08:46 +00:00
}
2018-08-24 06:04:17 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_CONSTRAINT_SOLVER_TYPE )
{
//check if the current type is different from requested one
if ( m_data - > m_constraintSolverType ! = clientCmd . m_physSimParamArgs . m_constraintSolverType )
{
m_data - > m_constraintSolverType = clientCmd . m_physSimParamArgs . m_constraintSolverType ;
btConstraintSolver * oldSolver = m_data - > m_dynamicsWorld - > getConstraintSolver ( ) ;
btMultiBodyConstraintSolver * newSolver = 0 ;
switch ( clientCmd . m_physSimParamArgs . m_constraintSolverType )
{
case eConstraintSolverLCP_SI :
{
newSolver = new btMultiBodyConstraintSolver ;
b3Printf ( " PyBullet: Constraint Solver: btMultiBodyConstraintSolver \n " ) ;
break ;
}
case eConstraintSolverLCP_PGS :
{
btSolveProjectedGaussSeidel * mlcp = new btSolveProjectedGaussSeidel ( ) ;
newSolver = new btMultiBodyMLCPConstraintSolver ( mlcp ) ;
b3Printf ( " PyBullet: Constraint Solver: MLCP + PGS \n " ) ;
break ;
}
case eConstraintSolverLCP_DANTZIG :
{
btDantzigSolver * mlcp = new btDantzigSolver ( ) ;
newSolver = new btMultiBodyMLCPConstraintSolver ( mlcp ) ;
b3Printf ( " PyBullet: Constraint Solver: MLCP + Dantzig \n " ) ;
break ;
}
case eConstraintSolverLCP_BLOCK_PGS :
{
break ;
}
default :
{
}
} ;
2018-06-12 23:08:46 +00:00
2018-08-24 06:04:17 +00:00
if ( newSolver )
{
delete oldSolver ;
m_data - > m_dynamicsWorld - > setMultiBodyConstraintSolver ( newSolver ) ;
m_data - > m_solver = newSolver ;
printf ( " switched solver \n " ) ;
}
}
}
2018-05-23 03:26:00 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_COLLISION_FILTER_MODE )
{
m_data - > m_broadphaseCollisionFilterCallback - > m_filterMode = clientCmd . m_physSimParamArgs . m_collisionFilterMode ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_splitImpulse = clientCmd . m_physSimParamArgs . m_useSplitImpulse ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_splitImpulsePenetrationThreshold = clientCmd . m_physSimParamArgs . m_splitImpulsePenetrationThreshold ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS )
{
m_data - > m_numSimulationSubSteps = clientCmd . m_physSimParamArgs . m_numSimulationSubSteps ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp2 = clientCmd . m_physSimParamArgs . m_defaultContactERP ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp = clientCmd . m_physSimParamArgs . m_defaultNonContactERP ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_frictionERP = clientCmd . m_physSimParamArgs . m_frictionERP ;
}
2018-05-16 20:46:19 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_globalCfm = clientCmd . m_physSimParamArgs . m_defaultGlobalCFM ;
}
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM )
{
2018-06-10 02:40:12 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_frictionCFM = clientCmd . m_physSimParamArgs . m_frictionCFM ;
2018-05-16 20:46:19 +00:00
}
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_restitutionVelocityThreshold = clientCmd . m_physSimParamArgs . m_restitutionVelocityThreshold ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_ENABLE_FILE_CACHING )
{
b3EnableFileCaching ( clientCmd . m_physSimParamArgs . m_enableFileCaching ) ;
}
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processInitPoseCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_INIT_POSE " ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Server Init Pose not implemented yet " ) ;
}
int bodyUniqueId = clientCmd . m_initPoseArgs . m_bodyUniqueId ;
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
btVector3 baseLinVel ( 0 , 0 , 0 ) ;
btVector3 baseAngVel ( 0 , 0 , 0 ) ;
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY )
{
baseLinVel . setValue ( clientCmd . m_initPoseArgs . m_initialStateQdot [ 0 ] ,
clientCmd . m_initPoseArgs . m_initialStateQdot [ 1 ] ,
clientCmd . m_initPoseArgs . m_initialStateQdot [ 2 ] ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY )
{
baseAngVel . setValue ( clientCmd . m_initPoseArgs . m_initialStateQdot [ 3 ] ,
clientCmd . m_initPoseArgs . m_initialStateQdot [ 4 ] ,
clientCmd . m_initPoseArgs . m_initialStateQdot [ 5 ] ) ;
}
btVector3 basePos ( 0 , 0 , 0 ) ;
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION )
{
basePos = btVector3 (
clientCmd . m_initPoseArgs . m_initialStateQ [ 0 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 1 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 2 ] ) ;
}
btQuaternion baseOrn ( 0 , 0 , 0 , 1 ) ;
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION )
{
baseOrn . setValue ( 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 ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY )
{
mb - > setBaseVel ( baseLinVel ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY )
{
mb - > setBaseOmega ( baseAngVel ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION )
{
btVector3 zero ( 0 , 0 , 0 ) ;
btAssert ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 0 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 1 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 2 ] ) ;
mb - > setBaseVel ( baseLinVel ) ;
mb - > setBasePos ( basePos ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION )
{
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 ] ) ;
mb - > setBaseOmega ( baseAngVel ) ;
btQuaternion invOrn ( baseOrn ) ;
mb - > setWorldToBaseRot ( invOrn . inverse ( ) ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_JOINT_STATE )
{
int uDofIndex = 6 ;
int posVarCountIndex = 7 ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
if ( ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ posVarCountIndex ] ) & & ( mb - > getLink ( i ) . m_dofCount = = 1 ) )
{
mb - > setJointPos ( i , clientCmd . m_initPoseArgs . m_initialStateQ [ posVarCountIndex ] ) ;
mb - > setJointVel ( i , 0 ) ; //backwards compatibility
}
if ( ( clientCmd . m_initPoseArgs . m_hasInitialStateQdot [ uDofIndex ] ) & & ( mb - > getLink ( i ) . m_dofCount = = 1 ) )
{
btScalar vel = clientCmd . m_initPoseArgs . m_initialStateQdot [ uDofIndex ] ;
mb - > setJointVel ( i , vel ) ;
}
posVarCountIndex + = mb - > getLink ( i ) . m_posVarCount ;
uDofIndex + = mb - > getLink ( i ) . m_dofCount ;
}
}
2018-06-02 05:50:06 +00:00
2017-11-18 00:14:18 +00:00
btAlignedObjectArray < btQuaternion > scratch_q ;
btAlignedObjectArray < btVector3 > scratch_m ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
mb - > forwardKinematics ( scratch_q , scratch_m ) ;
int nLinks = mb - > getNumLinks ( ) ;
scratch_q . resize ( nLinks + 1 ) ;
scratch_m . resize ( nLinks + 1 ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
mb - > updateCollisionObjectWorldTransforms ( scratch_q , scratch_m ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
if ( body & & body - > m_rigidBody )
{
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY )
{
body - > m_rigidBody - > setLinearVelocity ( baseLinVel ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY )
{
body - > m_rigidBody - > setAngularVelocity ( baseAngVel ) ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION )
{
body - > m_rigidBody - > getWorldTransform ( ) . setOrigin ( basePos ) ;
body - > m_rigidBody - > setLinearVelocity ( baseLinVel ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION )
{
body - > m_rigidBody - > getWorldTransform ( ) . setRotation ( baseOrn ) ;
body - > m_rigidBody - > setAngularVelocity ( baseAngVel ) ;
}
}
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processResetSimulationCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_RESET_SIMULATION " ) ;
m_data - > m_guiHelper - > setVisualizerFlag ( COV_ENABLE_SYNC_RENDERING_INTERNAL , 0 ) ;
resetSimulation ( ) ;
m_data - > m_guiHelper - > setVisualizerFlag ( COV_ENABLE_SYNC_RENDERING_INTERNAL , 1 ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RESET_SIMULATION_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processCreateRigidBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RIGID_BODY_CREATION_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_CREATE_RIGID_BODY " ) ;
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 )
{
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 ;
}
2017-11-23 02:12:02 +00:00
btMultiBodyWorldImporter * worldImporter = new btMultiBodyWorldImporter ( m_data - > m_dynamicsWorld ) ;
2017-11-18 00:14:18 +00:00
m_data - > m_worldImporters . push_back ( worldImporter ) ;
btCollisionShape * shape = 0 ;
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 ) ;
}
}
bool isDynamic = ( mass > 0 ) ;
btRigidBody * rb = worldImporter - > createRigidBody ( isDynamic , mass , startTrans , shape , 0 ) ;
//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 ) ;
int bodyUniqueId = m_data - > m_bodyHandles . allocHandle ( ) ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
serverCmd . m_rigidBodyCreateArgs . m_bodyUniqueId = bodyUniqueId ;
rb - > setUserIndex2 ( bodyUniqueId ) ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
bodyHandle - > m_rigidBody = rb ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = BODY_ADDED ;
notification . m_bodyArgs . m_bodyUniqueId = bodyUniqueId ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processPickBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " 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 ] ) ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processMovePickedBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " 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 ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRemovePickingConstraintCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REMOVE_PICKING_CONSTRAINT_BODY " ) ;
removePickingConstraint ( ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestAabbOverlapCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_AABB_OVERLAP " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
int curObjectIndex = clientCmd . m_requestOverlappingObjectsArgs . m_startingOverlappingObjectIndex ;
if ( 0 = = curObjectIndex )
{
//clientCmd.m_requestContactPointArguments.m_aabbQueryMin
btVector3 aabbMin , aabbMax ;
aabbMin . setValue ( clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMin [ 0 ] ,
clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMin [ 1 ] ,
clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMin [ 2 ] ) ;
aabbMax . setValue ( clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMax [ 0 ] ,
clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMax [ 1 ] ,
clientCmd . m_requestOverlappingObjectsArgs . m_aabbQueryMax [ 2 ] ) ;
m_data - > m_cachedOverlappingObjects . clear ( ) ;
m_data - > m_dynamicsWorld - > getBroadphase ( ) - > aabbTest ( aabbMin , aabbMax , m_data - > m_cachedOverlappingObjects ) ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
int totalBytesPerObject = sizeof ( b3OverlappingObject ) ;
int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1 ;
int numOverlap = m_data - > m_cachedOverlappingObjects . m_bodyUniqueIds . size ( ) ;
int remainingObjects = numOverlap - curObjectIndex ;
int curNumObjects = btMin ( overlapCapacity , remainingObjects ) ;
if ( numOverlap < overlapCapacity )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
b3OverlappingObject * overlapStorage = ( b3OverlappingObject * ) bufferServerToClient ;
for ( int i = 0 ; i < m_data - > m_cachedOverlappingObjects . m_bodyUniqueIds . size ( ) ; i + + )
{
overlapStorage [ i ] . m_objectUniqueId = m_data - > m_cachedOverlappingObjects . m_bodyUniqueIds [ i ] ;
overlapStorage [ i ] . m_linkIndex = m_data - > m_cachedOverlappingObjects . m_links [ i ] ;
}
serverCmd . m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED ;
//int m_startingOverlappingObjectIndex;
//int m_numOverlappingObjectsCopied;
//int m_numRemainingOverlappingObjects;
serverCmd . m_sendOverlappingObjectsArgs . m_startingOverlappingObjectIndex = clientCmd . m_requestOverlappingObjectsArgs . m_startingOverlappingObjectIndex ;
serverCmd . m_sendOverlappingObjectsArgs . m_numOverlappingObjectsCopied = m_data - > m_cachedOverlappingObjects . m_bodyUniqueIds . size ( ) ;
serverCmd . m_sendOverlappingObjectsArgs . m_numRemainingOverlappingObjects = remainingObjects - curNumObjects ;
}
else
{
serverCmd . m_type = CMD_REQUEST_AABB_OVERLAP_FAILED ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processRequestOpenGLVisualizeCameraCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_OPENGL_VISUALIZER_CAMERA " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
bool result = this - > m_data - > m_guiHelper - > getCameraInfo (
& serverCmd . m_visualizerCameraResultArgs . m_width ,
& serverCmd . m_visualizerCameraResultArgs . m_height ,
serverCmd . m_visualizerCameraResultArgs . m_viewMatrix ,
serverCmd . m_visualizerCameraResultArgs . m_projectionMatrix ,
serverCmd . m_visualizerCameraResultArgs . m_camUp ,
serverCmd . m_visualizerCameraResultArgs . m_camForward ,
serverCmd . m_visualizerCameraResultArgs . m_horizontal ,
serverCmd . m_visualizerCameraResultArgs . m_vertical ,
& serverCmd . m_visualizerCameraResultArgs . m_yaw ,
& serverCmd . m_visualizerCameraResultArgs . m_pitch ,
& serverCmd . m_visualizerCameraResultArgs . m_dist ,
serverCmd . m_visualizerCameraResultArgs . m_target ) ;
serverCmd . m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED : CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED ;
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processConfigureOpenGLVisualizerCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CONFIGURE_OPENGL_VISUALIZER " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
hasStatus = true ;
if ( clientCmd . m_updateFlags & COV_SET_FLAGS )
{
if ( clientCmd . m_configureOpenGLVisualizerArguments . m_setFlag = = COV_ENABLE_TINY_RENDERER )
{
m_data - > m_enableTinyRenderer = clientCmd . m_configureOpenGLVisualizerArguments . m_setEnabled ! = 0 ;
}
m_data - > m_guiHelper - > setVisualizerFlag ( clientCmd . m_configureOpenGLVisualizerArguments . m_setFlag ,
clientCmd . m_configureOpenGLVisualizerArguments . m_setEnabled ) ;
}
if ( clientCmd . m_updateFlags & COV_SET_CAMERA_VIEW_MATRIX )
{
m_data - > m_guiHelper - > resetCamera ( clientCmd . m_configureOpenGLVisualizerArguments . m_cameraDistance ,
clientCmd . m_configureOpenGLVisualizerArguments . m_cameraYaw ,
clientCmd . m_configureOpenGLVisualizerArguments . m_cameraPitch ,
clientCmd . m_configureOpenGLVisualizerArguments . m_cameraTargetPosition [ 0 ] ,
clientCmd . m_configureOpenGLVisualizerArguments . m_cameraTargetPosition [ 1 ] ,
clientCmd . m_configureOpenGLVisualizerArguments . m_cameraTargetPosition [ 2 ] ) ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processInverseDynamicsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CALCULATE_INVERSE_DYNAMICS " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
if ( tree )
{
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
const int num_dofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
btInverseDynamics : : vecx nu ( num_dofs + baseDofs ) , qdot ( num_dofs + baseDofs ) , q ( num_dofs + baseDofs ) , joint_force ( num_dofs + baseDofs ) ;
for ( int i = 0 ; i < num_dofs ; i + + )
{
q [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointPositionsQ [ i ] ;
qdot [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointVelocitiesQdot [ i ] ;
nu [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointAccelerations [ i ] ;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
serverCmd . m_inverseDynamicsResultArgs . m_bodyUniqueId = clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ;
serverCmd . m_inverseDynamicsResultArgs . m_dofCount = num_dofs ;
for ( int i = 0 ; i < num_dofs ; i + + )
{
serverCmd . m_inverseDynamicsResultArgs . m_jointForces [ i ] = joint_force [ i + baseDofs ] ;
}
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED ;
}
else
{
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
}
}
}
else
{
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processCalculateJacobianCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CALCULATE_JACOBIAN " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( clientCmd . m_calculateJacobianArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( tree )
{
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
const int numDofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
btInverseDynamics : : vecx q ( numDofs + baseDofs ) ;
btInverseDynamics : : vecx qdot ( numDofs + baseDofs ) ;
btInverseDynamics : : vecx nu ( numDofs + baseDofs ) ;
btInverseDynamics : : vecx joint_force ( numDofs + baseDofs ) ;
for ( int i = 0 ; i < numDofs ; i + + )
{
q [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointPositionsQ [ i ] ;
qdot [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointVelocitiesQdot [ i ] ;
nu [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointAccelerations [ i ] ;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
serverCmd . m_jacobianResultArgs . m_dofCount = numDofs + baseDofs ;
// Set jacobian value
tree - > calculateJacobians ( q ) ;
btInverseDynamics : : mat3x jac_t ( 3 , numDofs + baseDofs ) ;
btInverseDynamics : : mat3x jac_r ( 3 , numDofs + baseDofs ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree - > getBodyJacobianTrans ( clientCmd . m_calculateJacobianArguments . m_linkIndex + 1 , & jac_t ) ;
tree - > getBodyJacobianRot ( clientCmd . m_calculateJacobianArguments . m_linkIndex + 1 , & jac_r ) ;
// Update the translational jacobian based on the desired local point.
// v_pt = v_frame + w x pt
// v_pt = J_t * qd + (J_r * qd) x pt
// v_pt = J_t * qd - pt x (J_r * qd)
// v_pt = J_t * qd - pt_x * J_r * qd)
// v_pt = (J_t - pt_x * J_r) * qd
// J_t_new = J_t - pt_x * J_r
btInverseDynamics : : vec3 localPosition ;
for ( int i = 0 ; i < 3 ; + + i ) {
localPosition ( i ) = clientCmd . m_calculateJacobianArguments . m_localPosition [ i ] ;
}
// Only calculate if the localPosition is non-zero.
if ( btInverseDynamics : : maxAbs ( localPosition ) > 0.0 ) {
// Write the localPosition into world coordinates.
btInverseDynamics : : mat33 world_rotation_body ;
tree - > getBodyTransform ( clientCmd . m_calculateJacobianArguments . m_linkIndex + 1 , & world_rotation_body ) ;
localPosition = world_rotation_body * localPosition ;
// Correct the translational jacobian.
btInverseDynamics : : mat33 skewCrossProduct ;
btInverseDynamics : : skew ( localPosition , & skewCrossProduct ) ;
btInverseDynamics : : mat3x jac_l ( 3 , numDofs + baseDofs ) ;
btInverseDynamics : : mul ( skewCrossProduct , jac_r , & jac_l ) ;
btInverseDynamics : : mat3x jac_t_new ( 3 , numDofs + baseDofs ) ;
btInverseDynamics : : sub ( jac_t , jac_l , & jac_t_new ) ;
jac_t = jac_t_new ;
}
// Fill in the result into the shared memory.
for ( int i = 0 ; i < 3 ; + + i )
{
for ( int j = 0 ; j < ( numDofs + baseDofs ) ; + + j )
{
int element = ( numDofs + baseDofs ) * i + j ;
serverCmd . m_jacobianResultArgs . m_linearJacobian [ element ] = jac_t ( i , j ) ;
serverCmd . m_jacobianResultArgs . m_angularJacobian [ element ] = jac_r ( i , j ) ;
}
}
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_COMPLETED ;
}
else
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
else
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processCalculateMassMatrixCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CALCULATE_MASS_MATRIX " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CALCULATED_MASS_MATRIX_FAILED ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( clientCmd . m_calculateMassMatrixArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( tree )
{
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
const int numDofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
const int totDofs = numDofs + baseDofs ;
btInverseDynamics : : vecx q ( totDofs ) ;
btInverseDynamics : : matxx massMatrix ( totDofs , totDofs ) ;
for ( int i = 0 ; i < numDofs ; i + + )
{
q [ i + baseDofs ] = clientCmd . m_calculateMassMatrixArguments . m_jointPositionsQ [ i ] ;
}
if ( - 1 ! = tree - > calculateMassMatrix ( q , & massMatrix ) )
{
serverCmd . m_massMatrixResultArgs . m_dofCount = totDofs ;
// Fill in the result into the shared memory.
double * sharedBuf = ( double * ) bufferServerToClient ;
int sizeInBytes = totDofs * totDofs * sizeof ( double ) ;
if ( sizeInBytes < bufferSizeInBytes )
{
for ( int i = 0 ; i < ( totDofs ) ; + + i )
{
for ( int j = 0 ; j < ( totDofs ) ; + + j )
{
int element = ( totDofs ) * i + j ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
sharedBuf [ element ] = massMatrix ( i , j ) ;
}
}
serverCmd . m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED ;
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
else
{
serverCmd . m_type = CMD_CALCULATED_MASS_MATRIX_FAILED ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
2017-03-26 20:06:46 +00:00
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processApplyExternalForceCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_APPLY_EXTERNAL_FORCE " ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " CMD_APPLY_EXTERNAL_FORCE clientCmd = %d \n " , clientCmd . m_sequenceNumber ) ;
}
for ( int i = 0 ; i < clientCmd . m_externalForceArguments . m_numForcesAndTorques ; + + i )
{
InternalBodyData * body = m_data - > m_bodyHandles . getHandle ( clientCmd . m_externalForceArguments . m_bodyUniqueIds [ i ] ) ;
bool isLinkFrame = ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_LINK_FRAME ) ! = 0 ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_FORCE ) ! = 0 )
{
btVector3 tmpForce ( 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 tmpPosition (
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 2 ] ) ;
2016-09-01 17:45:14 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_externalForceArguments . m_linkIds [ i ] = = - 1 )
{
btVector3 forceWorld = isLinkFrame ? mb - > getBaseWorldTransform ( ) . getBasis ( ) * tmpForce : tmpForce ;
btVector3 relPosWorld = isLinkFrame ? mb - > getBaseWorldTransform ( ) . getBasis ( ) * tmpPosition : tmpPosition - mb - > getBaseWorldTransform ( ) . getOrigin ( ) ;
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 ] ;
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
btVector3 forceWorld = isLinkFrame ? mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * tmpForce : tmpForce ;
btVector3 relPosWorld = isLinkFrame ? mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * tmpPosition : tmpPosition - mb - > getBaseWorldTransform ( ) . getOrigin ( ) ;
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 ] ) ;
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
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]);
}
}
}
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
if ( body & & body - > m_rigidBody )
{
btRigidBody * rb = body - > m_rigidBody ;
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 ] ) ;
btVector3 forceWorld = isLinkFrame ? forceLocal : rb - > getWorldTransform ( ) . getBasis ( ) * forceLocal ;
btVector3 relPosWorld = isLinkFrame ? positionLocal : rb - > getWorldTransform ( ) . getBasis ( ) * positionLocal ;
rb - > applyForce ( forceWorld , relPosWorld ) ;
2016-12-01 06:24:20 +00:00
2017-11-18 00:14:18 +00:00
}
2016-09-01 17:45:14 +00:00
2017-11-18 00:14:18 +00:00
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 ] ) ;
2017-05-04 04:53:29 +00:00
2017-11-18 00:14:18 +00:00
btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb - > getWorldTransform ( ) . getBasis ( ) * torqueLocal ;
rb - > applyTorque ( torqueWorld ) ;
}
}
}
2017-05-16 19:19:03 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
return hasStatus ;
}
2017-05-04 20:14:24 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRemoveBodyCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2017-05-04 20:14:24 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_REMOVE_BODY_FAILED ;
serverCmd . m_removeObjectArgs . m_numBodies = 0 ;
serverCmd . m_removeObjectArgs . m_numUserConstraints = 0 ;
2017-05-04 20:14:24 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > setVisualizerFlag ( COV_ENABLE_SYNC_RENDERING_INTERNAL , 0 ) ;
2017-05-04 20:14:24 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; i < clientCmd . m_removeObjectArgs . m_numBodies ; i + + )
{
int bodyUniqueId = clientCmd . m_removeObjectArgs . m_bodyUniqueIds [ i ] ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( bodyHandle )
{
if ( bodyHandle - > m_multiBody )
{
serverCmd . m_removeObjectArgs . m_bodyUniqueIds [ serverCmd . m_removeObjectArgs . m_numBodies + + ] = bodyUniqueId ;
2017-05-04 20:14:24 +00:00
2018-01-14 19:01:57 +00:00
if ( m_data - > m_pickingMultiBodyPoint2Point & & m_data - > m_pickingMultiBodyPoint2Point - > getMultiBodyA ( ) = = bodyHandle - > m_multiBody )
{
//memory will be deleted in the code that follows
m_data - > m_pickingMultiBodyPoint2Point = 0 ;
}
2017-11-18 00:14:18 +00:00
//also remove user constraints...
for ( int i = m_data - > m_dynamicsWorld - > getNumMultiBodyConstraints ( ) - 1 ; i > = 0 ; i - - )
{
btMultiBodyConstraint * mbc = m_data - > m_dynamicsWorld - > getMultiBodyConstraint ( i ) ;
if ( ( mbc - > getMultiBodyA ( ) = = bodyHandle - > m_multiBody ) | | ( mbc - > getMultiBodyB ( ) = = bodyHandle - > m_multiBody ) )
{
m_data - > m_dynamicsWorld - > removeMultiBodyConstraint ( mbc ) ;
//also remove user constraint and submit it as removed
for ( int c = m_data - > m_userConstraints . size ( ) - 1 ; c > = 0 ; c - - )
{
InteralUserConstraintData * userConstraintPtr = m_data - > m_userConstraints . getAtIndex ( c ) ;
int userConstraintKey = m_data - > m_userConstraints . getKeyAtIndex ( c ) . getUid1 ( ) ;
if ( userConstraintPtr - > m_mbConstraint = = mbc )
2017-05-04 04:53:29 +00:00
{
2017-11-18 00:14:18 +00:00
m_data - > m_userConstraints . remove ( userConstraintKey ) ;
serverCmd . m_removeObjectArgs . m_userConstraintUniqueIds [ serverCmd . m_removeObjectArgs . m_numUserConstraints + + ] = userConstraintKey ;
2017-05-04 04:53:29 +00:00
}
}
2017-11-18 00:14:18 +00:00
delete mbc ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
2018-03-15 03:47:56 +00:00
2018-01-14 19:01:57 +00:00
2017-11-18 00:14:18 +00:00
if ( bodyHandle - > m_multiBody - > getBaseCollider ( ) )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > removeVisualShape ( bodyHandle - > m_multiBody - > getBaseCollider ( ) - > getBroadphaseHandle ( ) - > getUid ( ) ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
m_data - > m_dynamicsWorld - > removeCollisionObject ( bodyHandle - > m_multiBody - > getBaseCollider ( ) ) ;
int graphicsIndex = bodyHandle - > m_multiBody - > getBaseCollider ( ) - > getUserIndex ( ) ;
m_data - > m_guiHelper - > removeGraphicsInstance ( graphicsIndex ) ;
}
for ( int link = 0 ; link < bodyHandle - > m_multiBody - > getNumLinks ( ) ; link + + )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( bodyHandle - > m_multiBody - > getLink ( link ) . m_collider )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > removeVisualShape ( bodyHandle - > m_multiBody - > getLink ( link ) . m_collider - > getBroadphaseHandle ( ) - > getUid ( ) ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
m_data - > m_dynamicsWorld - > removeCollisionObject ( bodyHandle - > m_multiBody - > getLink ( link ) . m_collider ) ;
int graphicsIndex = bodyHandle - > m_multiBody - > getLink ( link ) . m_collider - > getUserIndex ( ) ;
m_data - > m_guiHelper - > removeGraphicsInstance ( graphicsIndex ) ;
2017-05-04 04:53:29 +00:00
}
2017-11-18 00:14:18 +00:00
}
int numCollisionObjects = m_data - > m_dynamicsWorld - > getNumCollisionObjects ( ) ;
m_data - > m_dynamicsWorld - > removeMultiBody ( bodyHandle - > m_multiBody ) ;
numCollisionObjects = m_data - > m_dynamicsWorld - > getNumCollisionObjects ( ) ;
//todo: clear all other remaining data, release memory etc
delete bodyHandle - > m_multiBody ;
bodyHandle - > m_multiBody = 0 ;
serverCmd . m_type = CMD_REMOVE_BODY_COMPLETED ;
}
if ( bodyHandle - > m_rigidBody )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > removeVisualShape ( bodyHandle - > m_rigidBody - > getBroadphaseHandle ( ) - > getUid ( ) ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
serverCmd . m_removeObjectArgs . m_bodyUniqueIds [ serverCmd . m_removeObjectArgs . m_numBodies + + ] = bodyUniqueId ;
2018-03-15 03:47:56 +00:00
2018-01-14 19:01:57 +00:00
if ( m_data - > m_pickedConstraint & & m_data - > m_pickedBody = = bodyHandle - > m_rigidBody )
{
m_data - > m_pickedConstraint = 0 ;
m_data - > m_pickedBody = 0 ;
}
2017-11-18 00:14:18 +00:00
//todo: clear all other remaining data...
m_data - > m_dynamicsWorld - > removeRigidBody ( bodyHandle - > m_rigidBody ) ;
int graphicsInstance = bodyHandle - > m_rigidBody - > getUserIndex2 ( ) ;
m_data - > m_guiHelper - > removeGraphicsInstance ( graphicsInstance ) ;
delete bodyHandle - > m_rigidBody ;
bodyHandle - > m_rigidBody = 0 ;
serverCmd . m_type = CMD_REMOVE_BODY_COMPLETED ;
}
2018-07-03 15:45:19 +00:00
for ( int i = 0 ; i < bodyHandle - > m_userDataHandles . size ( ) ; i + + ) {
int userDataHandle = bodyHandle - > m_userDataHandles [ i ] ;
SharedMemoryUserData * userData = m_data - > m_userDataHandles . getHandle ( userDataHandle ) ;
m_data - > m_userDataHandleLookup . remove ( SharedMemoryUserDataHashKey ( userData ) ) ;
m_data - > m_userDataHandles . freeHandle ( userDataHandle ) ;
}
2018-01-14 20:35:40 +00:00
m_data - > m_bodyHandles . freeHandle ( bodyUniqueId ) ;
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
m_data - > m_guiHelper - > setVisualizerFlag ( COV_ENABLE_SYNC_RENDERING_INTERNAL , 1 ) ;
2018-07-24 12:12:16 +00:00
for ( int i = 0 ; i < serverCmd . m_removeObjectArgs . m_numBodies ; i + + )
{
b3Notification notification ;
notification . m_notificationType = BODY_REMOVED ;
notification . m_bodyArgs . m_bodyUniqueId = serverCmd . m_removeObjectArgs . m_bodyUniqueIds [ i ] ;
m_data - > m_pluginManager . addNotification ( notification ) ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processCreateUserConstraintCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2017-05-16 19:19:03 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_USER_CONSTRAINT " ) ;
2017-05-04 04:53:29 +00:00
2017-11-18 00:14:18 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_USER_CONSTRAINT_FAILED ;
hasStatus = true ;
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_REQUEST_STATE )
{
int constraintUid = clientCmd . m_userConstraintArguments . m_userConstraintUniqueId ;
InteralUserConstraintData * userConstraintPtr = m_data - > m_userConstraints . find ( constraintUid ) ;
if ( userConstraintPtr )
{
serverCmd . m_userConstraintStateResultArgs . m_numDofs = 0 ;
for ( int i = 0 ; i < 6 ; i + + )
{
serverCmd . m_userConstraintStateResultArgs . m_appliedConstraintForces [ i ] = 0 ;
}
if ( userConstraintPtr - > m_mbConstraint )
{
serverCmd . m_userConstraintStateResultArgs . m_numDofs = userConstraintPtr - > m_mbConstraint - > getNumRows ( ) ;
for ( int i = 0 ; i < userConstraintPtr - > m_mbConstraint - > getNumRows ( ) ; i + + )
{
serverCmd . m_userConstraintStateResultArgs . m_appliedConstraintForces [ i ] = userConstraintPtr - > m_mbConstraint - > getAppliedImpulse ( i ) / m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_timeStep ;
2017-05-04 04:53:29 +00:00
}
2017-11-18 00:14:18 +00:00
serverCmd . m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED ;
}
}
} ;
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_REQUEST_INFO )
{
int userConstraintUidChange = clientCmd . m_userConstraintArguments . m_userConstraintUniqueId ;
InteralUserConstraintData * userConstraintPtr = m_data - > m_userConstraints . find ( userConstraintUidChange ) ;
if ( userConstraintPtr )
{
serverCmd . m_userConstraintResultArgs = userConstraintPtr - > m_userConstraintData ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED ;
}
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT )
{
btScalar defaultMaxForce = 500.0 ;
InternalBodyData * parentBody = m_data - > m_bodyHandles . getHandle ( clientCmd . m_userConstraintArguments . m_parentBodyIndex ) ;
if ( parentBody & & parentBody - > m_multiBody )
{
if ( ( clientCmd . m_userConstraintArguments . m_parentJointIndex > = - 1 ) & & clientCmd . m_userConstraintArguments . m_parentJointIndex < parentBody - > m_multiBody - > getNumLinks ( ) )
{
InternalBodyData * childBody = clientCmd . m_userConstraintArguments . m_childBodyIndex > = 0 ? m_data - > m_bodyHandles . getHandle ( clientCmd . m_userConstraintArguments . m_childBodyIndex ) : 0 ;
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{
btVector3 pivotInParent ( clientCmd . m_userConstraintArguments . m_parentFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 2 ] ) ;
btVector3 pivotInChild ( clientCmd . m_userConstraintArguments . m_childFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 2 ] ) ;
btMatrix3x3 frameInParent ( btQuaternion ( clientCmd . m_userConstraintArguments . m_parentFrame [ 3 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 4 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 5 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 6 ] ) ) ;
btMatrix3x3 frameInChild ( btQuaternion ( clientCmd . m_userConstraintArguments . m_childFrame [ 3 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 4 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 5 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 6 ] ) ) ;
btVector3 jointAxis ( clientCmd . m_userConstraintArguments . m_jointAxis [ 0 ] , clientCmd . m_userConstraintArguments . m_jointAxis [ 1 ] , clientCmd . m_userConstraintArguments . m_jointAxis [ 2 ] ) ;
2018-03-15 03:47:56 +00:00
2017-03-26 20:06:46 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_userConstraintArguments . m_jointType = = eGearType )
2017-10-19 02:15:35 +00:00
{
2017-11-18 00:14:18 +00:00
if ( childBody & & childBody - > m_multiBody )
2017-10-19 02:15:35 +00:00
{
2017-11-18 00:14:18 +00:00
if ( ( clientCmd . m_userConstraintArguments . m_childJointIndex > = - 1 ) & & ( clientCmd . m_userConstraintArguments . m_childJointIndex < childBody - > m_multiBody - > getNumLinks ( ) ) )
2017-10-19 02:15:35 +00:00
{
2017-11-18 00:14:18 +00:00
btMultiBodyGearConstraint * multibodyGear = new btMultiBodyGearConstraint ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_childJointIndex , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
multibodyGear - > setMaxAppliedImpulse ( defaultMaxForce ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( multibodyGear ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = multibodyGear ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2017-10-19 02:15:35 +00:00
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
else if ( clientCmd . m_userConstraintArguments . m_jointType = = eFixedType )
{
if ( childBody & & childBody - > m_multiBody )
{
if ( ( clientCmd . m_userConstraintArguments . m_childJointIndex > = - 1 ) & & ( clientCmd . m_userConstraintArguments . m_childJointIndex < childBody - > m_multiBody - > getNumLinks ( ) ) )
2017-10-19 02:15:35 +00:00
{
2017-11-18 00:14:18 +00:00
btMultiBodyFixedConstraint * multibodyFixed = new btMultiBodyFixedConstraint ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_childJointIndex , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
multibodyFixed - > setMaxAppliedImpulse ( defaultMaxForce ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( multibodyFixed ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = multibodyFixed ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2017-10-19 02:15:35 +00:00
}
2018-03-15 03:47:56 +00:00
2017-10-19 02:15:35 +00:00
}
2017-11-18 00:14:18 +00:00
else
2017-01-23 03:08:31 +00:00
{
2017-11-18 00:14:18 +00:00
btRigidBody * rb = childBody ? childBody - > m_rigidBody : 0 ;
btMultiBodyFixedConstraint * rigidbodyFixed = new btMultiBodyFixedConstraint ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , rb , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
rigidbodyFixed - > setMaxAppliedImpulse ( defaultMaxForce ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( rigidbodyFixed ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = rigidbodyFixed ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2017-01-23 03:08:31 +00:00
}
2018-03-15 03:47:56 +00:00
2017-01-23 03:08:31 +00:00
}
2017-11-18 00:14:18 +00:00
else if ( clientCmd . m_userConstraintArguments . m_jointType = = ePrismaticType )
2016-11-14 22:08:05 +00:00
{
2017-11-18 00:14:18 +00:00
if ( childBody & & childBody - > m_multiBody )
2016-11-14 22:08:05 +00:00
{
2017-11-18 00:14:18 +00:00
btMultiBodySliderConstraint * multibodySlider = new btMultiBodySliderConstraint ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_childJointIndex , pivotInParent , pivotInChild , frameInParent , frameInChild , jointAxis ) ;
multibodySlider - > setMaxAppliedImpulse ( defaultMaxForce ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( multibodySlider ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = multibodySlider ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
}
else
{
btRigidBody * rb = childBody ? childBody - > m_rigidBody : 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btMultiBodySliderConstraint * rigidbodySlider = new btMultiBodySliderConstraint ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , rb , pivotInParent , pivotInChild , frameInParent , frameInChild , jointAxis ) ;
rigidbodySlider - > setMaxAppliedImpulse ( defaultMaxForce ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( rigidbodySlider ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = rigidbodySlider ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ; }
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
} else if ( clientCmd . m_userConstraintArguments . m_jointType = = ePoint2PointType )
{
if ( childBody & & childBody - > m_multiBody )
{
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_childJointIndex , pivotInParent , pivotInChild ) ;
p2p - > setMaxAppliedImpulse ( defaultMaxForce ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( p2p ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = p2p ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
}
else
{
btRigidBody * rb = childBody ? childBody - > m_rigidBody : 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( parentBody - > m_multiBody , clientCmd . m_userConstraintArguments . m_parentJointIndex , rb , pivotInParent , pivotInChild ) ;
p2p - > setMaxAppliedImpulse ( defaultMaxForce ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( p2p ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_mbConstraint = p2p ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
} else
{
b3Warning ( " unknown constraint type " ) ;
}
2016-11-14 22:08:05 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
}
}
else
{
InternalBodyData * childBody = clientCmd . m_userConstraintArguments . m_childBodyIndex > = 0 ? m_data - > m_bodyHandles . getHandle ( clientCmd . m_userConstraintArguments . m_childBodyIndex ) : 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( parentBody & & childBody )
{
if ( parentBody - > m_rigidBody )
{
2017-08-30 16:54:38 +00:00
2017-11-18 00:14:18 +00:00
btRigidBody * parentRb = 0 ;
if ( clientCmd . m_userConstraintArguments . m_parentJointIndex = = - 1 )
{
parentRb = parentBody - > m_rigidBody ;
} else
{
if ( ( clientCmd . m_userConstraintArguments . m_parentJointIndex > = 0 ) & &
( clientCmd . m_userConstraintArguments . m_parentJointIndex < parentBody - > m_rigidBodyJoints . size ( ) ) )
{
parentRb = & parentBody - > m_rigidBodyJoints [ clientCmd . m_userConstraintArguments . m_parentJointIndex ] - > getRigidBodyB ( ) ;
}
}
2018-03-15 03:47:56 +00:00
2017-06-07 20:44:34 +00:00
2017-11-18 00:14:18 +00:00
btRigidBody * childRb = 0 ;
if ( childBody - > m_rigidBody )
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( clientCmd . m_userConstraintArguments . m_childJointIndex = = - 1 )
{
childRb = childBody - > m_rigidBody ;
}
else
{
if ( ( clientCmd . m_userConstraintArguments . m_childJointIndex > = 0 )
& & ( clientCmd . m_userConstraintArguments . m_childJointIndex < childBody - > m_rigidBodyJoints . size ( ) ) )
{
childRb = & childBody - > m_rigidBodyJoints [ clientCmd . m_userConstraintArguments . m_childJointIndex ] - > getRigidBodyB ( ) ;
2016-11-14 22:08:05 +00:00
}
2018-03-15 03:47:56 +00:00
2017-06-07 20:44:34 +00:00
}
2016-11-14 22:08:05 +00:00
}
2017-11-18 00:14:18 +00:00
switch ( clientCmd . m_userConstraintArguments . m_jointType )
2017-01-12 18:30:46 +00:00
{
2017-11-18 00:14:18 +00:00
case eRevoluteType :
{
break ;
}
case ePrismaticType :
{
break ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
case eFixedType :
2017-01-12 18:30:46 +00:00
{
2017-11-18 00:14:18 +00:00
if ( childRb & & parentRb & & ( childRb ! = parentRb ) )
2017-01-12 18:30:46 +00:00
{
2017-11-18 00:14:18 +00:00
btVector3 pivotInParent ( clientCmd . m_userConstraintArguments . m_parentFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 2 ] ) ;
btVector3 pivotInChild ( clientCmd . m_userConstraintArguments . m_childFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 2 ] ) ;
2017-09-27 02:54:36 +00:00
2017-11-18 00:14:18 +00:00
btTransform offsetTrA , offsetTrB ;
offsetTrA . setIdentity ( ) ;
offsetTrA . setOrigin ( pivotInParent ) ;
offsetTrB . setIdentity ( ) ;
offsetTrB . setOrigin ( pivotInChild ) ;
2017-06-07 23:22:02 +00:00
2017-11-18 00:14:18 +00:00
btGeneric6DofSpring2Constraint * dof6 = new btGeneric6DofSpring2Constraint ( * parentRb , * childRb , offsetTrA , offsetTrB ) ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
dof6 - > setLinearLowerLimit ( btVector3 ( 0 , 0 , 0 ) ) ;
dof6 - > setLinearUpperLimit ( btVector3 ( 0 , 0 , 0 ) ) ;
dof6 - > setAngularLowerLimit ( btVector3 ( 0 , 0 , 0 ) ) ;
dof6 - > setAngularUpperLimit ( btVector3 ( 0 , 0 , 0 ) ) ;
m_data - > m_dynamicsWorld - > addConstraint ( dof6 ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_rbConstraint = dof6 ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2017-01-12 18:30:46 +00:00
}
2017-06-07 23:22:02 +00:00
2017-11-18 00:14:18 +00:00
break ;
2017-01-12 18:30:46 +00:00
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
case ePoint2PointType :
2016-11-14 22:08:05 +00:00
{
2017-11-18 00:14:18 +00:00
if ( childRb & & parentRb & & ( childRb ! = parentRb ) )
2016-11-14 22:08:05 +00:00
{
2017-11-18 00:14:18 +00:00
btVector3 pivotInParent ( clientCmd . m_userConstraintArguments . m_parentFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_parentFrame [ 2 ] ) ;
btVector3 pivotInChild ( clientCmd . m_userConstraintArguments . m_childFrame [ 0 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 1 ] , clientCmd . m_userConstraintArguments . m_childFrame [ 2 ] ) ;
btPoint2PointConstraint * p2p = new btPoint2PointConstraint ( * parentRb , * childRb , pivotInParent , pivotInChild ) ;
p2p - > m_setting . m_impulseClamp = defaultMaxForce ;
m_data - > m_dynamicsWorld - > addConstraint ( p2p ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_rbConstraint = p2p ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2016-11-14 22:08:05 +00:00
}
2017-11-18 00:14:18 +00:00
break ;
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
case eGearType :
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
if ( childRb & & parentRb & & ( childRb ! = parentRb ) )
2016-11-14 22:08:05 +00:00
{
2017-11-18 00:14:18 +00:00
btVector3 axisA ( clientCmd . m_userConstraintArguments . m_jointAxis [ 0 ] ,
clientCmd . m_userConstraintArguments . m_jointAxis [ 1 ] ,
clientCmd . m_userConstraintArguments . m_jointAxis [ 2 ] ) ;
//for now we use the same local axis for both objects
btVector3 axisB ( clientCmd . m_userConstraintArguments . m_jointAxis [ 0 ] ,
clientCmd . m_userConstraintArguments . m_jointAxis [ 1 ] ,
clientCmd . m_userConstraintArguments . m_jointAxis [ 2 ] ) ;
btScalar ratio = 1 ;
btGearConstraint * gear = new btGearConstraint ( * parentRb , * childRb , axisA , axisB , ratio ) ;
m_data - > m_dynamicsWorld - > addConstraint ( gear , true ) ;
InteralUserConstraintData userConstraintData ;
userConstraintData . m_rbConstraint = gear ;
int uid = m_data - > m_userConstraintUIDGenerator + + ;
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = uid ;
serverCmd . m_userConstraintResultArgs . m_maxAppliedForce = defaultMaxForce ;
userConstraintData . m_userConstraintData = serverCmd . m_userConstraintResultArgs ;
m_data - > m_userConstraints . insert ( uid , userConstraintData ) ;
serverCmd . m_type = CMD_USER_CONSTRAINT_COMPLETED ;
2016-11-14 22:08:05 +00:00
}
2017-11-18 00:14:18 +00:00
break ;
}
case eSphericalType :
{
b3Warning ( " constraint type not handled yet " ) ;
break ;
}
case ePlanarType :
{
b3Warning ( " constraint type not handled yet " ) ;
break ;
}
default :
{
b3Warning ( " unknown constraint type " ) ;
}
} ;
}
}
}
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT )
{
serverCmd . m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED ;
int userConstraintUidChange = clientCmd . m_userConstraintArguments . m_userConstraintUniqueId ;
InteralUserConstraintData * userConstraintPtr = m_data - > m_userConstraints . find ( userConstraintUidChange ) ;
if ( userConstraintPtr )
{
if ( userConstraintPtr - > m_mbConstraint )
{
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B )
{
btVector3 pivotInB ( clientCmd . m_userConstraintArguments . m_childFrame [ 0 ] ,
clientCmd . m_userConstraintArguments . m_childFrame [ 1 ] ,
clientCmd . m_userConstraintArguments . m_childFrame [ 2 ] ) ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 0 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 0 ] ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 1 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 1 ] ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 2 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 2 ] ;
userConstraintPtr - > m_mbConstraint - > setPivotInB ( pivotInB ) ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B )
{
btQuaternion childFrameOrn ( clientCmd . m_userConstraintArguments . m_childFrame [ 3 ] ,
clientCmd . m_userConstraintArguments . m_childFrame [ 4 ] ,
clientCmd . m_userConstraintArguments . m_childFrame [ 5 ] ,
clientCmd . m_userConstraintArguments . m_childFrame [ 6 ] ) ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 3 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 3 ] ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 4 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 4 ] ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 5 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 5 ] ;
userConstraintPtr - > m_userConstraintData . m_childFrame [ 6 ] = clientCmd . m_userConstraintArguments . m_childFrame [ 6 ] ;
btMatrix3x3 childFrameBasis ( childFrameOrn ) ;
userConstraintPtr - > m_mbConstraint - > setFrameInB ( childFrameBasis ) ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE )
{
btScalar maxImp = clientCmd . m_userConstraintArguments . m_maxAppliedForce * m_data - > m_physicsDeltaTime ;
userConstraintPtr - > m_userConstraintData . m_maxAppliedForce = clientCmd . m_userConstraintArguments . m_maxAppliedForce ;
userConstraintPtr - > m_mbConstraint - > setMaxAppliedImpulse ( maxImp ) ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO )
{
userConstraintPtr - > m_mbConstraint - > setGearRatio ( clientCmd . m_userConstraintArguments . m_gearRatio ) ;
userConstraintPtr - > m_userConstraintData . m_gearRatio = clientCmd . m_userConstraintArguments . m_gearRatio ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET )
{
userConstraintPtr - > m_mbConstraint - > setRelativePositionTarget ( clientCmd . m_userConstraintArguments . m_relativePositionTarget ) ;
userConstraintPtr - > m_userConstraintData . m_relativePositionTarget = clientCmd . m_userConstraintArguments . m_relativePositionTarget ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_ERP )
{
userConstraintPtr - > m_mbConstraint - > setErp ( clientCmd . m_userConstraintArguments . m_erp ) ;
userConstraintPtr - > m_userConstraintData . m_erp = clientCmd . m_userConstraintArguments . m_erp ;
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK )
{
userConstraintPtr - > m_mbConstraint - > setGearAuxLink ( clientCmd . m_userConstraintArguments . m_gearAuxLink ) ;
userConstraintPtr - > m_userConstraintData . m_gearAuxLink = clientCmd . m_userConstraintArguments . m_gearAuxLink ;
}
}
if ( userConstraintPtr - > m_rbConstraint )
{
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE )
{
btScalar maxImp = clientCmd . m_userConstraintArguments . m_maxAppliedForce * m_data - > m_physicsDeltaTime ;
userConstraintPtr - > m_userConstraintData . m_maxAppliedForce = clientCmd . m_userConstraintArguments . m_maxAppliedForce ;
//userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp);
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO )
{
if ( userConstraintPtr - > m_rbConstraint - > getObjectType ( ) = = GEAR_CONSTRAINT_TYPE )
{
btGearConstraint * gear = ( btGearConstraint * ) userConstraintPtr - > m_rbConstraint ;
gear - > setRatio ( clientCmd . m_userConstraintArguments . m_gearRatio ) ;
}
}
}
serverCmd . m_userConstraintResultArgs = clientCmd . m_userConstraintArguments ;
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = userConstraintUidChange ;
serverCmd . m_updateFlags = clientCmd . m_updateFlags ;
serverCmd . m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED ;
}
}
if ( clientCmd . m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT )
{
serverCmd . m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED ;
int userConstraintUidRemove = clientCmd . m_userConstraintArguments . m_userConstraintUniqueId ;
InteralUserConstraintData * userConstraintPtr = m_data - > m_userConstraints . find ( userConstraintUidRemove ) ;
if ( userConstraintPtr )
{
if ( userConstraintPtr - > m_mbConstraint )
{
m_data - > m_dynamicsWorld - > removeMultiBodyConstraint ( userConstraintPtr - > m_mbConstraint ) ;
delete userConstraintPtr - > m_mbConstraint ;
m_data - > m_userConstraints . remove ( userConstraintUidRemove ) ;
}
if ( userConstraintPtr - > m_rbConstraint )
{
m_data - > m_dynamicsWorld - > removeConstraint ( userConstraintPtr - > m_rbConstraint ) ;
delete userConstraintPtr - > m_rbConstraint ;
2018-03-15 03:47:56 +00:00
m_data - > m_userConstraints . remove ( userConstraintUidRemove ) ;
2017-11-18 00:14:18 +00:00
}
serverCmd . m_userConstraintResultArgs . m_userConstraintUniqueId = userConstraintUidRemove ;
serverCmd . m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED ;
2017-01-23 03:08:31 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
2016-11-14 22:08:05 +00:00
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
}
return hasStatus ;
}
2016-09-08 22:15:58 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCalculateInverseKinematicsCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_CALCULATE_INVERSE_KINEMATICS " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( clientCmd . m_calculateInverseKinematicsArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
IKTrajectoryHelper * * ikHelperPtrPtr = m_data - > m_inverseKinematicsHelpers . find ( bodyHandle - > m_multiBody ) ;
IKTrajectoryHelper * ikHelperPtr = 0 ;
2018-03-15 03:47:56 +00:00
2016-09-08 22:15:58 +00:00
2017-11-18 00:14:18 +00:00
if ( ikHelperPtrPtr )
{
ikHelperPtr = * ikHelperPtrPtr ;
}
else
{
IKTrajectoryHelper * tmpHelper = new IKTrajectoryHelper ;
m_data - > m_inverseKinematicsHelpers . insert ( bodyHandle - > m_multiBody , tmpHelper ) ;
ikHelperPtr = tmpHelper ;
}
2016-09-08 22:15:58 +00:00
2017-11-18 00:14:18 +00:00
int endEffectorLinkIndex = clientCmd . m_calculateInverseKinematicsArguments . m_endEffectorLinkIndex ;
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
btAlignedObjectArray < double > startingPositions ;
startingPositions . reserve ( bodyHandle - > m_multiBody - > getNumLinks ( ) ) ;
btVector3 targetPosWorld ( clientCmd . m_calculateInverseKinematicsArguments . m_targetPosition [ 0 ] ,
clientCmd . m_calculateInverseKinematicsArguments . m_targetPosition [ 1 ] ,
clientCmd . m_calculateInverseKinematicsArguments . m_targetPosition [ 2 ] ) ;
btQuaternion targetOrnWorld ( clientCmd . m_calculateInverseKinematicsArguments . m_targetOrientation [ 0 ] ,
clientCmd . m_calculateInverseKinematicsArguments . m_targetOrientation [ 1 ] ,
clientCmd . m_calculateInverseKinematicsArguments . m_targetOrientation [ 2 ] ,
clientCmd . m_calculateInverseKinematicsArguments . m_targetOrientation [ 3 ] ) ;
btTransform targetBaseCoord ;
if ( clientCmd . m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
targetBaseCoord . setOrigin ( targetPosWorld ) ;
targetBaseCoord . setRotation ( targetOrnWorld ) ;
} else
{
btTransform targetWorld ;
targetWorld . setOrigin ( targetPosWorld ) ;
targetWorld . setRotation ( targetOrnWorld ) ;
btTransform tr = bodyHandle - > m_multiBody - > getBaseWorldTransform ( ) ;
targetBaseCoord = tr . inverse ( ) * targetWorld ;
}
2016-09-22 20:27:09 +00:00
2018-06-02 05:50:06 +00:00
{
int DofIndex = 0 ;
for ( int i = 0 ; i < bodyHandle - > m_multiBody - > getNumLinks ( ) ; + + i )
{
if ( bodyHandle - > m_multiBody - > getLink ( i ) . m_jointType > = 0 & & bodyHandle - > m_multiBody - > getLink ( i ) . m_jointType < = 2 )
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = 0 ;
if ( clientCmd . m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS )
{
curPos = clientCmd . m_calculateInverseKinematicsArguments . m_currentPositions [ DofIndex ] ;
} else
{
curPos = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
}
startingPositions . push_back ( curPos ) ;
DofIndex + + ;
}
}
}
2017-10-25 07:32:47 +00:00
2018-06-02 05:50:06 +00:00
int numIterations = 20 ;
if ( clientCmd . m_updateFlags & IK_HAS_MAX_ITERATIONS )
{
numIterations = clientCmd . m_calculateInverseKinematicsArguments . m_maxNumIterations ;
}
double residualThreshold = 1e-4 ;
if ( clientCmd . m_updateFlags & IK_HAS_RESIDUAL_THRESHOLD )
{
residualThreshold = clientCmd . m_calculateInverseKinematicsArguments . m_residualThreshold ;
}
btScalar currentDiff = 1e30 f ;
b3AlignedObjectArray < double > jacobian_linear ;
b3AlignedObjectArray < double > jacobian_angular ;
btAlignedObjectArray < double > q_current ;
btAlignedObjectArray < double > q_new ;
btAlignedObjectArray < double > lower_limit ;
btAlignedObjectArray < double > upper_limit ;
btAlignedObjectArray < double > joint_range ;
btAlignedObjectArray < double > rest_pose ;
const int numDofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
btInverseDynamics : : vecx nu ( numDofs + baseDofs ) , qdot ( numDofs + baseDofs ) , q ( numDofs + baseDofs ) , joint_force ( numDofs + baseDofs ) ;
for ( int i = 0 ; i < numIterations & & currentDiff > residualThreshold ; i + + )
{
BT_PROFILE ( " InverseKinematics1Step " ) ;
if ( ikHelperPtr & & ( endEffectorLinkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) ) )
{
jacobian_linear . resize ( 3 * numDofs ) ;
jacobian_angular . resize ( 3 * numDofs ) ;
int jacSize = 0 ;
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
q_current . resize ( numDofs ) ;
if ( tree & & ( ( numDofs + baseDofs ) = = tree - > numDoFs ( ) ) )
{
btInverseDynamics : : vec3 world_origin ;
btInverseDynamics : : mat33 world_rot ;
jacSize = jacobian_linear . size ( ) ;
// Set jacobian value
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
int DofIndex = 0 ;
for ( int i = 0 ; i < bodyHandle - > m_multiBody - > getNumLinks ( ) ; + + i )
{
if ( bodyHandle - > m_multiBody - > getLink ( i ) . m_jointType > = 0 & & bodyHandle - > m_multiBody - > getLink ( i ) . m_jointType < = 2 )
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = startingPositions [ DofIndex ] ;
q_current [ DofIndex ] = curPos ;
q [ DofIndex + baseDofs ] = curPos ;
qdot [ DofIndex + baseDofs ] = 0 ;
nu [ DofIndex + baseDofs ] = 0 ;
DofIndex + + ;
}
} // Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
{
BT_PROFILE ( " calculateInverseDynamics " ) ;
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
tree - > calculateJacobians ( q ) ;
btInverseDynamics : : mat3x jac_t ( 3 , numDofs + baseDofs ) ;
btInverseDynamics : : mat3x jac_r ( 3 , numDofs + baseDofs ) ;
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree - > getBodyJacobianTrans ( endEffectorLinkIndex + 1 , & jac_t ) ;
tree - > getBodyJacobianRot ( endEffectorLinkIndex + 1 , & jac_r ) ;
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree - > getBodyOrigin ( endEffectorLinkIndex + 1 , & world_origin ) ;
tree - > getBodyTransform ( endEffectorLinkIndex + 1 , & world_rot ) ;
for ( int i = 0 ; i < 3 ; + + i )
{
for ( int j = 0 ; j < numDofs ; + + j )
{
jacobian_linear [ i * numDofs + j ] = jac_t ( i , ( baseDofs + j ) ) ;
jacobian_angular [ i * numDofs + j ] = jac_r ( i , ( baseDofs + j ) ) ;
}
}
}
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
q_new . resize ( numDofs ) ;
int ikMethod = 0 ;
if ( ( clientCmd . m_updateFlags & IK_HAS_TARGET_ORIENTATION ) & & ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY ) )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE ;
2017-11-18 00:14:18 +00:00
}
2018-06-02 05:50:06 +00:00
else if ( clientCmd . m_updateFlags & IK_HAS_TARGET_ORIENTATION )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
if ( clientCmd . m_updateFlags & IK_SDLS )
{
ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION ;
}
else
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION ;
}
2017-11-18 00:14:18 +00:00
}
2018-06-02 05:50:06 +00:00
else if ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE ;
2017-11-18 00:14:18 +00:00
}
else
{
2018-06-02 05:50:06 +00:00
if ( clientCmd . m_updateFlags & IK_SDLS )
{
ikMethod = IK2_VEL_SDLS ;
}
else
{
ikMethod = IK2_VEL_DLS ; ;
}
2017-11-18 00:14:18 +00:00
}
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
if ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
lower_limit . resize ( numDofs ) ;
upper_limit . resize ( numDofs ) ;
joint_range . resize ( numDofs ) ;
rest_pose . resize ( numDofs ) ;
for ( int i = 0 ; i < numDofs ; + + i )
{
lower_limit [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_lowerLimit [ i ] ;
upper_limit [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_upperLimit [ i ] ;
joint_range [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_jointRange [ i ] ;
rest_pose [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_restPose [ i ] ;
}
{
BT_PROFILE ( " computeNullspaceVel " ) ;
ikHelperPtr - > computeNullspaceVel ( numDofs , & q_current [ 0 ] , & lower_limit [ 0 ] , & upper_limit [ 0 ] , & joint_range [ 0 ] , & rest_pose [ 0 ] ) ;
}
2017-11-18 00:14:18 +00:00
}
2018-06-02 05:50:06 +00:00
//btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
btVector3DoubleData endEffectorWorldPosition ;
btQuaternionDoubleData endEffectorWorldOrientation ;
//get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
btVector3 endEffectorPosWorldOrg = world_origin ;
btQuaternion endEffectorOriWorldOrg ;
world_rot . getRotation ( endEffectorOriWorldOrg ) ;
btTransform endEffectorBaseCoord ;
endEffectorBaseCoord . setOrigin ( endEffectorPosWorldOrg ) ;
endEffectorBaseCoord . setRotation ( endEffectorOriWorldOrg ) ;
//don't need the next two lines
//btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
//endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv;
2017-10-25 04:06:44 +00:00
2018-06-02 05:50:06 +00:00
//btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
btQuaternion endEffectorOriBaseCoord = endEffectorBaseCoord . getRotation ( ) ;
2017-10-25 04:06:44 +00:00
2018-06-02 05:50:06 +00:00
//btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
endEffectorBaseCoord . getOrigin ( ) . serializeDouble ( endEffectorWorldPosition ) ;
endEffectorBaseCoord . getRotation ( ) . serializeDouble ( endEffectorWorldOrientation ) ;
2018-03-15 03:47:56 +00:00
2018-06-02 05:50:06 +00:00
//diff
currentDiff = ( endEffectorBaseCoord . getOrigin ( ) - targetBaseCoord . getOrigin ( ) ) . length ( ) ;
btVector3DoubleData targetPosBaseCoord ;
btQuaternionDoubleData targetOrnBaseCoord ;
targetBaseCoord . getOrigin ( ) . serializeDouble ( targetPosBaseCoord ) ;
targetBaseCoord . getRotation ( ) . serializeDouble ( targetOrnBaseCoord ) ;
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray < double > joint_damping ;
joint_damping . resize ( numDofs , 0.5 ) ;
if ( clientCmd . m_updateFlags & IK_HAS_JOINT_DAMPING )
2017-11-18 00:14:18 +00:00
{
2018-06-02 05:50:06 +00:00
for ( int i = 0 ; i < numDofs ; + + i )
{
joint_damping [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_jointDamping [ i ] ;
}
}
ikHelperPtr - > setDampingCoeff ( numDofs , & joint_damping [ 0 ] ) ;
double targetDampCoeff [ 6 ] = { 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 } ;
{
BT_PROFILE ( " computeIK " ) ;
ikHelperPtr - > computeIK ( targetPosBaseCoord . m_floats , targetOrnBaseCoord . m_floats ,
endEffectorWorldPosition . m_floats , endEffectorWorldOrientation . m_floats ,
& q_current [ 0 ] ,
numDofs , clientCmd . m_calculateInverseKinematicsArguments . m_endEffectorLinkIndex ,
& q_new [ 0 ] , ikMethod , & jacobian_linear [ 0 ] , & jacobian_angular [ 0 ] , jacSize * 2 , targetDampCoeff ) ;
}
serverCmd . m_inverseKinematicsResultArgs . m_bodyUniqueId = clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ;
for ( int i = 0 ; i < numDofs ; i + + )
{
serverCmd . m_inverseKinematicsResultArgs . m_jointPositions [ i ] = q_new [ i ] ;
}
serverCmd . m_inverseKinematicsResultArgs . m_dofCount = numDofs ;
serverCmd . m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED ;
for ( int i = 0 ; i < numDofs ; i + + )
{
startingPositions [ i ] = q_new [ i ] ;
2017-11-18 00:14:18 +00:00
}
}
}
}
}
return hasStatus ;
}
2018-06-02 05:50:06 +00:00
2018-01-04 03:17:28 +00:00
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
// PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
// PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
// PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
// PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
int PhysicsServerCommandProcessor : : extractCollisionShapes ( const btCollisionShape * colShape , const btTransform & transform , b3CollisionShapeData * collisionShapeBuffer , int maxCollisionShapes )
{
if ( maxCollisionShapes < = 0 )
{
b3Warning ( " No space in buffer " ) ;
return 0 ;
}
int numConverted = 0 ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 0 ] = transform . getOrigin ( ) [ 0 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 1 ] = transform . getOrigin ( ) [ 1 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 2 ] = transform . getOrigin ( ) [ 2 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 3 ] = transform . getRotation ( ) [ 0 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 4 ] = transform . getRotation ( ) [ 1 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 5 ] = transform . getRotation ( ) [ 2 ] ;
collisionShapeBuffer [ 0 ] . m_localCollisionFrame [ 6 ] = transform . getRotation ( ) [ 3 ] ;
collisionShapeBuffer [ 0 ] . m_meshAssetFileName [ 0 ] = 0 ;
2017-11-18 00:14:18 +00:00
2018-01-04 03:17:28 +00:00
switch ( colShape - > getShapeType ( ) )
{
case CONVEX_HULL_SHAPE_PROXYTYPE :
{
2018-01-08 20:25:56 +00:00
UrdfCollision * urdfCol = m_data - > m_bulletCollisionShape2UrdfCollision . find ( colShape ) ;
2018-01-15 23:13:11 +00:00
if ( urdfCol & & ( urdfCol - > m_geometry . m_type = = URDF_GEOM_MESH ) )
2018-01-08 20:25:56 +00:00
{
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_MESH ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = urdfCol - > m_geometry . m_meshScale [ 0 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = urdfCol - > m_geometry . m_meshScale [ 1 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = urdfCol - > m_geometry . m_meshScale [ 2 ] ;
strcpy ( collisionShapeBuffer [ 0 ] . m_meshAssetFileName , urdfCol - > m_geometry . m_meshFileName . c_str ( ) ) ;
numConverted + = 1 ;
}
else
{
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_MESH ;
sprintf ( collisionShapeBuffer [ 0 ] . m_meshAssetFileName , " unknown_file " ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = 1 ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = 1 ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = 1 ;
numConverted + + ;
}
2018-03-15 03:47:56 +00:00
2018-01-08 20:25:56 +00:00
break ;
}
case CAPSULE_SHAPE_PROXYTYPE :
{
btCapsuleShapeZ * capsule = ( btCapsuleShapeZ * ) colShape ;
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_CAPSULE ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = 2. * capsule - > getHalfHeight ( ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = capsule - > getRadius ( ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = 0 ;
2018-01-04 03:17:28 +00:00
numConverted + + ;
break ;
}
case CYLINDER_SHAPE_PROXYTYPE :
{
btCylinderShapeZ * cyl = ( btCylinderShapeZ * ) colShape ;
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_CYLINDER ;
2018-03-15 03:47:56 +00:00
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = 2. * cyl - > getHalfExtentsWithMargin ( ) . getZ ( ) ;
2018-01-04 03:17:28 +00:00
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = cyl - > getHalfExtentsWithMargin ( ) . getX ( ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = 0 ;
numConverted + + ;
break ;
}
case BOX_SHAPE_PROXYTYPE :
{
btBoxShape * box = ( btBoxShape * ) colShape ;
btVector3 halfExtents = box - > getHalfExtentsWithMargin ( ) ;
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_BOX ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = 2. * halfExtents [ 0 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = 2. * halfExtents [ 1 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = 2. * halfExtents [ 2 ] ;
numConverted + + ;
break ;
}
case SPHERE_SHAPE_PROXYTYPE :
{
btSphereShape * sphere = ( btSphereShape * ) colShape ;
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_SPHERE ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = sphere - > getRadius ( ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = sphere - > getRadius ( ) ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = sphere - > getRadius ( ) ;
numConverted + + ;
break ;
}
case COMPOUND_SHAPE_PROXYTYPE :
{
2018-01-08 20:25:56 +00:00
//it could be a compound mesh from a wavefront OBJ, check it
UrdfCollision * urdfCol = m_data - > m_bulletCollisionShape2UrdfCollision . find ( colShape ) ;
2018-01-15 23:13:11 +00:00
if ( urdfCol & & ( urdfCol - > m_geometry . m_type = = URDF_GEOM_MESH ) )
2018-01-08 20:25:56 +00:00
{
collisionShapeBuffer [ 0 ] . m_collisionGeometryType = GEOM_MESH ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 0 ] = urdfCol - > m_geometry . m_meshScale [ 0 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 1 ] = urdfCol - > m_geometry . m_meshScale [ 1 ] ;
collisionShapeBuffer [ 0 ] . m_dimensions [ 2 ] = urdfCol - > m_geometry . m_meshScale [ 2 ] ;
strcpy ( collisionShapeBuffer [ 0 ] . m_meshAssetFileName , urdfCol - > m_geometry . m_meshFileName . c_str ( ) ) ;
numConverted + = 1 ;
}
else
2018-01-04 03:17:28 +00:00
{
2018-01-08 20:25:56 +00:00
//recurse, accumulate childTransform
btCompoundShape * compound = ( btCompoundShape * ) colShape ;
for ( int i = 0 ; i < compound - > getNumChildShapes ( ) ; i + + )
{
btTransform childTrans = transform * compound - > getChildTransform ( i ) ;
int remain = maxCollisionShapes - numConverted ;
int converted = extractCollisionShapes ( compound - > getChildShape ( i ) , childTrans , & collisionShapeBuffer [ numConverted ] , remain ) ;
numConverted + = converted ;
}
2018-01-04 03:17:28 +00:00
}
break ;
}
default :
{
b3Warning ( " Unexpected collision shape type in PhysicsServerCommandProcessor::extractCollisionShapes " ) ;
}
} ;
2018-03-15 03:47:56 +00:00
2018-01-04 03:17:28 +00:00
return numConverted ;
}
bool PhysicsServerCommandProcessor : : processRequestCollisionShapeInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_COLLISION_SHAPE_INFO " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_COLLISION_SHAPE_INFO_FAILED ;
int bodyUniqueId = clientCmd . m_requestCollisionShapeDataArguments . m_bodyUniqueId ;
int linkIndex = clientCmd . m_requestCollisionShapeDataArguments . m_linkIndex ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( bodyHandle )
{
if ( bodyHandle - > m_multiBody )
{
b3CollisionShapeData * collisionShapeStoragePtr = ( b3CollisionShapeData * ) bufferServerToClient ;
int totalBytesPerObject = sizeof ( b3CollisionShapeData ) ;
int maxNumColObjects = bufferSizeInBytes / totalBytesPerObject - 1 ;
btTransform childTrans ;
childTrans . setIdentity ( ) ;
serverCmd . m_sendCollisionShapeArgs . m_bodyUniqueId = bodyUniqueId ;
serverCmd . m_sendCollisionShapeArgs . m_linkIndex = linkIndex ;
if ( linkIndex = = - 1 )
{
if ( bodyHandle - > m_multiBody - > getBaseCollider ( ) )
{
//extract shape info from base collider
int numConvertedCollisionShapes = extractCollisionShapes ( bodyHandle - > m_multiBody - > getBaseCollider ( ) - > getCollisionShape ( ) , childTrans , collisionShapeStoragePtr , maxNumColObjects ) ;
serverCmd . m_sendCollisionShapeArgs . m_numCollisionShapes = numConvertedCollisionShapes ;
serverCmd . m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED ;
}
}
else
{
if ( linkIndex > = 0 & & linkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) & & bodyHandle - > m_multiBody - > getLinkCollider ( linkIndex ) )
{
int numConvertedCollisionShapes = extractCollisionShapes ( bodyHandle - > m_multiBody - > getLinkCollider ( linkIndex ) - > getCollisionShape ( ) , childTrans , collisionShapeStoragePtr , maxNumColObjects ) ;
serverCmd . m_sendCollisionShapeArgs . m_numCollisionShapes = numConvertedCollisionShapes ;
serverCmd . m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED ;
}
}
}
}
2017-11-18 00:14:18 +00:00
2018-01-04 03:17:28 +00:00
return hasStatus ;
}
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processRequestVisualShapeInfoCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_REQUEST_VISUAL_SHAPE_INFO " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_VISUAL_SHAPE_INFO_FAILED ;
//retrieve the visual shape information for a specific body
2018-03-15 03:47:56 +00:00
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
int totalNumVisualShapes = m_data - > m_pluginManager . getRenderInterface ( ) - > getNumVisualShapes ( clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ) ;
2018-01-17 01:58:19 +00:00
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
b3VisualShapeData * visualShapeStoragePtr = ( b3VisualShapeData * ) bufferServerToClient ;
int remain = totalNumVisualShapes - clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
int shapeIndex = clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
2018-01-17 20:48:48 +00:00
int success = m_data - > m_pluginManager . getRenderInterface ( ) - > getVisualShapesData ( clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ,
2018-01-17 01:58:19 +00:00
shapeIndex ,
visualShapeStoragePtr ) ;
if ( success ) {
2018-07-08 09:23:12 +00:00
//find the matching texture unique ids.
if ( visualShapeStoragePtr - > m_tinyRendererTextureId > = 0 )
{
b3AlignedObjectArray < int > usedHandles ;
m_data - > m_textureHandles . getUsedHandles ( usedHandles ) ;
for ( int i = 0 ; i < usedHandles . size ( ) ; i + + )
{
int texHandle = usedHandles [ i ] ;
InternalTextureHandle * texH = m_data - > m_textureHandles . getHandle ( texHandle ) ;
if ( texH & & ( texH - > m_tinyRendererTextureId = = visualShapeStoragePtr - > m_tinyRendererTextureId ) )
{
visualShapeStoragePtr - > m_openglTextureId = texH - > m_openglTextureId ;
visualShapeStoragePtr - > m_textureUniqueId = texHandle ;
}
}
}
2018-01-17 01:58:19 +00:00
serverCmd . m_sendVisualShapeArgs . m_numRemainingVisualShapes = remain - 1 ;
serverCmd . m_sendVisualShapeArgs . m_numVisualShapesCopied = 1 ;
serverCmd . m_sendVisualShapeArgs . m_startingVisualShapeIndex = clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
serverCmd . m_sendVisualShapeArgs . m_bodyUniqueId = clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ;
serverCmd . m_numDataStreamBytes = sizeof ( b3VisualShapeData ) * serverCmd . m_sendVisualShapeArgs . m_numVisualShapesCopied ;
serverCmd . m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED ;
}
2017-11-18 00:14:18 +00:00
}
return hasStatus ;
}
bool PhysicsServerCommandProcessor : : processUpdateVisualShapeCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
BT_PROFILE ( " CMD_UPDATE_VISUAL_SHAPE " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED ;
InternalTextureHandle * texHandle = 0 ;
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE )
2017-11-18 00:14:18 +00:00
{
texHandle = m_data - > m_textureHandles . getHandle ( clientCmd . m_updateVisualShapeDataArguments . m_textureUniqueId ) ;
if ( clientCmd . m_updateVisualShapeDataArguments . m_textureUniqueId > = 0 )
{
if ( texHandle )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-03-15 03:47:56 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > changeShapeTexture ( clientCmd . m_updateVisualShapeDataArguments . m_bodyUniqueId ,
clientCmd . m_updateVisualShapeDataArguments . m_jointIndex ,
clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex ,
2018-01-21 19:15:35 +00:00
texHandle - > m_tinyRendererTextureId ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
}
}
2018-03-15 03:47:56 +00:00
}
2017-11-18 00:14:18 +00:00
{
int bodyUniqueId = clientCmd . m_updateVisualShapeDataArguments . m_bodyUniqueId ;
int linkIndex = clientCmd . m_updateVisualShapeDataArguments . m_jointIndex ;
2017-06-30 20:35:07 +00:00
2017-11-18 00:14:18 +00:00
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( bodyHandle )
{
if ( bodyHandle - > m_multiBody )
{
if ( linkIndex = = - 1 )
{
if ( bodyHandle - > m_multiBody - > getBaseCollider ( ) )
{
int graphicsIndex = bodyHandle - > m_multiBody - > getBaseCollider ( ) - > getUserIndex ( ) ;
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE )
2017-05-13 16:18:36 +00:00
{
2017-06-30 20:35:07 +00:00
if ( texHandle )
{
2017-11-18 00:14:18 +00:00
int shapeIndex = m_data - > m_guiHelper - > getShapeIndexFromInstance ( graphicsIndex ) ;
m_data - > m_guiHelper - > replaceTexture ( shapeIndex , texHandle - > m_openglTextureId ) ;
2017-06-30 20:35:07 +00:00
}
2017-05-13 16:18:36 +00:00
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR )
2017-05-13 16:18:36 +00:00
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-21 19:15:35 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > changeRGBAColor ( bodyUniqueId , linkIndex ,
clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex ,
clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > changeRGBAColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
2017-05-13 16:18:36 +00:00
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR )
2017-11-18 00:14:18 +00:00
{
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > changeSpecularColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_specularColor ) ;
}
2018-03-15 03:47:56 +00:00
2017-05-13 16:18:36 +00:00
}
2017-11-18 00:14:18 +00:00
} else
2017-06-30 20:35:07 +00:00
{
2017-11-18 00:14:18 +00:00
if ( linkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) )
2017-06-30 20:35:07 +00:00
{
2017-11-18 00:14:18 +00:00
if ( bodyHandle - > m_multiBody - > getLink ( linkIndex ) . m_collider )
2017-06-30 20:35:07 +00:00
{
2017-11-18 00:14:18 +00:00
int graphicsIndex = bodyHandle - > m_multiBody - > getLink ( linkIndex ) . m_collider - > getUserIndex ( ) ;
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE )
2017-06-30 20:35:07 +00:00
{
2017-11-18 00:14:18 +00:00
if ( texHandle )
2017-06-30 20:35:07 +00:00
{
2017-11-18 00:14:18 +00:00
int shapeIndex = m_data - > m_guiHelper - > getShapeIndexFromInstance ( graphicsIndex ) ;
m_data - > m_guiHelper - > replaceTexture ( shapeIndex , texHandle - > m_openglTextureId ) ;
2017-06-30 20:35:07 +00:00
}
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR )
2017-11-18 00:14:18 +00:00
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-21 19:15:35 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > changeRGBAColor ( bodyUniqueId , linkIndex ,
2018-03-15 03:47:56 +00:00
clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex , clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > changeRGBAColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR )
2017-11-18 00:14:18 +00:00
{
m_data - > m_guiHelper - > changeSpecularColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_specularColor ) ;
}
2017-06-30 20:35:07 +00:00
}
}
}
2017-11-18 00:14:18 +00:00
} else
{
if ( bodyHandle - > m_rigidBody )
2016-11-12 02:07:42 +00:00
{
2017-11-18 00:14:18 +00:00
int graphicsIndex = bodyHandle - > m_rigidBody - > getUserIndex ( ) ;
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE )
2016-11-12 02:07:42 +00:00
{
2017-11-18 00:14:18 +00:00
if ( texHandle )
2016-11-12 02:07:42 +00:00
{
2017-11-18 00:14:18 +00:00
int shapeIndex = m_data - > m_guiHelper - > getShapeIndexFromInstance ( graphicsIndex ) ;
m_data - > m_guiHelper - > replaceTexture ( shapeIndex , texHandle - > m_openglTextureId ) ;
2016-11-12 02:07:42 +00:00
}
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR )
2016-11-12 02:07:42 +00:00
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-21 19:15:35 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > changeRGBAColor ( bodyUniqueId , linkIndex ,
2018-03-15 03:47:56 +00:00
clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex , clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > changeRGBAColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_rgbaColor ) ;
2016-11-12 02:07:42 +00:00
}
2018-03-15 03:47:56 +00:00
if ( clientCmd . m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR )
2016-11-12 02:07:42 +00:00
{
2017-11-18 00:14:18 +00:00
m_data - > m_guiHelper - > changeSpecularColor ( graphicsIndex , clientCmd . m_updateVisualShapeDataArguments . m_specularColor ) ;
2016-11-12 02:07:42 +00:00
}
}
2017-11-18 00:14:18 +00:00
}
}
}
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = VISUAL_SHAPE_CHANGED ;
notification . m_visualShapeArgs . m_bodyUniqueId = clientCmd . m_updateVisualShapeDataArguments . m_bodyUniqueId ;
notification . m_visualShapeArgs . m_linkIndex = clientCmd . m_updateVisualShapeDataArguments . m_jointIndex ;
notification . m_visualShapeArgs . m_visualShapeIndex = clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex ;
m_data - > m_pluginManager . addNotification ( notification ) ;
return hasStatus ;
2017-11-18 00:14:18 +00:00
}
2016-11-12 02:07:42 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processChangeTextureCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
InternalTextureHandle * texH = m_data - > m_textureHandles . getHandle ( clientCmd . m_changeTextureArgs . m_textureUniqueId ) ;
if ( texH )
{
int gltex = texH - > m_openglTextureId ;
m_data - > m_guiHelper - > changeTexture ( gltex ,
( const unsigned char * ) bufferServerToClient , clientCmd . m_changeTextureArgs . m_width , clientCmd . m_changeTextureArgs . m_height ) ;
2016-11-12 02:07:42 +00:00
2017-11-18 00:14:18 +00:00
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
}
return hasStatus ;
}
2017-05-10 22:01:25 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processLoadTextureCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_LOAD_TEXTURE " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_LOAD_TEXTURE_FAILED ;
2016-12-31 22:43:15 +00:00
2017-11-18 00:14:18 +00:00
char relativeFileName [ 1024 ] ;
char pathPrefix [ 1024 ] ;
2016-12-31 22:43:15 +00:00
2017-11-18 00:14:18 +00:00
if ( b3ResourcePath : : findResourcePath ( clientCmd . m_loadTextureArguments . m_textureFileName , relativeFileName , 1024 ) )
{
b3FileUtils : : extractPath ( relativeFileName , pathPrefix , 1024 ) ;
int texHandle = m_data - > m_textureHandles . allocHandle ( ) ;
InternalTextureHandle * texH = m_data - > m_textureHandles . getHandle ( texHandle ) ;
if ( texH )
{
texH - > m_tinyRendererTextureId = - 1 ;
texH - > m_openglTextureId = - 1 ;
2018-01-17 01:58:19 +00:00
int uid = - 1 ;
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-03-13 02:40:13 +00:00
uid = m_data - > m_pluginManager . getRenderInterface ( ) - > loadTextureFile ( relativeFileName ) ;
2018-01-17 01:58:19 +00:00
}
2017-11-18 00:14:18 +00:00
if ( uid > = 0 )
{
texH - > m_tinyRendererTextureId = uid ;
}
2016-12-31 22:43:15 +00:00
2017-11-18 00:14:18 +00:00
{
int width , height , n ;
unsigned char * imageData = stbi_load ( relativeFileName , & width , & height , & n , 3 ) ;
if ( imageData )
{
texH - > m_openglTextureId = m_data - > m_guiHelper - > registerTexture ( imageData , width , height ) ;
free ( imageData ) ;
2016-11-12 02:07:42 +00:00
}
2017-11-18 00:14:18 +00:00
else
{
b3Warning ( " Unsupported texture image format [%s] \n " , relativeFileName ) ;
}
}
serverCmd . m_loadTextureResultArguments . m_textureUniqueId = texHandle ;
serverCmd . m_type = CMD_LOAD_TEXTURE_COMPLETED ;
}
}
else
{
serverCmd . m_type = CMD_LOAD_TEXTURE_FAILED ;
}
return hasStatus ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-12-31 23:37:16 +00:00
bool PhysicsServerCommandProcessor : : processSaveStateCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
BT_PROFILE ( " CMD_RESTORE_STATE " ) ;
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_SAVE_STATE_FAILED ;
btDefaultSerializer * ser = new btDefaultSerializer ( ) ;
int currentFlags = ser - > getSerializationFlags ( ) ;
ser - > setSerializationFlags ( currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS ) ;
m_data - > m_dynamicsWorld - > serialize ( ser ) ;
bParse : : btBulletFile * bulletFile = new bParse : : btBulletFile ( ( char * ) ser - > getBufferPointer ( ) , ser - > getCurrentBufferSize ( ) ) ;
bulletFile - > parse ( false ) ;
if ( bulletFile - > ok ( ) )
{
serverCmd . m_type = CMD_SAVE_STATE_COMPLETED ;
serverCmd . m_saveStateResultArgs . m_stateId = m_data - > m_savedStates . size ( ) ;
SaveStateData sd ;
sd . m_bulletFile = bulletFile ;
sd . m_serializer = ser ;
m_data - > m_savedStates . push_back ( sd ) ;
}
return hasStatus ;
}
2017-12-28 18:05:51 +00:00
bool PhysicsServerCommandProcessor : : processRestoreStateCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
BT_PROFILE ( " CMD_RESTORE_STATE " ) ;
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RESTORE_STATE_FAILED ;
btMultiBodyWorldImporter * importer = new btMultiBodyWorldImporter ( m_data - > m_dynamicsWorld ) ;
importer - > setImporterFlags ( eRESTORE_EXISTING_OBJECTS ) ;
bool ok = false ;
if ( clientCmd . m_loadStateArguments . m_stateId > = 0 )
{
2017-12-31 23:37:16 +00:00
if ( clientCmd . m_loadStateArguments . m_stateId < m_data - > m_savedStates . size ( ) )
{
bParse : : btBulletFile * bulletFile = m_data - > m_savedStates [ clientCmd . m_loadStateArguments . m_stateId ] . m_bulletFile ;
ok = importer - > convertAllObjects ( bulletFile ) ;
}
2017-12-28 18:05:51 +00:00
}
else
{
const char * prefix [ ] = { " " , " ./ " , " ./data/ " , " ../data/ " , " ../../data/ " , " ../../../data/ " , " ../../../../data/ " } ;
int numPrefixes = sizeof ( prefix ) / sizeof ( const char * ) ;
char relativeFileName [ 1024 ] ;
FILE * f = 0 ;
bool found = false ;
for ( int i = 0 ; ! f & & i < numPrefixes ; i + + )
{
sprintf ( relativeFileName , " %s%s " , prefix [ i ] , clientCmd . m_fileArguments . m_fileName ) ;
f = fopen ( relativeFileName , " rb " ) ;
if ( f )
{
found = true ;
break ;
}
}
if ( f )
{
fclose ( f ) ;
}
if ( found )
{
ok = importer - > loadFile ( relativeFileName ) ;
}
}
if ( ok )
{
serverCmd . m_type = CMD_RESTORE_STATE_COMPLETED ;
2018-03-15 03:47:56 +00:00
}
2017-12-28 18:05:51 +00:00
return hasStatus ;
}
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processLoadBulletCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
BT_PROFILE ( " CMD_LOAD_BULLET " ) ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
bool hasStatus = true ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_BULLET_LOADING_FAILED ;
2018-03-15 03:47:56 +00:00
2017-11-23 02:12:02 +00:00
//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btMultiBodyWorldImporter * importer = new btMultiBodyWorldImporter ( m_data - > m_dynamicsWorld ) ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
const char * prefix [ ] = { " " , " ./ " , " ./data/ " , " ../data/ " , " ../../data/ " , " ../../../data/ " , " ../../../../data/ " } ;
int numPrefixes = sizeof ( prefix ) / sizeof ( const char * ) ;
char relativeFileName [ 1024 ] ;
FILE * f = 0 ;
bool found = false ;
2017-10-25 15:15:01 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; ! f & & i < numPrefixes ; i + + )
{
sprintf ( relativeFileName , " %s%s " , prefix [ i ] , clientCmd . m_fileArguments . m_fileName ) ;
f = fopen ( relativeFileName , " rb " ) ;
if ( f )
{
found = true ;
break ;
}
}
if ( f )
{
fclose ( f ) ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
if ( found )
{
bool ok = importer - > loadFile ( relativeFileName ) ;
if ( ok )
{
int numRb = importer - > getNumRigidBodies ( ) ;
serverStatusOut . m_sdfLoadedArgs . m_numBodies = 0 ;
serverStatusOut . m_sdfLoadedArgs . m_numUserConstraints = 0 ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
for ( int i = 0 ; i < numRb ; i + + )
{
btCollisionObject * colObj = importer - > getRigidBodyByIndex ( i ) ;
if ( colObj )
{
btRigidBody * rb = btRigidBody : : upcast ( colObj ) ;
if ( rb )
{
int bodyUniqueId = m_data - > m_bodyHandles . allocHandle ( ) ;
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
colObj - > setUserIndex2 ( bodyUniqueId ) ;
bodyHandle - > m_rigidBody = rb ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
if ( serverStatusOut . m_sdfLoadedArgs . m_numBodies < MAX_SDF_BODIES )
2017-01-17 23:42:32 +00:00
{
2017-11-18 00:14:18 +00:00
serverStatusOut . m_sdfLoadedArgs . m_numBodies + + ;
serverStatusOut . m_sdfLoadedArgs . m_bodyUniqueIds [ i ] = bodyUniqueId ;
2017-01-17 23:42:32 +00:00
}
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = BODY_ADDED ;
notification . m_bodyArgs . m_bodyUniqueId = bodyUniqueId ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2017-11-18 00:14:18 +00:00
}
}
}
serverCmd . m_type = CMD_BULLET_LOADING_COMPLETED ;
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( m_data - > m_dynamicsWorld ) ;
}
}
return hasStatus ;
}
2016-11-21 15:42:11 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processLoadMJCFCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
2016-11-21 15:42:11 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_LOAD_MJCF " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_MJCF_LOADING_FAILED ;
const MjcfArgs & mjcfArgs = clientCmd . m_mjcfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_MJCF:%s " , mjcfArgs . m_mjcfFileName ) ;
}
bool useMultiBody = ( clientCmd . m_updateFlags & URDF_ARGS_USE_MULTIBODY ) ? ( mjcfArgs . m_useMultiBody ! = 0 ) : true ;
int flags = CUF_USE_MJCF ;
if ( clientCmd . m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS )
{
flags | = clientCmd . m_mjcfArguments . m_flags ;
}
2016-11-21 15:42:11 +00:00
2017-11-18 00:14:18 +00:00
bool completedOk = loadMjcf ( mjcfArgs . m_mjcfFileName , bufferServerToClient , bufferSizeInBytes , useMultiBody , flags ) ;
if ( completedOk )
{
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_sdfLoadedArgs . m_numBodies = m_data - > m_sdfRecentLoadedBodies . size ( ) ;
serverStatusOut . m_sdfLoadedArgs . m_numUserConstraints = 0 ;
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 ] ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
serverStatusOut . m_type = CMD_MJCF_LOADING_COMPLETED ;
} else
{
serverStatusOut . m_type = CMD_MJCF_LOADING_FAILED ;
}
return hasStatus ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processSaveBulletCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
bool hasStatus = true ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
BT_PROFILE ( " CMD_SAVE_BULLET " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
2018-03-15 03:47:56 +00:00
2017-11-18 00:14:18 +00:00
FILE * f = fopen ( clientCmd . m_fileArguments . m_fileName , " wb " ) ;
if ( f )
{
btDefaultSerializer * ser = new btDefaultSerializer ( ) ;
2017-12-30 22:19:13 +00:00
int currentFlags = ser - > getSerializationFlags ( ) ;
ser - > setSerializationFlags ( currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS ) ;
2017-11-18 00:14:18 +00:00
m_data - > m_dynamicsWorld - > serialize ( ser ) ;
fwrite ( ser - > getBufferPointer ( ) , ser - > getCurrentBufferSize ( ) , 1 , f ) ;
fclose ( f ) ;
serverCmd . m_type = CMD_BULLET_SAVING_COMPLETED ;
delete ser ;
2017-11-23 02:12:02 +00:00
return hasStatus ;
2017-11-18 00:14:18 +00:00
}
serverCmd . m_type = CMD_BULLET_SAVING_FAILED ;
return hasStatus ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-11-18 00:14:18 +00:00
bool PhysicsServerCommandProcessor : : processCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
// BT_PROFILE("processCommand");
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
int sz = sizeof ( SharedMemoryStatus ) ;
int sz2 = sizeof ( SharedMemoryCommand ) ;
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
bool hasStatus = false ;
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
if ( m_data - > m_commandLogger )
{
m_data - > m_commandLogger - > logCommand ( clientCmd ) ;
}
serverStatusOut . m_type = CMD_INVALID_STATUS ;
serverStatusOut . m_numDataStreamBytes = 0 ;
serverStatusOut . m_dataStream = 0 ;
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
//consume the command
switch ( clientCmd . m_type )
{
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
case CMD_STATE_LOGGING :
{
hasStatus = processStateLoggingCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_SET_VR_CAMERA_STATE :
{
hasStatus = processSetVRCameraStateCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_VR_EVENTS_DATA :
{
hasStatus = processRequestVREventsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_REQUEST_MOUSE_EVENTS_DATA :
{
hasStatus = processRequestMouseEventsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_REQUEST_KEYBOARD_EVENTS_DATA :
{
hasStatus = processRequestKeyboardEventsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
2016-11-14 15:39:34 +00:00
2017-11-18 00:14:18 +00:00
case CMD_REQUEST_RAY_CAST_INTERSECTIONS :
{
2017-09-23 02:17:57 +00:00
2017-11-18 00:14:18 +00:00
hasStatus = processRequestRaycastIntersectionsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_REQUEST_DEBUG_LINES :
{
hasStatus = processRequestDebugLinesCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-10-03 22:00:52 +00:00
2017-11-18 00:14:18 +00:00
case CMD_REQUEST_CAMERA_IMAGE_DATA :
{
hasStatus = processRequestCameraImageCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_SYNC_BODY_INFO :
{
hasStatus = processSyncBodyInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_BODY_INFO :
{
hasStatus = processRequestBodyInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_SAVE_WORLD :
{
hasStatus = processSaveWorldCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_LOAD_SDF :
{
hasStatus = processLoadSDFCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CREATE_COLLISION_SHAPE :
{
hasStatus = processCreateCollisionShapeCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CREATE_VISUAL_SHAPE :
{
hasStatus = processCreateVisualShapeCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CREATE_MULTI_BODY :
{
hasStatus = processCreateMultiBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_SET_ADDITIONAL_SEARCH_PATH :
{
hasStatus = processSetAdditionalSearchPathCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_LOAD_URDF :
{
hasStatus = processLoadURDFCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2018-01-08 03:24:37 +00:00
case CMD_LOAD_SOFT_BODY :
2017-11-18 00:14:18 +00:00
{
2018-01-08 03:24:37 +00:00
hasStatus = processLoadSoftBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
2017-11-18 00:14:18 +00:00
break ;
}
case CMD_CREATE_SENSOR :
{
hasStatus = processCreateSensorCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_PROFILE_TIMING :
{
hasStatus = processProfileTimingCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-09-23 02:17:57 +00:00
2017-11-18 00:14:18 +00:00
case CMD_SEND_DESIRED_STATE :
{
hasStatus = processSendDesiredStateCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_COLLISION_INFO :
{
hasStatus = processRequestCollisionInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-09-23 02:17:57 +00:00
2017-11-18 00:14:18 +00:00
case CMD_REQUEST_ACTUAL_STATE :
{
hasStatus = processRequestActualStateCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_STEP_FORWARD_SIMULATION :
{
hasStatus = processForwardDynamicsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2016-06-24 14:31:17 +00:00
2017-11-18 00:14:18 +00:00
case CMD_REQUEST_INTERNAL_DATA :
{
hasStatus = processRequestInternalDataCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_CHANGE_DYNAMICS_INFO :
{
hasStatus = processChangeDynamicsInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_GET_DYNAMICS_INFO :
{
hasStatus = processGetDynamicsInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS :
{
hasStatus = processRequestPhysicsSimulationParametersCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2015-11-23 04:50:32 +00:00
2017-11-18 00:14:18 +00:00
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
hasStatus = processSendPhysicsParametersCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
} ;
case CMD_INIT_POSE :
{
hasStatus = processInitPoseCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_RESET_SIMULATION :
{
hasStatus = processResetSimulationCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CREATE_RIGID_BODY :
{
hasStatus = processCreateRigidBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CREATE_BOX_COLLISION_SHAPE :
{
//for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY
hasStatus = processCreateRigidBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_PICK_BODY :
{
hasStatus = processPickBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_MOVE_PICKED_BODY :
{
hasStatus = processMovePickedBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY :
{
hasStatus = processRemovePickingConstraintCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_AABB_OVERLAP :
{
hasStatus = processRequestAabbOverlapCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA :
{
hasStatus = processRequestOpenGLVisualizeCameraCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CONFIGURE_OPENGL_VISUALIZER :
{
hasStatus = processConfigureOpenGLVisualizerCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION :
{
hasStatus = processRequestContactpointInformationCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CALCULATE_INVERSE_DYNAMICS :
{
hasStatus = processInverseDynamicsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CALCULATE_JACOBIAN :
{
hasStatus = processCalculateJacobianCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CALCULATE_MASS_MATRIX :
{
hasStatus = processCalculateMassMatrixCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_APPLY_EXTERNAL_FORCE :
{
hasStatus = processApplyExternalForceCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REMOVE_BODY :
{
hasStatus = processRemoveBodyCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_USER_CONSTRAINT :
{
hasStatus = processCreateUserConstraintCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CALCULATE_INVERSE_KINEMATICS :
{
hasStatus = processCalculateInverseKinematicsCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO :
{
hasStatus = processRequestVisualShapeInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2018-01-04 03:17:28 +00:00
case CMD_REQUEST_COLLISION_SHAPE_INFO :
{
hasStatus = processRequestCollisionShapeInfoCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-11-18 00:14:18 +00:00
case CMD_UPDATE_VISUAL_SHAPE :
{
hasStatus = processUpdateVisualShapeCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_CHANGE_TEXTURE :
{
hasStatus = processChangeTextureCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_LOAD_TEXTURE :
{
hasStatus = processLoadTextureCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-12-28 18:05:51 +00:00
case CMD_RESTORE_STATE :
{
hasStatus = processRestoreStateCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-12-31 23:37:16 +00:00
case CMD_SAVE_STATE :
{
hasStatus = processSaveStateCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-11-18 00:14:18 +00:00
case CMD_LOAD_BULLET :
{
hasStatus = processLoadBulletCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_SAVE_BULLET :
{
hasStatus = processSaveBulletCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_LOAD_MJCF :
{
hasStatus = processLoadMJCFCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_USER_DEBUG_DRAW :
{
hasStatus = processUserDebugDrawCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
2018-03-15 03:47:56 +00:00
break ;
2017-11-18 00:14:18 +00:00
}
case CMD_CUSTOM_COMMAND :
{
hasStatus = processCustomCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2018-06-02 20:40:08 +00:00
case CMD_SYNC_USER_DATA :
{
hasStatus = processSyncUserDataCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REQUEST_USER_DATA :
{
hasStatus = processRequestUserDataCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_ADD_USER_DATA :
{
hasStatus = processAddUserDataCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
case CMD_REMOVE_USER_DATA :
{
hasStatus = processRemoveUserDataCommand ( clientCmd , serverStatusOut , bufferServerToClient , bufferSizeInBytes ) ;
break ;
}
2017-11-18 00:14:18 +00:00
default :
{
BT_PROFILE ( " CMD_UNKNOWN " ) ;
b3Error ( " Unknown command encountered " ) ;
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 ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
void PhysicsServerCommandProcessor : : syncPhysicsToGraphics ( )
{
m_data - > m_guiHelper - > syncPhysicsToGraphics ( m_data - > m_dynamicsWorld ) ;
}
2017-05-16 19:19:03 +00:00
void PhysicsServerCommandProcessor : : renderScene ( int renderFlags )
2015-11-23 04:50:32 +00:00
{
if ( m_data - > m_guiHelper )
{
2018-03-15 03:47:56 +00:00
if ( 0 = = ( renderFlags & COV_DISABLE_SYNC_RENDERING ) )
{
m_data - > m_guiHelper - > syncPhysicsToGraphics ( m_data - > m_dynamicsWorld ) ;
2017-05-24 06:34:55 +00:00
}
2015-11-23 04:50:32 +00:00
m_data - > m_guiHelper - > render ( m_data - > m_dynamicsWorld ) ;
}
2018-01-10 06:47:56 +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 ( ) ;
2018-01-10 06:47:56 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) . size ( ) ; i + + )
{
btSoftBody * psb = ( btSoftBody * ) m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) [ i ] ;
if ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) & & ! ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) - > getDebugMode ( ) & ( btIDebugDraw : : DBG_DrawWireframe ) ) )
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers : : Draw ( psb , m_data - > m_dynamicsWorld - > getDebugDrawer ( ) , m_data - > m_dynamicsWorld - > getDrawFlags ( ) ) ;
}
}
# endif
2015-11-23 04:50:32 +00:00
}
}
}
2016-09-15 23:57:00 +00:00
2018-01-15 16:36:08 +00:00
struct MyResultCallback : public btCollisionWorld : : ClosestRayResultCallback
{
MyResultCallback ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
: btCollisionWorld : : ClosestRayResultCallback ( rayFromWorld , rayToWorld )
{
}
2018-03-15 03:47:56 +00:00
2018-01-15 16:36:08 +00:00
virtual bool needsCollision ( btBroadphaseProxy * proxy0 ) const
{
return true ;
}
} ;
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : pickBody ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
{
2016-11-17 00:12:59 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_dynamicsWorld = = 0 )
return false ;
2018-01-15 16:36:08 +00:00
//btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
MyResultCallback rayCallback ( rayFromWorld , rayToWorld ) ;
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > rayTest ( rayFromWorld , rayToWorld , rayCallback ) ;
if ( rayCallback . hasHit ( ) )
{
btVector3 pickPos = rayCallback . m_hitPointWorld ;
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
btRigidBody * body = ( btRigidBody * ) btRigidBody : : upcast ( rayCallback . m_collisionObject ) ;
if ( body )
{
//other exclusions?
if ( ! ( body - > isStaticObject ( ) | | body - > isKinematicObject ( ) ) )
{
m_data - > m_pickedBody = body ;
2017-03-20 17:58:07 +00:00
m_data - > m_savedActivationState = body - > getActivationState ( ) ;
2015-11-23 04:50:32 +00:00
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 ;
}
2018-03-15 03:47:56 +00:00
2015-11-23 04:50:32 +00:00
} else
{
btMultiBodyLinkCollider * multiCol = ( btMultiBodyLinkCollider * ) btMultiBodyLinkCollider : : upcast ( rayCallback . m_collisionObject ) ;
2017-01-17 23:42:32 +00:00
if ( multiCol & & multiCol - > m_multiBody )
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
2018-06-02 05:50:06 +00:00
m_data - > m_prevCanSleep = multiCol - > m_multiBody - > getCanSleep ( ) ;
multiCol - > m_multiBody - > setCanSleep ( false ) ;
2015-11-23 04:50:32 +00:00
2018-06-02 05:50:06 +00:00
btVector3 pivotInA = multiCol - > m_multiBody - > worldPosToLocal ( multiCol - > m_link , pickPos ) ;
2016-06-24 14:31:17 +00:00
2018-06-02 05:50:06 +00:00
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 = 10 ;
p2p - > setMaxAppliedImpulse ( 2 * scaling ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( p2p ) ;
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 ;
2017-03-20 17:58:07 +00:00
m_data - > m_pickedBody - > forceActivationState ( m_data - > m_savedActivationState ) ;
2015-11-23 04:50:32 +00:00
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-09-23 02:48:26 +00:00
2016-10-04 15:53:59 +00:00
2017-05-31 02:54:55 +00:00
2016-09-09 21:30:37 +00:00
2016-09-09 18:28:38 +00:00
int gDroppedSimulationSteps = 0 ;
int gNumSteps = 0 ;
double gDtInSec = 0.f ;
double gSubStep = 0.f ;
2016-12-11 17:28:36 +00:00
void PhysicsServerCommandProcessor : : enableRealTimeSimulation ( bool enableRealTimeSim )
{
2017-10-05 18:43:14 +00:00
m_data - > m_useRealTimeSimulation = enableRealTimeSim ;
2016-12-11 17:28:36 +00:00
}
2017-05-31 02:54:55 +00:00
bool PhysicsServerCommandProcessor : : isRealTimeSimulationEnabled ( ) const
{
2017-10-05 18:43:14 +00:00
return m_data - > m_useRealTimeSimulation ;
2017-05-31 02:54:55 +00:00
}
2017-06-17 20:29:14 +00:00
void PhysicsServerCommandProcessor : : stepSimulationRealTime ( double dtInSec , const struct b3VRControllerEvent * vrControllerEvents , int numVRControllerEvents , const struct b3KeyboardEvent * keyEvents , int numKeyEvents , const struct b3MouseEvent * mouseEvents , int numMouseEvents )
2016-07-18 06:50:11 +00:00
{
2017-04-08 05:53:36 +00:00
m_data - > m_vrControllerEvents . addNewVREvents ( vrControllerEvents , numVRControllerEvents ) ;
2017-10-03 22:00:52 +00:00
m_data - > m_pluginManager . addEvents ( vrControllerEvents , numVRControllerEvents , keyEvents , numKeyEvents , mouseEvents , numMouseEvents ) ;
2017-09-24 01:05:23 +00:00
2017-03-29 23:29:34 +00:00
for ( int i = 0 ; i < m_data - > m_stateLoggers . size ( ) ; i + + )
2016-12-27 03:40:09 +00:00
{
2017-03-29 23:29:34 +00:00
if ( m_data - > m_stateLoggers [ i ] - > m_loggingType = = STATE_LOGGING_VR_CONTROLLERS )
2016-12-27 03:40:09 +00:00
{
2017-03-29 23:29:34 +00:00
VRControllerStateLogger * vrLogger = ( VRControllerStateLogger * ) m_data - > m_stateLoggers [ i ] ;
2017-04-08 05:53:36 +00:00
vrLogger - > m_vrEvents . addNewVREvents ( vrControllerEvents , numVRControllerEvents ) ;
2016-12-27 03:40:09 +00:00
}
}
2017-06-17 20:29:14 +00:00
for ( int ii = 0 ; ii < numMouseEvents ; ii + + )
{
const b3MouseEvent & event = mouseEvents [ ii ] ;
bool found = false ;
//search a matching one first, otherwise add new event
for ( int e = 0 ; e < m_data - > m_mouseEvents . size ( ) ; e + + )
{
if ( event . m_eventType = = m_data - > m_mouseEvents [ e ] . m_eventType )
{
if ( event . m_eventType = = MOUSE_MOVE_EVENT )
{
m_data - > m_mouseEvents [ e ] . m_mousePosX = event . m_mousePosX ;
m_data - > m_mouseEvents [ e ] . m_mousePosY = event . m_mousePosY ;
found = true ;
} else
if ( ( event . m_eventType = = MOUSE_BUTTON_EVENT ) & & event . m_buttonIndex = = m_data - > m_mouseEvents [ e ] . m_buttonIndex )
{
m_data - > m_mouseEvents [ e ] . m_buttonState | = event . m_buttonState ;
if ( event . m_buttonState & eButtonIsDown )
{
m_data - > m_mouseEvents [ e ] . m_buttonState | = eButtonIsDown ;
} else
{
m_data - > m_mouseEvents [ e ] . m_buttonState & = ~ eButtonIsDown ;
}
found = true ;
}
}
2018-03-15 03:47:56 +00:00
}
2017-06-17 20:29:14 +00:00
if ( ! found )
{
m_data - > m_mouseEvents . push_back ( event ) ;
}
}
2017-03-02 20:33:22 +00:00
for ( int i = 0 ; i < numKeyEvents ; i + + )
{
const b3KeyboardEvent & event = keyEvents [ i ] ;
bool found = false ;
//search a matching one first, otherwise add new event
for ( int e = 0 ; e < m_data - > m_keyboardEvents . size ( ) ; e + + )
{
if ( event . m_keyCode = = m_data - > m_keyboardEvents [ e ] . m_keyCode )
{
m_data - > m_keyboardEvents [ e ] . m_keyState | = event . m_keyState ;
if ( event . m_keyState & eButtonIsDown )
{
m_data - > m_keyboardEvents [ e ] . m_keyState | = eButtonIsDown ;
} else
{
m_data - > m_keyboardEvents [ e ] . m_keyState & = ~ eButtonIsDown ;
}
found = true ;
}
}
if ( ! found )
{
m_data - > m_keyboardEvents . push_back ( event ) ;
}
}
2016-11-30 01:08:47 +00:00
if ( gResetSimulation )
{
resetSimulation ( ) ;
gResetSimulation = false ;
}
2017-05-17 23:29:30 +00:00
if ( gVRTrackingObjectUniqueId > = 0 )
{
InternalBodyHandle * bodyHandle = m_data - > m_bodyHandles . getHandle ( gVRTrackingObjectUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
2017-05-18 02:29:12 +00:00
// gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform();
if ( gVRTrackingObjectUniqueId > = 0 )
{
gVRTrackingObjectTr . setOrigin ( bodyHandle - > m_multiBody - > getBaseWorldTransform ( ) . getOrigin ( ) ) ;
2017-05-31 02:54:55 +00:00
gVRTeleportPos1 = gVRTrackingObjectTr . getOrigin ( ) ;
2017-05-18 02:29:12 +00:00
}
if ( gVRTrackingObjectFlag & VR_CAMERA_TRACK_OBJECT_ORIENTATION )
{
gVRTrackingObjectTr . setBasis ( bodyHandle - > m_multiBody - > getBaseWorldTransform ( ) . getBasis ( ) ) ;
2017-05-31 02:54:55 +00:00
gVRTeleportOrn = gVRTrackingObjectTr . getRotation ( ) ;
2018-03-15 03:47:56 +00:00
}
2017-05-31 02:54:55 +00:00
2017-05-17 23:29:30 +00:00
}
}
2017-10-05 18:43:14 +00:00
if ( ( m_data - > m_useRealTimeSimulation ) & & m_data - > m_guiHelper )
2016-07-18 06:50:11 +00:00
{
2018-03-15 03:47:56 +00:00
2016-10-23 14:14:50 +00:00
int maxSteps = m_data - > m_numSimulationSubSteps + 3 ;
if ( m_data - > m_numSimulationSubSteps )
2016-07-18 06:50:11 +00:00
{
2016-10-23 14:14:50 +00:00
gSubStep = m_data - > m_physicsDeltaTime / m_data - > m_numSimulationSubSteps ;
}
else
{
gSubStep = m_data - > m_physicsDeltaTime ;
}
2018-03-15 03:47:56 +00:00
2016-11-17 00:12:59 +00:00
2016-10-23 14:14:50 +00:00
int numSteps = m_data - > m_dynamicsWorld - > stepSimulation ( dtInSec * simTimeScalingFactor , maxSteps , gSubStep ) ;
gDroppedSimulationSteps + = numSteps > maxSteps ? numSteps - maxSteps : 0 ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
if ( numSteps )
{
gNumSteps = numSteps ;
gDtInSec = dtInSec ;
2018-07-24 12:12:16 +00:00
addTransformChangedNotifications ( ) ;
2016-10-23 14:14:50 +00:00
}
}
}
2016-07-18 06:50:11 +00:00
2018-07-24 12:12:16 +00:00
b3Notification createTransformChangedNotification ( int bodyUniqueId , int linkIndex , const btCollisionObject * colObj )
{
b3Notification notification ;
notification . m_notificationType = TRANSFORM_CHANGED ;
notification . m_transformChangeArgs . m_bodyUniqueId = bodyUniqueId ;
notification . m_transformChangeArgs . m_linkIndex = linkIndex ;
const btTransform & tr = colObj - > getWorldTransform ( ) ;
notification . m_transformChangeArgs . m_worldPosition [ 0 ] = tr . getOrigin ( ) [ 0 ] ;
notification . m_transformChangeArgs . m_worldPosition [ 1 ] = tr . getOrigin ( ) [ 1 ] ;
notification . m_transformChangeArgs . m_worldPosition [ 2 ] = tr . getOrigin ( ) [ 2 ] ;
notification . m_transformChangeArgs . m_worldRotation [ 0 ] = tr . getRotation ( ) [ 0 ] ;
notification . m_transformChangeArgs . m_worldRotation [ 1 ] = tr . getRotation ( ) [ 1 ] ;
notification . m_transformChangeArgs . m_worldRotation [ 2 ] = tr . getRotation ( ) [ 2 ] ;
notification . m_transformChangeArgs . m_worldRotation [ 3 ] = tr . getRotation ( ) [ 3 ] ;
const btVector3 & scaling = colObj - > getCollisionShape ( ) - > getLocalScaling ( ) ;
notification . m_transformChangeArgs . m_localScaling [ 0 ] = scaling [ 0 ] ;
notification . m_transformChangeArgs . m_localScaling [ 1 ] = scaling [ 1 ] ;
notification . m_transformChangeArgs . m_localScaling [ 2 ] = scaling [ 2 ] ;
return notification ;
}
void PhysicsServerCommandProcessor : : addTransformChangedNotifications ( )
{
2018-08-01 15:49:58 +00:00
b3Notification notification ;
notification . m_notificationType = SIMULATION_STEPPED ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2018-07-24 12:12:16 +00:00
b3AlignedObjectArray < int > usedHandles ;
m_data - > m_bodyHandles . getUsedHandles ( usedHandles ) ;
for ( int i = 0 ; i < usedHandles . size ( ) ; i + + ) {
int bodyUniqueId = usedHandles [ i ] ;
InternalBodyData * bodyData = m_data - > m_bodyHandles . getHandle ( bodyUniqueId ) ;
if ( ! bodyData ) {
continue ;
}
if ( bodyData - > m_multiBody & & bodyData - > m_multiBody - > isAwake ( ) ) {
btMultiBody * mb = bodyData - > m_multiBody ;
m_data - > m_pluginManager . addNotification ( createTransformChangedNotification ( bodyUniqueId , - 1 , mb - > getBaseCollider ( ) ) ) ;
2017-05-03 17:49:04 +00:00
2018-07-24 12:12:16 +00:00
for ( int linkIndex = 0 ; linkIndex < mb - > getNumLinks ( ) ; linkIndex + + ) {
m_data - > m_pluginManager . addNotification ( createTransformChangedNotification ( bodyUniqueId , linkIndex , mb - > getLinkCollider ( linkIndex ) ) ) ;
}
}
else if ( bodyData - > m_rigidBody & & bodyData - > m_rigidBody - > isActive ( ) ) {
m_data - > m_pluginManager . addNotification ( createTransformChangedNotification ( bodyUniqueId , - 1 , bodyData - > m_rigidBody ) ) ;
}
}
}
2016-10-09 01:40:09 +00:00
2016-11-30 01:08:47 +00:00
void PhysicsServerCommandProcessor : : resetSimulation ( )
{
//clean up all data
2018-05-02 22:39:16 +00:00
2018-05-23 03:26:00 +00:00
m_data - > m_cachedVUrdfisualShapes . clear ( ) ;
2018-05-02 22:39:16 +00:00
# ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2018-04-30 18:01:53 +00:00
if ( m_data & & m_data - > m_dynamicsWorld )
{
m_data - > m_dynamicsWorld - > getWorldInfo ( ) . m_sparsesdf . Reset ( ) ;
}
2018-05-02 22:39:16 +00:00
# endif
2016-11-30 01:08:47 +00:00
if ( m_data & & m_data - > m_guiHelper )
{
m_data - > m_guiHelper - > removeAllGraphicsInstances ( ) ;
2017-04-12 22:02:47 +00:00
m_data - > m_guiHelper - > removeAllUserDebugItems ( ) ;
2016-11-30 01:08:47 +00:00
}
if ( m_data )
{
2018-01-17 20:48:48 +00:00
if ( m_data - > m_pluginManager . getRenderInterface ( ) )
2018-01-17 01:58:19 +00:00
{
2018-01-17 20:48:48 +00:00
m_data - > m_pluginManager . getRenderInterface ( ) - > resetAll ( ) ;
2018-01-17 01:58:19 +00:00
}
2017-12-31 23:37:16 +00:00
for ( int i = 0 ; i < m_data - > m_savedStates . size ( ) ; i + + )
{
delete m_data - > m_savedStates [ i ] . m_bulletFile ;
delete m_data - > m_savedStates [ i ] . m_serializer ;
}
m_data - > m_savedStates . clear ( ) ;
2016-11-30 01:08:47 +00:00
}
2017-01-17 23:42:32 +00:00
removePickingConstraint ( ) ;
2016-11-30 01:08:47 +00:00
deleteDynamicsWorld ( ) ;
createEmptyDynamicsWorld ( ) ;
2017-05-03 17:49:04 +00:00
m_data - > m_bodyHandles . exitHandles ( ) ;
m_data - > m_bodyHandles . initHandles ( ) ;
2016-11-30 01:08:47 +00:00
2017-06-03 17:57:56 +00:00
m_data - > m_userCollisionShapeHandles . exitHandles ( ) ;
m_data - > m_userCollisionShapeHandles . initHandles ( ) ;
2016-11-30 01:08:47 +00:00
2018-07-03 15:45:19 +00:00
m_data - > m_userDataHandles . exitHandles ( ) ;
m_data - > m_userDataHandles . initHandles ( ) ;
m_data - > m_userDataHandleLookup . clear ( ) ;
2018-07-24 12:12:16 +00:00
b3Notification notification ;
notification . m_notificationType = SIMULATION_RESET ;
m_data - > m_pluginManager . addNotification ( notification ) ;
2016-11-30 01:08:47 +00:00
}
2017-01-23 03:08:31 +00:00
2016-10-23 14:14:50 +00:00
2017-05-31 02:54:55 +00:00
void PhysicsServerCommandProcessor : : setTimeOut ( double /*timeOutInSeconds*/ )
{
2016-11-01 22:46:09 +00:00
}
2017-02-24 23:34:11 +00:00
2017-05-31 02:54:55 +00:00
const btVector3 & PhysicsServerCommandProcessor : : getVRTeleportPosition ( ) const
{
return gVRTeleportPos1 ;
}
void PhysicsServerCommandProcessor : : setVRTeleportPosition ( const btVector3 & vrTeleportPos )
{
gVRTeleportPos1 = vrTeleportPos ;
}
const btQuaternion & PhysicsServerCommandProcessor : : getVRTeleportOrientation ( ) const
{
return gVRTeleportOrn ;
}
void PhysicsServerCommandProcessor : : setVRTeleportOrientation ( const btQuaternion & vrTeleportOrn )
2017-02-24 23:34:11 +00:00
{
2017-05-31 02:54:55 +00:00
gVRTeleportOrn = vrTeleportOrn ;
2018-06-05 23:25:43 +00:00
}