Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[pybullet] getNumConstraints, getConstraintInfo APIs.

Browse Source 
[pybullet] updated pybullet_quickstartguide.pdf
Fail clearly (assert, return BT_INFINITY) if link index is out of range for btMultiBody methods localPosToWorld,worldPosToLocal,localDirToWorld,worldDirToLocal.
pybullet getConstraintInfo
Fix warnings due to Mac OSX 10.12 upgrade (with backward compatibility)
This commit is contained in:
Erwin Coumans
2017-01-22 19:08:31 -08:00
parent f237c4440f
commit cf9f022d39
17 changed files with 841 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

2
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -336,8 +336,6 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat) void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{ {
//const char* nodeName = node->Attribute("id");
//printf("processing node %s\n", nodeName);
btMatrix4x4 nodeTrans; btMatrix4x4 nodeTrans;

97
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -10,6 +10,61 @@
#include <stddef.h> #include <stddef.h>
#include <string.h> #include <string.h>
#include <Availability.h>
//aargh, Mac OSX 10.12 broke backwards compatibility, fix it here for now
#ifdef __MAC_10_12
#define MyNSTitledWindowMask NSWindowStyleMaskTitled
#define MyNSResizableWindowMask NSWindowStyleMaskResizable
#define MyNSClosableWindowMask NSWindowStyleMaskClosable
#define MyNSMiniaturizableWindowMask NSWindowStyleMaskMiniaturizable
#define MyNSAnyEventMask NSEventMaskAny
#define MyNSEventTypeFlagsChanged NSEventTypeFlagsChanged
#define MyNSEventModifierFlagShift NSEventModifierFlagShift
#define MyNSEventModifierFlagControl NSEventModifierFlagControl
#define MyNSEventModifierFlagOption NSEventModifierFlagOption
#define MyNSKeyUp NSEventTypeKeyUp
#define MyNSKeyDown NSEventTypeKeyDown
#define MyNSRightMouseDown NSEventTypeRightMouseDown
#define MyNSLeftMouseDown NSEventTypeLeftMouseDown
#define MyNSOtherMouseDown NSEventTypeOtherMouseDown
#define MyNSRightMouseUp NSEventTypeRightMouseUp
#define MyNSLeftMouseUp NSEventTypeLeftMouseUp
#define MyNSOtherMouseUp NSEventTypeOtherMouseUp
#define MyNSMouseMoved NSEventTypeMouseMoved
#define MyNSLeftMouseDragged NSEventTypeLeftMouseDragged
#define MyNSRightMouseDragged NSEventTypeRightMouseDragged
#define MyNSOtherMouseDragged NSEventTypeOtherMouseDragged
#define MyNSScrollWheel NSEventTypeScrollWheel
#else
#define MyNSTitledWindowMask NSTitledWindowMask
#define MyNSResizableWindowMask NSResizableWindowMask
#define MyNSClosableWindowMask NSClosableWindowMask
#define MyNSMiniaturizableWindowMask NSMiniaturizableWindowMask
#define MyNSAnyEventMask NSAnyEventMask
#define MyNSEventTypeFlagsChanged NSFlagsChanged
#define MyNSEventModifierFlagShift NSShiftKeyMask
#define MyNSEventModifierFlagControl NSControlKeyMask
#define MyNSEventModifierFlagOption NSAlternateKeyMask
#define MyNSKeyUp NSKeyUp
#define MyNSKeyDown NSKeyDown
#define NSRightMouseDown NSRightMouseDown
#define MyNSLeftMouseDown LeftMouseDown
#define MyNSOtherMouseDown NSOtherMouseDown
#define MyNSLeftMouseUp NSLeftMouseUp
#define MyNSRightMouseUp NSRightMouseUp
#define MyNSOtherMouseUp NSOtherMouseUp
#define MyNSMouseMoved NSMouseMoved
#define MyNSLeftMouseDragged NSLeftMouseDragged
#define MyNSRightMouseDragged NSRightMouseDragged
#define MyNSOtherMouseDragged NSOtherMouseDragged
#define MyNSScrollWheel NSScrollWheel
#endif
enum enum
{ {
@@ -368,7 +423,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_internalData->m_window = [NSWindow alloc]; m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame [m_internalData->m_window initWithContentRect:frame
styleMask:NSTitledWindowMask |NSResizableWindowMask| NSClosableWindowMask | NSMiniaturizableWindowMask styleMask:MyNSTitledWindowMask |MyNSResizableWindowMask| MyNSClosableWindowMask | MyNSMiniaturizableWindowMask
backing:NSBackingStoreBuffered backing:NSBackingStoreBuffered
defer:false]; defer:false];
@@ -771,7 +826,7 @@ void MacOpenGLWindow::startRendering()
[pool release]; [pool release];
pool = [[NSAutoreleasePool alloc] init]; pool = [[NSAutoreleasePool alloc] init];
event = [m_internalData->m_myApp event = [m_internalData->m_myApp
nextEventMatchingMask:NSAnyEventMask nextEventMatchingMask:MyNSAnyEventMask
untilDate:[NSDate distantPast] untilDate:[NSDate distantPast]
inMode:NSDefaultRunLoopMode inMode:NSDefaultRunLoopMode
// inMode:NSEventTrackingRunLoopMode // inMode:NSEventTrackingRunLoopMode
@@ -780,12 +835,12 @@ void MacOpenGLWindow::startRendering()
//NSShiftKeyMask = 1 << 17, //NSShiftKeyMask = 1 << 17,
//NSControlKeyMask //NSControlKeyMask
if ([event type] == NSFlagsChanged) if ([event type] == MyNSEventTypeFlagsChanged)
{ {
int modifiers = [event modifierFlags]; int modifiers = [event modifierFlags];
if (m_keyboardCallback) if (m_keyboardCallback)
{ {
if ((modifiers & NSShiftKeyMask)) if ((modifiers & MyNSEventModifierFlagShift))
{ {
m_keyboardCallback(B3G_SHIFT,1); m_keyboardCallback(B3G_SHIFT,1);
m_modifierFlags |= MY_MAC_SHIFTKEY; m_modifierFlags |= MY_MAC_SHIFTKEY;
@@ -797,7 +852,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_SHIFTKEY; m_modifierFlags &= ~MY_MAC_SHIFTKEY;
} }
} }
if (modifiers & NSControlKeyMask) if (modifiers & MyNSEventModifierFlagControl)
{ {
m_keyboardCallback(B3G_CONTROL,1); m_keyboardCallback(B3G_CONTROL,1);
m_modifierFlags |= MY_MAC_CONTROL_KEY; m_modifierFlags |= MY_MAC_CONTROL_KEY;
@@ -809,7 +864,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_CONTROL_KEY; m_modifierFlags &= ~MY_MAC_CONTROL_KEY;
} }
} }
if (modifiers & NSAlternateKeyMask) if (modifiers & MyNSEventModifierFlagOption)
{ {
m_keyboardCallback(B3G_ALT,1); m_keyboardCallback(B3G_ALT,1);
m_modifierFlags |= MY_MAC_ALTKEY; m_modifierFlags |= MY_MAC_ALTKEY;
@@ -826,7 +881,7 @@ void MacOpenGLWindow::startRendering()
} }
} }
if ([event type] == NSKeyUp) if ([event type] == MyNSKeyUp)
{ {
handledEvent = true; handledEvent = true;
@@ -841,7 +896,9 @@ void MacOpenGLWindow::startRendering()
m_keyboardCallback(keycode,state); m_keyboardCallback(keycode,state);
} }
} }
if ([event type] == NSKeyDown)
if ([event type] == MyNSKeyDown)
{ {
handledEvent = true; handledEvent = true;
@@ -861,7 +918,8 @@ void MacOpenGLWindow::startRendering()
} }
if (([event type]== NSRightMouseDown) || ([ event type]==NSLeftMouseDown)||([event type]==NSOtherMouseDown))
if (([event type]== MyNSRightMouseDown) || ([ event type]==MyNSLeftMouseDown)||([event type]==MyNSOtherMouseDown))
{ {
// printf("right mouse!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -873,17 +931,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseDown: case MyNSLeftMouseDown:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseDown: case MyNSOtherMouseDown:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseDown: case MyNSRightMouseDown:
{ {
button=2; button=2;
break; break;
@@ -902,7 +960,7 @@ void MacOpenGLWindow::startRendering()
} }
if (([event type]== NSRightMouseUp) || ([ event type]==NSLeftMouseUp)||([event type]==NSOtherMouseUp)) if (([event type]== MyNSRightMouseUp) || ([ event type]==MyNSLeftMouseUp)||([event type]==MyNSOtherMouseUp))
{ {
// printf("right mouse!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -915,17 +973,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseUp: case MyNSLeftMouseUp:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseUp: case MyNSOtherMouseUp:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseUp: case MyNSRightMouseUp:
{ {
button=2; button=2;
break; break;
@@ -943,7 +1001,7 @@ void MacOpenGLWindow::startRendering()
} }
if ([event type] == NSMouseMoved) if ([event type] == MyNSMouseMoved)
{ {
NSPoint eventLocation = [event locationInWindow]; NSPoint eventLocation = [event locationInWindow];
@@ -960,7 +1018,8 @@ void MacOpenGLWindow::startRendering()
} }
} }
if (([event type] == NSLeftMouseDragged) || ([event type] == NSRightMouseDragged) || ([event type] == NSOtherMouseDragged))
if (([event type] == MyNSLeftMouseDragged) || ([event type] == MyNSRightMouseDragged) || ([event type] == MyNSOtherMouseDragged))
{ {
int dx1, dy1; int dx1, dy1;
CGGetLastMouseDelta (&dx1, &dy1); CGGetLastMouseDelta (&dx1, &dy1);
@@ -979,7 +1038,7 @@ void MacOpenGLWindow::startRendering()
// printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY); // printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY);
} }
if ([event type] == NSScrollWheel) if ([event type] == MyNSScrollWheel)
{ {
float dy, dx; float dy, dx;
dy = [ event deltaY ]; dy = [ event deltaY ];

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -34,6 +34,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0; virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0; virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1045,6 +1045,31 @@ int b3GetNumBodies(b3PhysicsClientHandle physClient)
return cl->getNumBodies(); return cl->getNumBodies();
} }
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumUserConstraints();
}
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* infoPtr)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3UserConstraint constraintInfo1;
b3Assert(physClient);
b3Assert(infoPtr);
b3Assert(constraintUniqueId>=0);
if (infoPtr==0)
return 0;
if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1))
{
*infoPtr = constraintInfo1;
return 1;
}
return 0;
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() ) /// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{ {

