mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
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
This commit is contained in:
Erwin Coumans
parent
e592290f4c
commit
826c5854a8
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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(); }
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -47,6 +47,9 @@ 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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
@ -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;
|
||||
@ -235,6 +242,10 @@ struct MotionArgs
|
||||
|
||||
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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
45
examples/pybullet/vrEvent.py
Normal 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]
|
||||
Loading…
Reference in New Issue
Block a user