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

See also pybullet quickstart guide here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#

Browse Source 
vrevent.py: add a Tiltbrush-style drawing example using pybullet
Expose getVREvents to pybullet / shared memory API, access to any VR controller state & state changes.
Improve performance of user debug lines (pybullet/shared memory API) by batching lines with same color/width
expose rayTest to pybullet/shared memory API (single ray for now)
add pybullet getMatrixFromQuaterion
This commit is contained in:
Erwin Coumans
2016-12-26 19:40:09 -08:00
parent e592290f4c
commit 826c5854a8
19 changed files with 830 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -52,6 +52,10 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0;
};
#endif // BT_PHYSICS_CLIENT_API_H

55
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1122,6 +1122,38 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX;
command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY;
command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
}
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedRaycastHits(raycastInfo);
}
}
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2135,3 +2167,26 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
return true;
}
b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_VR_EVENTS_DATA;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedVREvents(vrEventsData);
}
}

14
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -291,6 +291,14 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
@@ -302,6 +310,12 @@ int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
#ifdef __cplusplus
}
#endif

45
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -42,6 +42,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -631,6 +633,35 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Overlapping object query failed");
break;
}
case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Raycast completed");
}
m_data->m_raycastHits.clear();
for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
{
m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
}
break;
}
case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Request VR Events completed");
}
m_data->m_cachedVREvents.clear();
for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++)
{
m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
}
break;
}
case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
{
if (m_data->m_verboseOutput)
@@ -983,6 +1014,19 @@ void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlap
&m_data->m_cachedOverlappingObjects[0] : 0;
}
void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents?
&m_data->m_cachedVREvents[0] : 0;
}
void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
{
raycastHits->m_numRayHits = m_data->m_raycastHits.size();
raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
}
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
@@ -1010,3 +1054,4 @@ const float* PhysicsClientSharedMemory::getDebugLinesColor() const {
return 0;
}
int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); }

5
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -60,6 +60,11 @@ public:
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
};
#endif // BT_PHYSICS_CLIENT_API_H

46
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -47,7 +47,10 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
PhysicsCommandProcessorInterface* m_commandProcessor;
bool m_ownsCommandProcessor;
@@ -581,6 +584,34 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
switch (serverCmd.m_type)
{
case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Raycast completed");
}
m_data->m_raycastHits.clear();
for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
{
m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
}
break;
}
case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Request VR Events completed");
}
m_data->m_cachedVREvents.clear();
for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++)
{
m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
}
break;
}
case CMD_REQUEST_INTERNAL_DATA_COMPLETED:
{
if (serverCmd.m_numDataStreamBytes)
@@ -851,3 +882,16 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents?
&m_data->m_cachedVREvents[0] : 0;
}
void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
{
raycastHits->m_numRayHits = m_data->m_raycastHits.size();
raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
}

3
examples/SharedMemory/PhysicsDirect.h
View File

@@ -81,6 +81,9 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);

9
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -146,8 +146,17 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
}
void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
return m_data->m_physicsClient->getCachedVREvents(vrEventsData);
}
void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects);
}
void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
{
return m_data->m_physicsClient->getCachedRaycastHits(raycastHits);
}

5
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -65,6 +65,11 @@ public:
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
};
#endif //PHYSICS_LOOP_BACK_H