10
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[], const double* actualStateQdot[],
const double* jointReactionForces[]); const double* jointReactionForces[]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info ///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
///return the total number of bodies in the simulation ///return the total number of bodies in the simulation
@@ -76,15 +76,21 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);

191
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -28,6 +28,7 @@ struct PhysicsClientSharedMemoryInternalData {
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom; btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo; btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
@@ -46,6 +47,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3RayHitInfo> m_raycastHits; btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo; btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus; SharedMemoryStatus m_lastServerStatus;
@@ -138,6 +141,22 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
return false; return false;
} }
int PhysicsClientSharedMemory::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory() PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{ {
@@ -156,6 +175,8 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
if (m_data->m_isConnected) { if (m_data->m_isConnected) {
disconnectSharedMemory(); disconnectSharedMemory();
} }
resetData();
if (m_data->m_ownsSharedMemory) if (m_data->m_ownsSharedMemory)
{ {
delete m_data->m_sharedMemory; delete m_data->m_sharedMemory;
@@ -163,6 +184,34 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
delete m_data; delete m_data;
} }
void PhysicsClientSharedMemory::resetData()
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
@@ -396,9 +445,74 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Printf("Server failed loading the URDF...\n"); b3Printf("Server failed loading the URDF...\n");
} }
break;
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break; break;
} }
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{
b3Warning("removeConstraint failed");
break;
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_BODY_INFO_COMPLETED: case CMD_BODY_INFO_COMPLETED:
{ {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
@@ -497,29 +611,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
} }
m_data->m_debugLinesFrom.clear(); resetData();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break; break;
} }
@@ -802,20 +894,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("User debug draw failed"); b3Warning("User debug draw failed");
break; break;
} }
case CMD_USER_CONSTRAINT_COMPLETED:
{
break;
}
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED: case CMD_SYNC_BODY_INFO_COMPLETED:
{ {
break; break;
@@ -841,6 +920,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)) if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{ {
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0) if (numBodies>0)
{ {
@@ -863,6 +948,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
} }
} }
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
{
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
}
else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{ {
//are there any bodies left to be processed? //are there any bodies left to be processed?
@@ -878,9 +982,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
submitClientCommand(command); submitClientCommand(command);
return 0; return 0;
} else } else
{
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
} else
{ {
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
} }
}
} }
if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED) if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)

