From 826c5854a8e34a32e8fc7381f13c5b9c4530a933 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 26 Dec 2016 19:40:09 -0800 Subject: [PATCH] See also pybullet quickstart guide here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit# 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 --- examples/SharedMemory/PhysicsClient.h | 4 + examples/SharedMemory/PhysicsClientC_API.cpp | 55 +++++ examples/SharedMemory/PhysicsClientC_API.h | 14 ++ .../PhysicsClientSharedMemory.cpp | 45 ++++ .../SharedMemory/PhysicsClientSharedMemory.h | 5 + examples/SharedMemory/PhysicsDirect.cpp | 46 +++- examples/SharedMemory/PhysicsDirect.h | 3 + examples/SharedMemory/PhysicsLoopBack.cpp | 9 + examples/SharedMemory/PhysicsLoopBack.h | 5 + .../PhysicsServerCommandProcessor.cpp | 130 ++++++++++- .../PhysicsServerCommandProcessor.h | 2 +- .../SharedMemory/PhysicsServerExample.cpp | 206 +++++++++++++++--- .../PhysicsServerSharedMemory.cpp | 4 +- .../SharedMemory/PhysicsServerSharedMemory.h | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 23 ++ examples/SharedMemory/SharedMemoryPublic.h | 61 +++++- .../StandaloneMain/hellovr_opengl_main.cpp | 2 +- examples/pybullet/pybullet.c | 204 ++++++++++++++++- examples/pybullet/vrEvent.py | 45 ++++ 19 files changed, 830 insertions(+), 35 deletions(-) create mode 100644 examples/pybullet/vrEvent.py diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index e1c20cadb..3e3248a0b 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a9d217c3e..89c180374 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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); + } +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 97b6a14a6..b8b27dd53 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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 diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e02d11e2d..ae0ab604b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -42,6 +42,8 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedVREvents; + btAlignedObjectArray m_raycastHits; btAlignedObjectArray 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;im_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(); } + diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index d5f6b3cff..854618456 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -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 diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index cb7a16abd..b12438cc2 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -47,7 +47,10 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; - + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray 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;im_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; +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 963281f7e..6b6b526be 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -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); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 3e9c3d77d..626653f43 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -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); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index c9187c4b8..0eaea6ff8 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1a2091532..93bb085f9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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;im_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;bm_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;im_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;bm_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(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index c8a060069..c4fd8e7a3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -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); }; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 1cee22132..804007cc1 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -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 m_mouseCommands; + + b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; + b3VRControllerEvent m_sendVrControllerEvents[MAX_VR_CONTROLLERS]; + PhysicsServerSharedMemory* m_physicsServerPtr; b3AlignedObjectArray 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;im_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;bm_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 > sortedIndices; + btAlignedObjectArray< btAlignedObjectArray > sortedLines; + + btHashMap hashedLines; + for (int i = 0; im_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;igetAppInterface()->m_renderer->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width); + } } for (int i = 0; im_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) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 158bdac6b..5c967c9c4 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index f29e844f2..bd9b9b152 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d905ca980..acab7d351 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fe79d9dcb..7eb9d37ad 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 1924869b6..71a1aa7e9 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -705,7 +705,7 @@ bool CMainApplication::HandleInput() // printf("Device PRESSED: %d, button %d\n", unDevice, button); if (button==2) { - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync ///so it can (and likely will) cause crashes ///add a special debug drawer that deals with this diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d130712d5..5928d357d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1956,6 +1956,188 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args return Py_None; } + +static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* rayFromObj=0; + PyObject* rayToObj=0; + double from[3]; + double to[3]; + static char *kwlist[] = { "rayFromPosition", "rayToPosition", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist, + &rayFromObj, &rayToObj)) + return NULL; + + pybullet_internalSetVectord(rayFromObj,from); + pybullet_internalSetVectord(rayToObj,to); + + commandHandle = b3CreateRaycastCommandInit(sm, from[0],from[1],from[2], + to[0],to[1],to[2]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) + { + struct b3RaycastInformation raycastInfo; + PyObject* rayHitsObj = 0; + int i; + b3GetRaycastInformation(sm, &raycastInfo); + + rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits); + for (i=0;i20): + widths[e[0]] = 1 + if (e[6][33]==1): + pt = prev[e[0]] + #print(prev[e[0]]) + #print(e[1]) + diff = [pt[0]-e[1][0],pt[1]-e[1][1],pt[2]-e[1][2]] + lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2] + ptDistThreshold = 0.01 + if (lenSqr>(ptDistThreshold*ptDistThreshold)): + p.addUserDebugLine(e[1],prev[e[0]],colors[e[0]],widths[e[0]]) + colors[e[0]] = [1-colors[e[0]][0],1-colors[e[0]][1],1-colors[e[0]][2]] + prev[e[0]] = e[1] \ No newline at end of file