130
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -463,6 +463,8 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
b3VRControllerEvent m_vrEvents[MAX_VR_CONTROLLERS];
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
btMultiBodyFixedConstraint* m_kukaGripperFixed;
@@ -562,6 +564,15 @@ struct PhysicsServerCommandProcessorInternalData
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
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;
}
}
initHandles();
#if 0
@@ -1308,6 +1319,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
#endif
case CMD_REQUEST_VR_EVENTS_DATA:
{
serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
if (m_data->m_vrEvents[i].m_numButtonEvents + m_data->m_vrEvents[i].m_numMoveEvents)
{
serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = m_data->m_vrEvents[i];
m_data->m_vrEvents[i].m_numButtonEvents = 0;
m_data->m_vrEvents[i].m_numMoveEvents = 0;
for (int b=0;b<MAX_VR_BUTTONS;b++)
{
m_data->m_vrEvents[i].m_buttons[b] = 0;
}
}
}
serverStatusOut.m_type = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED;
hasStatus = true;
break;
};
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
{
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]);
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
if (rayResultCallback.hasHit())
{
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction
= rayResultCallback.m_closestHitFraction;
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();
}
}
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId
= objectUniqueId;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex
= linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0]
= rayResultCallback.m_hitPointWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1]
= rayResultCallback.m_hitPointWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2]
= rayResultCallback.m_hitPointWorld[2];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0]
= rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1]
= rayResultCallback.m_hitNormalWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2]
= rayResultCallback.m_hitNormalWorld[2];
serverStatusOut.m_raycastHits.m_numRaycastHits++;
}
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
hasStatus = true;
break;
};
case CMD_REQUEST_DEBUG_LINES:
{
int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
@@ -4054,8 +4144,46 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, 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_data->m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis;
}
if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents)
{
m_data->m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId;
m_data->m_vrEvents[controlledId].m_pos[0] = vrEvents[i].m_pos[0];
m_data->m_vrEvents[controlledId].m_pos[1] = vrEvents[i].m_pos[1];
m_data->m_vrEvents[controlledId].m_pos[2] = vrEvents[i].m_pos[2];
m_data->m_vrEvents[controlledId].m_orn[0] = vrEvents[i].m_orn[0];
m_data->m_vrEvents[controlledId].m_orn[1] = vrEvents[i].m_orn[1];
m_data->m_vrEvents[controlledId].m_orn[2] = vrEvents[i].m_orn[2];
m_data->m_vrEvents[controlledId].m_orn[3] = vrEvents[i].m_orn[3];
}
m_data->m_vrEvents[controlledId].m_numButtonEvents += vrEvents[i].m_numButtonEvents;
m_data->m_vrEvents[controlledId].m_numMoveEvents += vrEvents[i].m_numMoveEvents;
for (int b=0;b<MAX_VR_BUTTONS;b++)
{
m_data->m_vrEvents[controlledId].m_buttons[b] |= vrEvents[i].m_buttons[b];
if (vrEvents[i].m_buttons[b] & eButtonIsDown)
{
m_data->m_vrEvents[controlledId].m_buttons[b] |= eButtonIsDown;
} else
{
m_data->m_vrEvents[controlledId].m_buttons[b] &= ~eButtonIsDown;
}
}
}
if (gResetSimulation)
{
resetSimulation();

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -84,7 +84,7 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void stepSimulationRealTime(double dtInSec);
void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents);
void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId);
};