6
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -12,7 +12,7 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected: protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
public: public:
PhysicsClientSharedMemory(); PhysicsClientSharedMemory();
@@ -44,6 +44,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len); virtual void uploadBulletFileToSharedMemory(const char* data, int len);

99
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -19,6 +19,8 @@ struct BodyJointInfoCache2
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
}; };
struct PhysicsDirectInternalData struct PhysicsDirectInternalData
{ {
DummyGUIHelper m_noGfx; DummyGUIHelper m_noGfx;
@@ -34,6 +36,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<TmpFloat3> m_debugLinesColor; btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
@@ -112,6 +115,7 @@ void PhysicsDirect::resetData()
} }
} }
m_data->m_bodyJointMap.clear(); m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
} }
// return true if connection succesfull, can also check 'isConnected' // return true if connection succesfull, can also check 'isConnected'
@@ -670,6 +674,47 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break; break;
} }
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED: case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED: case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED:
@@ -677,6 +722,29 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
//we'll stream further info from the physics server //we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy //so serverCmd will be invalid, make a copy
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_USER_CONSTRAINT;
infoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
if (hasStatus)
{
int cid = infoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,infoStatus.m_userConstraintResultArgs);
}
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i<numBodies; i++) for (int i = 0; i<numBodies; i++)
@@ -721,10 +789,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{ {
break; break;
} }
case CMD_USER_CONSTRAINT_COMPLETED:
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{ {
b3Warning("removeConstraint failed");
break; break;
} }
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_USER_CONSTRAINT_FAILED: case CMD_USER_CONSTRAINT_FAILED:
{ {
b3Warning("createConstraint failed"); b3Warning("createConstraint failed");
@@ -765,10 +841,12 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus; m_data->m_hasStatus = hasStatus;
if (hasStatus) /*if (hasStatus)
{ {
postProcessStatus(m_data->m_serverStatus); postProcessStatus(m_data->m_serverStatus);
m_data->m_hasStatus = false;
} }
*/
return hasStatus; return hasStatus;
} }
@@ -777,6 +855,23 @@ int PhysicsDirect::getNumBodies() const
return m_data->m_bodyJointMap.size(); return m_data->m_bodyJointMap.size();
} }
int PhysicsDirect::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{ {

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -63,6 +63,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
///todo: move this out of the ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
@@ -86,7 +90,7 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
//those 2 APIs are for internal use for visualization //the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper); virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene(); virtual void renderScene();
virtual void debugDraw(int debugDrawMode); virtual void debugDraw(int debugDrawMode);

11
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -99,7 +99,16 @@ bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3Joint
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
} }
///todo: move this out of the int PhysicsLoopBack::getNumUserConstraints() const
{
return m_data->m_physicsClient->getNumUserConstraints();
}
int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key) void PhysicsLoopBack::setSharedMemoryKey(int key)
{ {
m_data->m_physicsServer->setSharedMemoryKey(key); m_data->m_physicsServer->setSharedMemoryKey(key);

4
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -48,6 +48,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
///todo: move this out of the ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);

202
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -135,6 +135,9 @@ struct InteralUserConstraintData
{ {
btTypedConstraint* m_rbConstraint; btTypedConstraint* m_rbConstraint;
btMultiBodyConstraint* m_mbConstraint; btMultiBodyConstraint* m_mbConstraint;
b3UserConstraint m_userConstraintData;
InteralUserConstraintData() InteralUserConstraintData()
:m_rbConstraint(0), :m_rbConstraint(0),
m_mbConstraint(0) m_mbConstraint(0)
@@ -834,6 +837,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
deleteCachedInverseDynamicsBodies(); deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies(); deleteCachedInverseKinematicsBodies();
m_data->m_userConstraints.clear();
m_data->m_saveWorldBodyData.clear();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++) for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{ {
delete m_data->m_multiBodyJointFeedbacks[i]; delete m_data->m_multiBodyJointFeedbacks[i];
@@ -1760,6 +1766,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; 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; serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
hasStatus = true; hasStatus = true;
break; break;
@@ -1785,7 +1802,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
int constraintCount = 0;
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w"); FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f) if (f)
{ {
@@ -1900,6 +1917,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}; };
} }
//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;
switch (uc.m_jointType)
{
case eRevoluteType:
{
sprintf(jointTypeStr,"p.JOINT_REVOLUTE");
break;
}
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; }
default:
{
hasKnownJointType = false;
b3Warning("unknown constraint type in SAVE_WORLD");
}
};
if (hasKnownJointType)
{
{
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++;
}
}
}
}
}
{ {
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity(); btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
@@ -1942,6 +2046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); 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); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -3545,11 +3650,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true; hasStatus = true;
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;
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
{ {
btScalar defaultMaxForce = 500.0;
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody) if (parentBody && parentBody->m_multiBody)
{
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
{ {
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
//also create a constraint with just a single multibody/rigid body without child //also create a constraint with just a single multibody/rigid body without child
@@ -3563,30 +3680,38 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{ {
if (childBody && childBody->m_multiBody) if (childBody && childBody->m_multiBody)
{
if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
{ {
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); 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(500.0); multibodyFixed->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodyFixed; userConstraintData.m_mbConstraint = multibodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; 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; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
} }
else else
{ {
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(500.0); rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed); world->addMultiBodyConstraint(rigidbodyFixed);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodyFixed; userConstraintData.m_mbConstraint = rigidbodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; 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; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
@@ -3596,13 +3721,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (childBody && childBody->m_multiBody) if (childBody && childBody->m_multiBody)
{ {
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); 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(500.0); multibodySlider->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodySlider; userConstraintData.m_mbConstraint = multibodySlider;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; 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; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
else else
@@ -3610,29 +3738,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(500.0); rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider); world->addMultiBodyConstraint(rigidbodySlider);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodySlider; userConstraintData.m_mbConstraint = rigidbodySlider;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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 if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{ {
if (childBody && childBody->m_multiBody) 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); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500); p2p->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p; userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; 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; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
else else
@@ -3640,14 +3773,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500); p2p->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p); world->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p; userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; 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; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
@@ -3660,11 +3796,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
} }
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{ {
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr) if (userConstraintPtr)
{ {
if (userConstraintPtr->m_mbConstraint) if (userConstraintPtr->m_mbConstraint)
@@ -3674,7 +3812,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]); 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); userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
@@ -3683,13 +3823,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[4],
clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[5],
clientCmd.m_userConstraintArguments.m_childFrame[6]); 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); btMatrix3x3 childFrameBasis(childFrameOrn);
userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{ {
btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; 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); userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
} }
} }
@@ -3697,12 +3841,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
//todo //todo
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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) if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
{ {
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED;
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr) if (userConstraintPtr)
@@ -3717,8 +3864,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
} }
@@ -3974,6 +4123,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int numRb = importer->getNumRigidBodies(); int numRb = importer->getNumRigidBodies();
serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
for( int i=0;i<numRb;i++) for( int i=0;i<numRb;i++)
{ {
@@ -4044,6 +4194,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); 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); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -4539,7 +4690,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_gripperRigidbodyFixed = 0; m_data->m_gripperRigidbodyFixed = 0;
} }
//todo: move this to Python/scripting
//todo: move this to Python/scripting (it is almost ready to be removed!)
void PhysicsServerCommandProcessor::createDefaultRobotAssets() void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{ {
static btAlignedObjectArray<char> gBufferServerToClient; static btAlignedObjectArray<char> gBufferServerToClient;

30
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -432,6 +432,8 @@ struct SdfLoadedArgs
{ {
int m_numBodies; int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_numUserConstraints;
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
///@todo(erwincoumans) load cameras, lights etc ///@todo(erwincoumans) load cameras, lights etc
//int m_numCameras; //int m_numCameras;
@@ -541,27 +543,13 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
}; };
struct UserConstraintArgs
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct UserConstraintResultArgs
{
int m_userConstraintUniqueId;
};
enum EnumUserDebugDrawFlags enum EnumUserDebugDrawFlags
{ {
@@ -659,7 +647,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments; struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments; struct CalculateJacobianArgs m_calculateJacobianArguments;
struct UserConstraintArgs m_userConstraintArguments; struct b3UserConstraint m_userConstraintArguments;
struct RequestContactDataArgs m_requestContactPointArguments; struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs; struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
@@ -708,6 +696,10 @@ struct SharedMemoryStatus
int m_numDataStreamBytes; int m_numDataStreamBytes;
char* m_dataStream; char* m_dataStream;
//m_updateFlags is a bit fields to tell which parameters were updated,
//m_updateFlags is ignored for most status messages
int m_updateFlags;
union union
{ {
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
@@ -723,7 +715,7 @@ struct SharedMemoryStatus
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct UserConstraintResultArgs m_userConstraintResultArgs; struct b3UserConstraint m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents; struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits; struct SendRaycastHits m_raycastHits;
}; };

19
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -112,6 +112,11 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED, CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
CMD_CHANGE_USER_CONSTRAINT_FAILED,
CMD_USER_CONSTRAINT_FAILED, CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
@@ -164,6 +169,20 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame double m_jointAxis[3]; // joint axis in parent local frame
}; };
struct b3UserConstraint
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct b3BodyInfo struct b3BodyInfo
{ {
const char* m_baseName; const char* m_baseName;

132
examples/pybullet/pybullet.c
View File

@@ -1578,6 +1578,7 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
} }
} }
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int physicsClientId = 0; int physicsClientId = 0;
@@ -1650,6 +1651,122 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
} }
static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int constraintUniqueId= -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char *kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&constraintUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(sm,constraintUniqueId, &constraintInfo))
{
PyObject* pyListConstraintInfo = PyTuple_New(11);
PyTuple_SetItem(pyListConstraintInfo,0,PyLong_FromLong(constraintInfo.m_parentBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,1,PyLong_FromLong(constraintInfo.m_parentJointIndex));
PyTuple_SetItem(pyListConstraintInfo,2,PyLong_FromLong(constraintInfo.m_childBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,3,PyLong_FromLong(constraintInfo.m_childJointIndex));
PyTuple_SetItem(pyListConstraintInfo,4,PyLong_FromLong(constraintInfo.m_jointType));
{
PyObject* axisObj = PyTuple_New(3);
PyTuple_SetItem(axisObj,0,PyFloat_FromDouble(constraintInfo.m_jointAxis[0]));
PyTuple_SetItem(axisObj,1,PyFloat_FromDouble(constraintInfo.m_jointAxis[1]));
PyTuple_SetItem(axisObj,2,PyFloat_FromDouble(constraintInfo.m_jointAxis[2]));
PyTuple_SetItem(pyListConstraintInfo,5,axisObj);
}
{
PyObject* parentFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(parentFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[0]));
PyTuple_SetItem(parentFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[1]));
PyTuple_SetItem(parentFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,6,parentFramePositionObj);
}
{
PyObject* childFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(childFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_childFrame[0]));
PyTuple_SetItem(childFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_childFrame[1]));
PyTuple_SetItem(childFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_childFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,7,childFramePositionObj);
}
{
PyObject* parentFrameOrientationObj = PyTuple_New(4);
PyTuple_SetItem(parentFrameOrientationObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[3]));
PyTuple_SetItem(parentFrameOrientationObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[4]));
PyTuple_SetItem(parentFrameOrientationObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[5]));
PyTuple_SetItem(parentFrameOrientationObj,3,PyFloat_FromDouble(constraintInfo.m_parentFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,8,parentFrameOrientationObj);
}
{
PyObject* childFrameOrientation = PyTuple_New(4);
PyTuple_SetItem(childFrameOrientation,0,PyFloat_FromDouble(constraintInfo.m_childFrame[3]));
PyTuple_SetItem(childFrameOrientation,1,PyFloat_FromDouble(constraintInfo.m_childFrame[4]));
PyTuple_SetItem(childFrameOrientation,2,PyFloat_FromDouble(constraintInfo.m_childFrame[5]));
PyTuple_SetItem(childFrameOrientation,3,PyFloat_FromDouble(constraintInfo.m_childFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,9,childFrameOrientation);
}
PyTuple_SetItem(pyListConstraintInfo,10,PyFloat_FromDouble(constraintInfo.m_maxAppliedForce));
return pyListConstraintInfo;
} else
{
PyErr_SetString(SpamError, "Couldn't get user constraint info");
return NULL;
}
}
}
PyErr_SetString(SpamError, "error in getConstraintInfo.");
return NULL;
}
static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
{
int numConstraints = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
numConstraints = b3GetNumUserConstraints(sm);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(numConstraints);
#else
return PyInt_FromLong(numConstraints);
#endif
}
// Return the number of joints in an object based on // Return the number of joints in an object based on
// body index; body index is based on order of sequence // body index; body index is based on order of sequence
// the object is loaded into simulation // the object is loaded into simulation
@@ -2221,13 +2338,10 @@ static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args,
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
int res = 0;
char* text; char* text;
double colorRGB[3]={1,1,1};
PyObject* textPositionObj=0;
double rangeMin = 0.f; double rangeMin = 0.f;
double rangeMax = 1.f; double rangeMax = 1.f;
double startValue = 0.f; double startValue = 0.f;
@@ -4561,11 +4675,21 @@ static PyMethodDef SpamMethods[] = {
"Get the number of bodies in the simulation."}, "Get the number of bodies in the simulation."},
{"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."}, "getBodyUniqueId is used after connecting to server with existing bodies."
"Get the unique id of the body, given a integer range [0.. number of bodies)."},
{"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
"Get the body info, given a body unique id."}, "Get the body info, given a body unique id."},
{"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS| METH_KEYWORDS,
"Get the number of user-created constraints in the simulation."},
{"getConstraintInfo",(PyCFunction) pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
{"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation, {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation,
METH_VARARGS | METH_KEYWORDS, METH_VARARGS | METH_KEYWORDS,
"Get the world position and orientation of the base of the object. " "Get the world position and orientation of the base of the object. "

29
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -430,6 +430,13 @@ const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_pos; btVector3 result = local_pos;
while (i != -1) { while (i != -1) {
// 'result' is in frame i. transform it to frame parent(i) // 'result' is in frame i. transform it to frame parent(i)
@@ -447,6 +454,13 @@ btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
// world to base // world to base
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
@@ -458,6 +472,14 @@ btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_dir; btVector3 result = local_dir;
while (i != -1) { while (i != -1) {
result = quatRotate(getParentToLocalRot(i).inverse() , result); result = quatRotate(getParentToLocalRot(i).inverse() , result);
@@ -469,6 +491,13 @@ btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
return quatRotate(getWorldToBaseRot(), world_dir); return quatRotate(getWorldToBaseRot(), world_dir);
} else { } else {