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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
erwincoumans
2015-11-05 21:18:10 -08:00
parent 2920d7e61f 7ef06d532e
commit 8160354d02
4 changed files with 98 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -480,6 +480,41 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
} }
return bodyId; return bodyId;
} }
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
if (numDegreeOfFreedomQ) {
*numDegreeOfFreedomQ = args.m_numDegreeOfFreedomQ;
}
if (numDegreeOfFreedomU) {
*numDegreeOfFreedomU = args.m_numDegreeOfFreedomU;
}
if (rootLocalInertialFrame) {
*rootLocalInertialFrame = args.m_rootLocalInertialFrame;
}
if (actualStateQ) {
*actualStateQ = args.m_actualStateQ;
}
if (actualStateQdot) {
*actualStateQdot = args.m_actualStateQdot;
}
if (jointReactionForces) {
*jointReactionForces = args.m_jointReactionForces;
}
return true;
}
int b3CanSubmitCommand(b3PhysicsClientHandle physClient) int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -526,42 +561,54 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkInd
cl->getJointInfo(bodyIndex, linkIndex,*info); cl->getJointInfo(bodyIndex, linkIndex,*info);
} }
int b3PickBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ) double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{ {
b3Assert(command); PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(command->m_type == CMD_PICK_BODY); b3Assert(cl);
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; b3Assert(cl->canSubmitCommand());
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; b3Assert(command);
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; command->m_type = CMD_PICK_BODY;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
return 0; command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
} }
int b3MovePickedBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ) double rayToWorldX, double rayToWorldY,
double rayToWorldZ)
{ {
b3Assert(command); PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(command->m_type == CMD_MOVE_PICKED_BODY); b3Assert(cl);
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; b3Assert(cl->canSubmitCommand());
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; b3Assert(command);
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; command->m_type = CMD_MOVE_PICKED_BODY;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
return 0; command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
} }
int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(command); b3Assert(cl);
b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY); b3Assert(cl->canSubmitCommand());
return 0; struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_PICKING_CONSTRAINT_BODY;
return (b3SharedMemoryCommandHandle)command;
} }
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)

26
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -36,7 +36,15 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[]);
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
@@ -103,15 +111,15 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3PickBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY, double rayToWorldZ);
int b3MovePickedBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY,
int b3RemovePickingConstraint(struct SharedMemoryCommand *command); double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

2
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1915,4 +1915,4 @@ void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
{ {
CommandLogPlayback* pb = new CommandLogPlayback(fileName); CommandLogPlayback* pb = new CommandLogPlayback(fileName);
m_data->m_logPlayback = pb; m_data->m_logPlayback = pb;
} }

10
test/SharedMemory/test.c
View File

@@ -128,13 +128,12 @@ int main(int argc, char* argv[])
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{ {
#if 0 b3GetStatusActualState(statusHandle,
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; 0, &posVarCount, &dofCount,
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; 0, 0, 0, 0);
b3Printf("posVarCount = %d\n",posVarCount); b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount); printf("dofCount = %d\n",dofCount);
#endif
} }
} }
@@ -177,7 +176,6 @@ int main(int argc, char* argv[])
struct b3JointSensorState sensorState; struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[1],