206
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -14,6 +14,7 @@
#include "Bullet3Common/b3Matrix3x3.h"
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
#include "SharedMemoryPublic.h"
#ifdef BT_ENABLE_VR
#include "../RenderingExamples/TinyVRGui.h"
#endif//BT_ENABLE_VR
@@ -21,7 +22,6 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#define MAX_VR_CONTROLLERS 8
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
@@ -44,7 +44,7 @@ extern bool gResetSimulation;
extern int gEnableKukaControl;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
bool gBatchUserDebugLines = true;
extern bool gVRGripperClosed;
const char* startFileNameVR = "0_VRDemoSettings.txt";
@@ -225,6 +225,13 @@ struct MotionArgs
{
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
m_vrControllerEvents[i].m_numButtonEvents = 0;
m_vrControllerEvents[i].m_numMoveEvents = 0;
for (int b=0;b<MAX_VR_BUTTONS;b++)
{
m_vrControllerEvents[i].m_buttons[b]=0;
}
m_isVrControllerPicking[i] = false;
m_isVrControllerDragging[i] = false;
m_isVrControllerReleasing[i] = false;
@@ -234,7 +241,11 @@ struct MotionArgs
b3CriticalSection* m_cs;
btAlignedObjectArray<MyMouseCommand> m_mouseCommands;
b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS];
b3VRControllerEvent m_sendVrControllerEvents[MAX_VR_CONTROLLERS];
PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray<b3Vector3> m_positions;
@@ -348,7 +359,33 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
}
clock.reset();
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
args->m_cs->lock();
int numSendVrControllers = 0;
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
if (args->m_vrControllerEvents[i].m_numButtonEvents+args->m_vrControllerEvents[i].m_numMoveEvents)
{
args->m_sendVrControllerEvents[numSendVrControllers++] =
args->m_vrControllerEvents[i];
if (args->m_vrControllerEvents[i].m_numButtonEvents)
{
for (int b=0;b<MAX_VR_BUTTONS;b++)
{
args->m_vrControllerEvents[i].m_buttons[b] &= eButtonIsDown;
}
}
args->m_vrControllerEvents[i].m_numMoveEvents = 0;
args->m_vrControllerEvents[i].m_numButtonEvents = 0;
}
}
args->m_cs->unlock();
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
deltaTimeInSeconds = 0;
}
@@ -993,7 +1030,7 @@ public:
if (args.CheckCmdLineFlag("robotassets"))
{
gCreateDefaultRobotAssets = true;
// gCreateDefaultRobotAssets = true;
}
if (args.CheckCmdLineFlag("norobotassets"))
@@ -1408,6 +1445,40 @@ void PhysicsServerExample::drawUserDebugLines()
if (m_multiThreadedHelper)
{
//if gBatchUserDebugLines is true, batch lines based on color+width, to reduce line draw calls
struct LineSegment
{
btVector3 m_from;
btVector3 m_to;
};
struct ColorWidth
{
btVector3FloatData m_color;
int width;
int getHash() const
{
unsigned char r = (unsigned char) m_color.m_floats[0]*255;
unsigned char g = (unsigned char) m_color.m_floats[1]*255;
unsigned char b = (unsigned char) m_color.m_floats[2]*255;
unsigned char w = width;
return r+(256*g)+(256*256*b)+(256*256*256*w);
}
bool equals(const ColorWidth& other) const
{
bool same = ((width == other.width) && (m_color.m_floats[0] == other.m_color.m_floats[0]) &&
(m_color.m_floats[1] == other.m_color.m_floats[1]) &&
(m_color.m_floats[2] == other.m_color.m_floats[2]));
return same;
}
};
btAlignedObjectArray< btAlignedObjectArray<unsigned int> > sortedIndices;
btAlignedObjectArray< btAlignedObjectArray<btVector3FloatData> > sortedLines;
btHashMap<ColorWidth,int> hashedLines;
for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
{
btVector3 from;
@@ -1423,9 +1494,56 @@ void PhysicsServerExample::drawUserDebugLines()
color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
ColorWidth cw;
color.serializeFloat(cw.m_color);
cw.width = m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth;
int index = -1;
if (gBatchUserDebugLines)
{
int* indexPtr = hashedLines.find(cw);
if (indexPtr)
{
index = *indexPtr;
} else
{
index = sortedLines.size();
sortedLines.expand();
sortedIndices.expand();
hashedLines.insert(cw,index);
}
btAssert(index>=0);
if (index>=0)
{
btVector3FloatData from1,toX1;
sortedIndices[index].push_back(sortedLines[index].size());
from.serializeFloat(from1);
sortedLines[index].push_back(from1);
sortedIndices[index].push_back(sortedLines[index].size());
toX.serializeFloat(toX1);
sortedLines[index].push_back(toX1);
}
}
else
{
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
}
}
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
if (gBatchUserDebugLines)
{
for (int i=0;i<hashedLines.size();i++)
{
ColorWidth cw = hashedLines.getKeyAtIndex(i);
int index = *hashedLines.getAtIndex(i);
int stride = sizeof(btVector3FloatData);
const float* positions = &sortedLines[index][0].m_floats[0];
int numPoints = sortedLines[index].size();
const unsigned int* indices = &sortedIndices[index][0];
int numIndices = sortedIndices[index].size();
m_guiHelper->getAppInterface()->m_renderer->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width);
}
}
for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
@@ -1713,6 +1831,30 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
gGraspingController = controllerId;
gEnableKukaControl = true;
}
btTransform trLocal;
trLocal.setIdentity();
trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
btTransform trOrg;
trOrg.setIdentity();
trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));
btTransform tr2a;
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)
@@ -1786,27 +1928,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
}
btTransform trLocal;
trLocal.setIdentity();
trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
btTransform trOrg;
trOrg.setIdentity();
trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));
btTransform tr2a;
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
if ((button == 33) || (button == 1))
{
@@ -1817,6 +1938,26 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
}
m_args[0].m_cs->lock();
m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
m_args[0].m_vrControllerEvents[controllerId].m_numButtonEvents++;
if (state)
{
m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonIsDown+eButtonTriggered;
} else
{
m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased;
m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown;
}
m_args[0].m_cs->unlock();
}
@@ -1868,5 +2009,18 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
}
m_args[0].m_cs->lock();
m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
m_args[0].m_cs->unlock();
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

