AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues 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
Show Stats 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

2
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@ -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

204
examples/pybullet/pybullet.c
View File

@ -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;i<raycastInfo.m_numRayHits;i++)
{
PyObject* singleHitObj = PyTuple_New(5);
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
PyTuple_SetItem(singleHitObj,0,ob);
}
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
PyTuple_SetItem(singleHitObj,1,ob);
}
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
PyTuple_SetItem(singleHitObj,2,ob);
}
{
PyObject* posObj = PyTuple_New(3);
int p;
for (p=0;p<3;p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
PyTuple_SetItem(posObj,p,ob);
}
PyTuple_SetItem(singleHitObj,3,posObj);
}
{
PyObject* normalObj = PyTuple_New(3);
int p;
for (p=0;p<3;p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
PyTuple_SetItem(normalObj,p,ob);
}
PyTuple_SetItem(singleHitObj,4,normalObj);
}
PyTuple_SetItem(rayHitsObj,i,singleHitObj);
}
return rayHitsObj;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args)
{
PyObject* quatObj;
double quat[4];
if (PyArg_ParseTuple(args, "O", &quatObj))
{
if (pybullet_internalSetVector4d(quatObj,quat))
{
///see btMatrix3x3::setRotation
int i;
double d = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
double s = 2.0 / d;
double xs = quat[0] * s, ys = quat[1] * s, zs = quat[2] * s;
double wx = quat[3] * xs, wy = quat[3] * ys, wz = quat[3] * zs;
double xx = quat[0] * xs, xy = quat[0] * ys, xz = quat[0] * zs;
double yy = quat[1] * ys, yz = quat[1] * zs, zz = quat[2] * zs;
double mat3x3[9] = {
1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy)};
PyObject* matObj = PyTuple_New(9);
for (i=0;i<9;i++)
{
PyTuple_SetItem(matObj,i,PyFloat_FromDouble(mat3x3[i]));
}
return matObj;
}
}
PyErr_SetString(SpamError, "Couldn't convert quaternion [x,y,z,w].");
return NULL;
};
static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3RequestVREventsCommandInit(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
{
struct b3VREventsData vrEvents;
PyObject* vrEventsObj;
int i=0;
b3GetVREventsData(sm,&vrEvents);
vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents);
for (i=0;i<vrEvents.m_numControllerEvents;i++)
{
PyObject* vrEventObj = PyTuple_New(7);
PyTuple_SetItem(vrEventObj,0,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId));
{
PyObject* posObj = PyTuple_New(3);
PyTuple_SetItem(posObj,0,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[0]));
PyTuple_SetItem(posObj,1,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[1]));
PyTuple_SetItem(posObj,2,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[2]));
PyTuple_SetItem(vrEventObj,1,posObj);
}
{
PyObject* ornObj = PyTuple_New(4);
PyTuple_SetItem(ornObj,0,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[0]));
PyTuple_SetItem(ornObj,1,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[1]));
PyTuple_SetItem(ornObj,2,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[2]));
PyTuple_SetItem(ornObj,3,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[3]));
PyTuple_SetItem(vrEventObj,2,ornObj);
}
PyTuple_SetItem(vrEventObj,3,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_analogAxis));
PyTuple_SetItem(vrEventObj,4,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_numButtonEvents));
PyTuple_SetItem(vrEventObj,5,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_numMoveEvents));
{
PyObject* buttonsObj = PyTuple_New(MAX_VR_BUTTONS);
int b;
for (b=0;b<MAX_VR_BUTTONS;b++)
{
PyObject* button = PyInt_FromLong(vrEvents.m_controllerEvents[i].m_buttons[b]);
PyTuple_SetItem(buttonsObj,b,button);
}
PyTuple_SetItem(vrEventObj,6,buttonsObj);
}
PyTuple_SetItem(vrEventsObj,i,vrEventObj);
}
return vrEventsObj;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
{
PyObject* objectColorRGBObj = 0;
@ -3820,6 +4002,9 @@ static PyMethodDef SpamMethods[] = {
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
"convention"},
{"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
{"calculateInverseDynamics", pybullet_calculateInverseDynamics,
METH_VARARGS,
"Given an object id, joint positions, joint velocities and joint "
@ -3832,12 +4017,21 @@ static PyMethodDef SpamMethods[] = {
" for the end effector,"
"compute the inverse kinematics and return the new joint state"},
{ "getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS,
"Get Virtual Reality events, for example to track VR controllers position/buttons"
},
{ "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
"Cast a ray and return the first object hit, if any. "
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"
},
// todo(erwincoumans)
// saveSnapshot
// loadSnapshot
// raycast info
// object names
// collision query
{NULL, NULL, 0, NULL} /* Sentinel */
};
@ -3902,6 +4096,14 @@ initpybullet(void)
PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS);
PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS);
PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown);
PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered);
PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased);
PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);

45
examples/pybullet/vrEvent.py Normal file
View File

@ -0,0 +1,45 @@
#See pybullet quickstart guide here:
#https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#
# Create a Tiltbrush-like app, drawing lines using any controller
# Line width can be changed
import pybullet as p
#assume that the VR physics server is already started before
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
prev=[None]*p.VR_MAX_CONTROLLERS
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
widths = [3]*p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0,0,0]
colors[1] = [0.5,0,0]
colors[2] = [0,0.5,0]
colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
while True:
events = p.getVREvents()
for e in (events):
if (e[6][33]&2):
prev[e[0]] = e[1]
if (e[6][32]&2):
widths[e[0]]=widths[e[0]]+1
if (widths[e[0]]>20):
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]