4
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -236,9 +236,9 @@ void PhysicsServerSharedMemory::releaseSharedMemory()
}
void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents)
{
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents);
}
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)

2
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -26,7 +26,7 @@ public:
virtual void processClientCommands();
virtual void stepSimulationRealTime(double dtInSec);
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);

23
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -174,6 +174,18 @@ enum EnumRequestContactDataUpdateFlags
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
};
struct RequestRaycastIntersections
{
double m_rayFromPosition[3];
double m_rayToPosition[3];
};
struct SendRaycastHits
{
int m_numRaycastHits;
b3RayHitInfo m_rayHits[MAX_RAY_HITS];
};
struct RequestContactDataArgs
{
int m_startingContactPointIndex;
@@ -615,6 +627,7 @@ struct SharedMemoryCommand
struct LoadTextureArgs m_loadTextureArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
struct UserDebugDrawArgs m_userDebugDrawArgs;
struct RequestRaycastIntersections m_requestRaycastIntersections;
struct LoadBunnyArgs m_loadBunnyArguments;
};
};
@@ -638,6 +651,14 @@ struct SendOverlappingObjectsArgs
int m_numRemainingOverlappingObjects;
};
struct SendVREvents
{
int m_numVRControllerEvents;
b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
};
struct SharedMemoryStatus
{
int m_type;
@@ -665,6 +686,8 @@ struct SharedMemoryStatus
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct UserConstraintResultArgs m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits;
};
};

61
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -35,14 +35,17 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_JACOBIAN,
CMD_USER_CONSTRAINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_REQUEST_RAY_CAST_INTERSECTIONS,
CMD_REQUEST_AABB_OVERLAP,
CMD_SAVE_WORLD,
CMD_REQUEST_VISUAL_SHAPE_INFO,
CMD_UPDATE_VISUAL_SHAPE,
CMD_LOAD_TEXTURE,
CMD_SET_SHADOW,
CMD_USER_DEBUG_DRAW,
CMD_REQUEST_VR_EVENTS_DATA,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -107,6 +110,8 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -199,6 +204,44 @@ struct b3CameraImageData
};
enum b3VREventType
{
VR_CONTROLLER_MOVE_EVENT=1,
VR_CONTROLLER_BUTTON_EVENT
};
#define MAX_VR_BUTTONS 64
#define MAX_VR_CONTROLLERS 8
#define MAX_RAY_HITS 128
enum b3VRButtonInfo
{
eButtonIsDown = 1,
eButtonTriggered = 2,
eButtonReleased = 4,
};
struct b3VRControllerEvent
{
int m_controllerId;//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
int m_numMoveEvents;
int m_numButtonEvents;
float m_pos[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT
int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo
};
struct b3VREventsData
{
int m_numControllerEvents;
struct b3VRControllerEvent* m_controllerEvents;
};
struct b3ContactPointData
{
//todo: expose some contact flags, such as telling which fields below are valid
@@ -237,6 +280,22 @@ struct b3ContactInformation
struct b3ContactPointData* m_contactPointData;
};
struct b3RayHitInfo
{
double m_hitFraction;
int m_hitObjectUniqueId;
int m_hitObjectLinkIndex;
double m_hitPositionWorld[3];
double m_hitNormalWorld[3];
};
struct b3RaycastInformation
{
int m_numRayHits;
struct b3RayHitInfo* m_rayHits;
};
#define VISUAL_SHAPE_MAX_PATH_LEN 128
struct b3VisualShapeData