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

Merge pull request #898 from erwincoumans/master

Browse Source 
many pybullet / VR improvements
This commit is contained in:
erwincoumans
2016-12-27 15:57:17 -08:00
committed by GitHub
parent 1deb3c1666 e655fead29
commit a5ecbbc682
30 changed files with 1477 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/BasicDemo/CMakeLists.txt
View File

@@ -60,7 +60,7 @@ SET(AppBasicExampleGui_SRCS
ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE)
LINK_LIBRARIES(
BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common
)
#some code to support OpenGL and Glew cross platform
@@ -69,6 +69,7 @@ IF (WIN32)
${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
)
ADD_DEFINITIONS(-DGLEW_STATIC)
LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} )
ELSE(WIN32)
IF(APPLE)
find_library(COCOA NAMES Cocoa)
@@ -83,7 +84,7 @@ ELSE(WIN32)
ADD_DEFINITIONS("-DGLEW_STATIC")
ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
LINK_LIBRARIES( X11 pthread dl Xext)
LINK_LIBRARIES( pthread dl )
ENDIF(APPLE)
ENDIF(WIN32)

12
examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
View File

@@ -44,7 +44,7 @@ public:
CProfileIterator* profIter;
class MyMenuItems* m_menuItems;
MyProfileWindow ( Gwen::Controls::Base* pParent)
: Gwen::Controls::WindowControl( pParent ),
@@ -192,7 +192,8 @@ public:
// Gwen::Controls::TreeNode* curParent = m_node;
double accumulated_time = dumpRecursive(profileIterator,m_node);
const char* name = profileIterator->Get_Current_Parent_Name();
@@ -278,7 +279,7 @@ MyProfileWindow* setupProfileWindow(GwenInternalData* data)
//profWindow->SetHidden(true);
profWindow->m_menuItems = menuItems;
//profWindow->profIter = CProfileManager::Get_Iterator();
profWindow->profIter = CProfileManager::Get_Iterator();
data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect);
menuItems->m_profWindow = profWindow;
@@ -296,6 +297,11 @@ void processProfileData( MyProfileWindow* profWindow, bool idle)
}
}
bool isProfileWindowVisible(MyProfileWindow* window)
{
return !window->Hidden();
}
void profileWindowSetVisible(MyProfileWindow* window, bool visible)
{
window->SetHidden(!visible);

2
examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h
View File

@@ -4,6 +4,8 @@
class MyProfileWindow* setupProfileWindow(struct GwenInternalData* data);
void processProfileData(MyProfileWindow* window, bool idle);
void profileWindowSetVisible(MyProfileWindow* window, bool visible);
bool isProfileWindowVisible(MyProfileWindow* window);
void destroyProfileWindow(MyProfileWindow* window);
#endif//GWEN_PROFILE_WINDOW_H

291
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -17,7 +17,7 @@
#endif //_WIN32
#endif//__APPLE__
#include "../ThirdPartyLibs/Gwen/Renderers/OpenGL_DebugFont.h"
#include "LinearMath/btThreads.h"
#include "Bullet3Common/b3Vector3.h"
#include "assert.h"
#include <stdio.h>
@@ -25,7 +25,9 @@
#include "GwenGUISupport/gwenUserInterface.h"
#include "../Utils/b3Clock.h"
#include "GwenGUISupport/GwenParameterInterface.h"
#ifndef BT_NO_PROFILE
#include "GwenGUISupport/GwenProfileWindow.h"
#endif
#include "GwenGUISupport/GwenTextureWindow.h"
#include "GwenGUISupport/GraphingTexture.h"
#include "../CommonInterfaces/Common2dCanvasInterface.h"
@@ -68,7 +70,9 @@ struct OpenGLExampleBrowserInternalData
{
Gwen::Renderer::Base* m_gwenRenderer;
CommonGraphicsApp* m_app;
// MyProfileWindow* m_profWindow;
#ifndef BT_NO_PROFILE
MyProfileWindow* m_profWindow;
#endif //BT_NO_PROFILE
btAlignedObjectArray<Gwen::Controls::TreeNode*> m_nodes;
GwenUserInterface* m_gui;
GL3TexLoader* m_myTexLoader;
@@ -93,7 +97,9 @@ static CommonWindowInterface* s_window = 0;
static CommonParameterInterface* s_parameterInterface=0;
static CommonRenderInterface* s_instancingRenderer=0;
static OpenGLGuiHelper* s_guiHelper=0;
//static MyProfileWindow* s_profWindow =0;
#ifndef BT_NO_PROFILE
static MyProfileWindow* s_profWindow =0;
#endif //BT_NO_PROFILE
static SharedMemoryInterface* sSharedMem = 0;
#define DEMO_SELECTION_COMBOBOX 13
@@ -144,6 +150,198 @@ int gGpuArraySizeZ=45;
struct btTiming
{
const char* m_name;
int m_threadId;
unsigned long long int m_usStartTime;
unsigned long long int m_usEndTime;
};
FILE* gTimingFile = 0;
#ifndef __STDC_FORMAT_MACROS
#define __STDC_FORMAT_MACROS
#endif //__STDC_FORMAT_MACROS
#include <inttypes.h>
#define BT_TIMING_CAPACITY 65536
static bool m_firstTiming = true;
struct btTimings
{
btTimings()
:m_numTimings(0),
m_activeBuffer(0)
{
}
void flush()
{
for (int i=0;i<m_numTimings;i++)
{
const char* name = m_timings[m_activeBuffer][i].m_name;
int threadId = m_timings[m_activeBuffer][i].m_threadId;
unsigned long long int startTime = m_timings[m_activeBuffer][i].m_usStartTime;
unsigned long long int endTime = m_timings[m_activeBuffer][i].m_usEndTime;
if (!m_firstTiming)
{
fprintf(gTimingFile,",\n");
}
m_firstTiming = false;
unsigned long long int startTimeDiv1000 = startTime/1000;
unsigned long long int endTimeDiv1000 = endTime/1000;
#if 0
fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".123 ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
threadId, startTimeDiv1000, name);
fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".234 ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
threadId, endTimeDiv1000,name);
#else
if (startTime>endTime)
{
endTime = startTime;
}
unsigned int startTimeRem1000 = startTime%1000;
unsigned int endTimeRem1000 = endTime%1000;
char startTimeRem1000Str[16];
char endTimeRem1000Str[16];
if (startTimeRem1000<10)
{
sprintf(startTimeRem1000Str,"00%d",startTimeRem1000);
}
else
{
if (startTimeRem1000<100)
{
sprintf(startTimeRem1000Str,"0%d",startTimeRem1000);
} else
{
sprintf(startTimeRem1000Str,"%d",startTimeRem1000);
}
}
if (endTimeRem1000<10)
{
sprintf(endTimeRem1000Str,"00%d",endTimeRem1000);
}
else
{
if (endTimeRem1000<100)
{
sprintf(endTimeRem1000Str,"0%d",endTimeRem1000);
} else
{
sprintf(endTimeRem1000Str,"%d",endTimeRem1000);
}
}
char newname[1024];
static int counter2=0;
sprintf(newname,"%s%d",name,counter2++);
fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
threadId, startTimeDiv1000,startTimeRem1000Str, newname);
fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
threadId, endTimeDiv1000,endTimeRem1000Str,newname);
#endif
}
m_numTimings = 0;
}
void addTiming(const char* name, int threadId, unsigned long long int startTime, unsigned long long int endTime)
{
if (m_numTimings>=BT_TIMING_CAPACITY)
{
return;
}
int slot = m_numTimings++;
m_timings[m_activeBuffer][slot].m_name = name;
m_timings[m_activeBuffer][slot].m_threadId = threadId;
m_timings[m_activeBuffer][slot].m_usStartTime = startTime;
m_timings[m_activeBuffer][slot].m_usEndTime = endTime;
}
int m_numTimings;
int m_activeBuffer;
btTiming m_timings[2][BT_TIMING_CAPACITY];
};
btTimings gTimings[BT_MAX_THREAD_COUNT];
btClock clk;
#define MAX_NESTING 1024
bool gProfileDisabled = true;
int gStackDepths[BT_MAX_THREAD_COUNT] = {0};
const char* gFuncNames[BT_MAX_THREAD_COUNT][MAX_NESTING];
unsigned long long int gStartTimes[BT_MAX_THREAD_COUNT][MAX_NESTING];
void MyDummyEnterProfileZoneFunc(const char* msg)
{
}
void MyDummyLeaveProfileZoneFunc()
{
}
void MyEnterProfileZoneFunc(const char* msg)
{
if (gProfileDisabled)
return;
int threadId = btGetCurrentThreadIndex();
if (gStackDepths[threadId]>=MAX_NESTING)
{
btAssert(0);
return;
}
gFuncNames[threadId][gStackDepths[threadId]] = msg;
gStartTimes[threadId][gStackDepths[threadId]] = clk.getTimeNanoseconds();
if (gStartTimes[threadId][gStackDepths[threadId]]<=gStartTimes[threadId][gStackDepths[threadId]-1])
{
gStartTimes[threadId][gStackDepths[threadId]]=1+gStartTimes[threadId][gStackDepths[threadId]-1];
}
gStackDepths[threadId]++;
}
void MyLeaveProfileZoneFunc()
{
if (gProfileDisabled)
return;
int threadId = btGetCurrentThreadIndex();
if (gStackDepths[threadId]<=0)
{
return;
}
gStackDepths[threadId]--;
const char* name = gFuncNames[threadId][gStackDepths[threadId]];
unsigned long long int startTime = gStartTimes[threadId][gStackDepths[threadId]];
unsigned long long int endTime = clk.getTimeNanoseconds();
gTimings[threadId].addTiming(name,threadId,startTime,endTime);
}
void deleteDemo()
{
if (sCurrentDemo)
@@ -154,6 +352,7 @@ void deleteDemo()
sCurrentDemo=0;
delete s_guiHelper;
s_guiHelper = 0;
// CProfileManager::CleanupMemory();
}
}
@@ -237,6 +436,46 @@ void MyKeyboardCallback(int key, int state)
singleStepSimulation = true;
}
if (key=='p')
{
if (state)
{
m_firstTiming = true;
gProfileDisabled = false;//true;
b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
//also for Bullet 2.x API
btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
} else
{
b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
//also for Bullet 2.x API
btSetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
char fileName[1024];
static int fileCounter = 0;
sprintf(fileName,"timings_%d.json",fileCounter++);
gTimingFile = fopen(fileName,"w");
fprintf(gTimingFile,"{\"traceEvents\":[\n");
//dump the content to file
for (int i=0;i<BT_MAX_THREAD_COUNT;i++)
{
if (gTimings[i].m_numTimings)
{
printf("Writing %d timings for thread %d\n", gTimings[i].m_numTimings, i);
gTimings[i].flush();
}
}
fprintf(gTimingFile,"\n],\n\"displayTimeUnit\": \"ns\"}");
fclose(gTimingFile);
gTimingFile = 0;
}
}
#ifndef NO_OPENGL3
if (key=='s' && state)
@@ -333,15 +572,7 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa
void openFileDemo(const char* filename)
{
if (sCurrentDemo)
{
sCurrentDemo->exitPhysics();
s_instancingRenderer->removeAllInstances();
delete sCurrentDemo;
sCurrentDemo=0;
delete s_guiHelper;
s_guiHelper = 0;
}
deleteDemo();
s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2);
s_parameterInterface->removeAllParameters();
@@ -387,6 +618,7 @@ void selectDemo(int demoIndex)
demoIndex = 0;
}
deleteDemo();
CommonExampleInterface::CreateFunc* func = gAllExamples->getExampleCreateFunc(demoIndex);
if (func)
@@ -775,6 +1007,8 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser()
gAllExamples = 0;
}
#include "EmptyExample.h"
bool OpenGLExampleBrowser::init(int argc, char* argv[])
@@ -886,6 +1120,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
b3SetCustomPrintfFunc(MyGuiPrintf);
b3SetCustomErrorMessageFunc(MyStatusBarError);
assert(glGetError()==GL_NO_ERROR);
@@ -940,10 +1175,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
//gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader);
// s_profWindow= setupProfileWindow(gui2->getInternalData());
//m_internalData->m_profWindow = s_profWindow;
// profileWindowSetVisible(s_profWindow,false);
#ifndef BT_NO_PROFILE
s_profWindow= setupProfileWindow(gui2->getInternalData());
m_internalData->m_profWindow = s_profWindow;
profileWindowSetVisible(s_profWindow,false);
#endif //BT_NO_PROFILE
gui2->setFocus();
s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui2->getInternalData());
@@ -1073,7 +1309,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
}
CommonExampleInterface* OpenGLExampleBrowser::getCurrentExample()
{
btAssert(sCurrentDemo);
@@ -1087,6 +1322,8 @@ bool OpenGLExampleBrowser::requestedExit()
void OpenGLExampleBrowser::update(float deltaTime)
{
gProfileDisabled = false;
B3_PROFILE("OpenGLExampleBrowser::update");
assert(glGetError()==GL_NO_ERROR);
s_instancingRenderer->init();
@@ -1136,7 +1373,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
{
if (!pauseSimulation || singleStepSimulation)
{
singleStepSimulation = false;
//printf("---------------------------------------------------\n");
//printf("Framecount = %d\n",frameCount);
B3_PROFILE("sCurrentDemo->stepSimulation");
@@ -1167,7 +1404,8 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
BT_PROFILE("Render Scene");
sCurrentDemo->renderScene();
} else
}
//else
{
B3_PROFILE("physicsDebugDraw");
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -1198,9 +1436,18 @@ void OpenGLExampleBrowser::update(float deltaTime)
if (renderGui)
{
B3_PROFILE("renderGui");
// if (!pauseSimulation)
// processProfileData(s_profWindow,false);
#ifndef BT_NO_PROFILE
if (!pauseSimulation || singleStepSimulation)
{
if (isProfileWindowVisible(s_profWindow))
{
processProfileData(s_profWindow,false);
}
}
#endif //#ifndef BT_NO_PROFILE
if (sUseOpenGL2)
{
@@ -1219,7 +1466,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
singleStepSimulation = false;
toggle=1-toggle;

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

212
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
@@ -718,7 +729,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -1306,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();
@@ -4052,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();
@@ -4208,39 +4338,46 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
if (m_data->m_KukaId>=0)
{
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
#if 1
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
// loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#if 1
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
@@ -4255,12 +4392,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
}
}
}
}
#endif
#if 1
for (int i = 0; i < 6; i++)
{
loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
#endif
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
@@ -4274,6 +4413,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 0, 0);
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
@@ -4282,9 +4425,17 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
kukaBody = m_data->getHandle(m_data->m_KukaId);
}
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
gripperBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0, 0, 0.05);
btMatrix3x3 frameInParent;
@@ -4297,16 +4448,20 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
}
}
}
#if 0
for (int i = 0; i < 10; i++)
{
loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
@@ -4340,6 +4495,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());

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);
};

214
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";
@@ -111,7 +111,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
// b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
@@ -125,7 +125,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
}
}
@@ -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;
@@ -344,11 +355,37 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
if (deltaTimeInSeconds>clampedDeltaTime)
{
deltaTimeInSeconds = clampedDeltaTime;
b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
//b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
}
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;
}
@@ -1060,7 +1097,7 @@ m_options(options)
#endif
m_multiThreadedHelper = helper;
b3Printf("Started PhysicsServer\n");
// b3Printf("Started PhysicsServer\n");
}
@@ -1394,6 +1431,32 @@ extern double gSubStep;
extern int gHuskyId;
extern btTransform huskyTr;
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;
}
};
void PhysicsServerExample::drawUserDebugLines()
{
@@ -1408,6 +1471,14 @@ void PhysicsServerExample::drawUserDebugLines()
if (m_multiThreadedHelper)
{
//if gBatchUserDebugLines is true, batch lines based on color+width, to reduce line draw calls
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,11 +1831,35 @@ 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)
{
gResetSimulation = true;
//gResetSimulation = true;
//gVRTeleportPos1 = gLastPickPos;
}
} else
@@ -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

19
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -21,6 +21,7 @@
#include "LinearMath/btIDebugDraw.h"
int gSharedMemoryKey = -1;
int gDebugDrawFlags = 0;
bool gDisplayDistortion = false;
//how can you try typing on a keyboard, without seeing it?
//it is pretty funny, to see the desktop in VR!
@@ -704,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
@@ -831,15 +832,31 @@ void CMainApplication::RenderFrame()
DrawControllers();
}
RenderStereoTargets();
if (gDisplayDistortion)
{
B3_PROFILE("RenderDistortion");
RenderDistortion();
} else
{
glBindFramebuffer( GL_FRAMEBUFFER, 0 );
glDisable( GL_MULTISAMPLE );
glBindFramebuffer(GL_READ_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
glBlitFramebuffer( 0, 0, m_nRenderWidth, m_nRenderHeight, 0, 0, m_nRenderWidth, m_nRenderHeight,
GL_COLOR_BUFFER_BIT,
GL_LINEAR );
glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0 );
}
vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
vr::Texture_t rightEyeTexture = {(void*)rightEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture );
}
if ( m_bVblank && m_bGlFinishHack )

60
examples/Utils/b3Clock.cpp
View File

@@ -47,7 +47,6 @@ struct b3ClockData
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER mClockFrequency;
DWORD mStartTick;
LONGLONG mPrevElapsedTime;
LARGE_INTEGER mStartTime;
#else
#ifdef __CELLOS_LV2__
@@ -94,7 +93,6 @@ void b3Clock::reset()
#ifdef B3_USE_WINDOWS_TIMERS
QueryPerformanceCounter(&m_data->mStartTime);
m_data->mStartTick = GetTickCount();
m_data->mPrevElapsedTime = 0;
#else
#ifdef __CELLOS_LV2__
@@ -121,27 +119,7 @@ unsigned long int b3Clock::getTimeMilliseconds()
// Compute the number of millisecond ticks elapsed.
unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
// Check for unexpected leaps in the Win32 performance counter.
// (This is caused by unexpected data across the PCI to ISA
// bridge, aka south bridge. See Microsoft KB274323.)
unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
signed long msecOff = (signed long)(msecTicks - elapsedTicks);
if (msecOff < -100 || msecOff > 100)
{
// Adjust the starting time forwards.
LONGLONG msecAdjustment = b3ClockMin(msecOff *
m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
m_data->mPrevElapsedTime);
m_data->mStartTime.QuadPart += msecAdjustment;
elapsedTime -= msecAdjustment;
// Recompute the number of millisecond ticks elapsed.
msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
}
// Store the current elapsed time for adjustments next time.
m_data->mPrevElapsedTime = elapsedTime;
return msecTicks;
#else
@@ -170,38 +148,16 @@ unsigned long int b3Clock::getTimeMilliseconds()
unsigned long long int b3Clock::getTimeMicroseconds()
{
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
QueryPerformanceCounter(&currentTime);
LONGLONG elapsedTime = currentTime.QuadPart -
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
// Compute the number of millisecond ticks elapsed.
unsigned long long msecTicks = (unsigned long long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
// Check for unexpected leaps in the Win32 performance counter.
// (This is caused by unexpected data across the PCI to ISA
// bridge, aka south bridge. See Microsoft KB274323.)
unsigned long long elapsedTicks = GetTickCount() - m_data->mStartTick;
signed long long msecOff = (signed long)(msecTicks - elapsedTicks);
if (msecOff < -100 || msecOff > 100)
{
// Adjust the starting time forwards.
LONGLONG msecAdjustment = b3ClockMin(msecOff *
m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
m_data->mPrevElapsedTime);
m_data->mStartTime.QuadPart += msecAdjustment;
elapsedTime -= msecAdjustment;
}
// Store the current elapsed time for adjustments next time.
m_data->mPrevElapsedTime = elapsedTime;
// Convert to microseconds.
unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
return usecTicks;
return (unsigned long long) elapsedTime.QuadPart;
#else
#ifdef __CELLOS_LV2__

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);

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

@@ -0,0 +1,57 @@
# 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
CONTROLLER_ID = 0
POSITION=1
BUTTONS=6
#assume that the VR physics server is already started before
p.connect(p.SHARED_MEMORY)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
prevPosition=[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[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]]=widths[e[0]]+1
if (widths[e[CONTROLLER_ID]]>20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
p.resetSimulation()
#p.setGravity(0,0,-10)
p.removeAllUserDebugItems()
p.loadURDF("plane.urdf")
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
pt = prevPosition[e[CONTROLLER_ID]]
#print(prevPosition[e[0]])
#print(e[1])
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
ptDistThreshold = 0.01
if (lenSqr>(ptDistThreshold*ptDistThreshold)):
p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
#p.loadURDF("cube_small.urdf",e[1])
colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]]
prevPosition[e[CONTROLLER_ID]] = e[POSITION]

3
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -239,10 +239,7 @@ public:
virtual bool processOverlap(btBroadphasePair& pair)
{
BT_PROFILE("btCollisionDispatcher::processOverlap");
(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
return false;
}
};

2
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -257,7 +257,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
int iObj = collisionObject->getWorldArrayIndex();
btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
// btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
if (iObj >= 0 && iObj < m_collisionObjects.size())
{
btAssert(collisionObject == m_collisionObjects[iObj]);

298
src/LinearMath/btQuickprof.cpp
View File

@@ -16,9 +16,7 @@
#include "btQuickprof.h"
#if BT_THREADSAFE
#include "btThreads.h"
#endif //#if BT_THREADSAFE
#ifdef __CELLOS_LV2__
@@ -30,6 +28,9 @@
#if defined (SUNOS) || defined (__SUNOS__)
#include <stdio.h>
#endif
#ifdef __APPLE__
#include <mach/mach_time.h>
#endif
#if defined(WIN32) || defined(_WIN32)
@@ -55,6 +56,12 @@
#else //_WIN32
#include <sys/time.h>
#ifdef BT_LINUX_REALTIME
//required linking against rt (librt)
#include <time.h>
#endif //BT_LINUX_REALTIME
#endif //_WIN32
#define mymin(a,b) (a > b ? a : b)
@@ -65,12 +72,14 @@ struct btClockData
#ifdef BT_USE_WINDOWS_TIMERS
LARGE_INTEGER mClockFrequency;
LONGLONG mStartTick;
LONGLONG mPrevElapsedTime;
LARGE_INTEGER mStartTime;
#else
#ifdef __CELLOS_LV2__
uint64_t mStartTime;
#else
#ifdef __APPLE__
uint64_t mStartTimeNano;
#endif
struct timeval mStartTime;
#endif
#endif //__CELLOS_LV2__
@@ -111,7 +120,6 @@ void btClock::reset()
#ifdef BT_USE_WINDOWS_TIMERS
QueryPerformanceCounter(&m_data->mStartTime);
m_data->mStartTick = GetTickCount64();
m_data->mPrevElapsedTime = 0;
#else
#ifdef __CELLOS_LV2__
@@ -121,6 +129,9 @@ void btClock::reset()
SYS_TIMEBASE_GET( newTime );
m_data->mStartTime = newTime;
#else
#ifdef __APPLE__
m_data->mStartTimeNano = mach_absolute_time();
#endif
gettimeofday(&m_data->mStartTime, 0);
#endif
#endif
@@ -128,7 +139,7 @@ void btClock::reset()
/// Returns the time in ms since the last call to reset or since
/// the btClock was created.
unsigned long int btClock::getTimeMilliseconds()
unsigned long long int btClock::getTimeMilliseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
@@ -138,27 +149,6 @@ unsigned long int btClock::getTimeMilliseconds()
// Compute the number of millisecond ticks elapsed.
unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
// Check for unexpected leaps in the Win32 performance counter.
// (This is caused by unexpected data across the PCI to ISA
// bridge, aka south bridge. See Microsoft KB274323.)
unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
signed long msecOff = (signed long)(msecTicks - elapsedTicks);
if (msecOff < -100 || msecOff > 100)
{
// Adjust the starting time forwards.
LONGLONG msecAdjustment = mymin(msecOff *
m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
m_data->mPrevElapsedTime);
m_data->mStartTime.QuadPart += msecAdjustment;
elapsedTime -= msecAdjustment;
// Recompute the number of millisecond ticks elapsed.
msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
}
// Store the current elapsed time for adjustments next time.
m_data->mPrevElapsedTime = elapsedTime;
return msecTicks;
#else
@@ -184,41 +174,19 @@ unsigned long int btClock::getTimeMilliseconds()
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long int btClock::getTimeMicroseconds()
unsigned long long int btClock::getTimeMicroseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
QueryPerformanceCounter(&currentTime);
LONGLONG elapsedTime = currentTime.QuadPart -
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
// Compute the number of millisecond ticks elapsed.
unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
// Check for unexpected leaps in the Win32 performance counter.
// (This is caused by unexpected data across the PCI to ISA
// bridge, aka south bridge. See Microsoft KB274323.)
unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
signed long msecOff = (signed long)(msecTicks - elapsedTicks);
if (msecOff < -100 || msecOff > 100)
{
// Adjust the starting time forwards.
LONGLONG msecAdjustment = mymin(msecOff *
m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
m_data->mPrevElapsedTime);
m_data->mStartTime.QuadPart += msecAdjustment;
elapsedTime -= msecAdjustment;
}
// Store the current elapsed time for adjustments next time.
m_data->mPrevElapsedTime = elapsedTime;
// Convert to microseconds.
unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
return usecTicks;
return (unsigned long long) elapsedTime.QuadPart;
#else
#ifdef __CELLOS_LV2__
@@ -240,6 +208,66 @@ unsigned long int btClock::getTimeMicroseconds()
#endif
}
unsigned long long int btClock::getTimeNanoseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1e9;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
return (unsigned long long) elapsedTime.QuadPart;
#else
#ifdef __CELLOS_LV2__
uint64_t freq=sys_time_get_timebase_frequency();
double dFreq=((double) freq)/ 1e9;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET( newTime );
return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
#else
#ifdef __APPLE__
uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano;
static long double conversion = 0.0L;
if( 0.0L == conversion )
{
// attempt to get conversion to nanoseconds
mach_timebase_info_data_t info;
int err = mach_timebase_info( &info );
if( err )
{
btAssert(0);
conversion = 1.;
}
conversion = info.numer / info.denom;
}
return (ticks * conversion);
#else//__APPLE__
#ifdef BT_LINUX_REALTIME
timespec ts;
clock_gettime(CLOCK_REALTIME,&ts);
return 1000000000*ts.tv_sec + ts.tv_nsec;
#else
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec)*1000;
#endif //BT_LINUX_REALTIME
#endif//__APPLE__
#endif//__CELLOS_LV2__
#endif
}
/// Returns the time in s since the last call to reset or since
@@ -370,6 +398,7 @@ bool CProfileNode::Return( void )
if ( --RecursionCounter == 0 && TotalCalls != 0 ) {
unsigned long int time;
Profile_Get_Ticks(&time);
time-=StartTime;
TotalTime += (float)time / Profile_Get_Tick_Rate();
}
@@ -437,11 +466,69 @@ void CProfileIterator::Enter_Parent( void )
**
***************************************************************************************************/
CProfileNode CProfileManager::Root( "Root", NULL );
CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root;
#include "btThreads.h"
CProfileNode gRoots[BT_MAX_THREAD_COUNT]={
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL)
};
CProfileNode* gCurrentNodes[BT_MAX_THREAD_COUNT]=
{
&gRoots[ 0], &gRoots[ 1], &gRoots[ 2], &gRoots[ 3],
&gRoots[ 4], &gRoots[ 5], &gRoots[ 6], &gRoots[ 7],
&gRoots[ 8], &gRoots[ 9], &gRoots[10], &gRoots[11],
&gRoots[12], &gRoots[13], &gRoots[14], &gRoots[15],
&gRoots[16], &gRoots[17], &gRoots[18], &gRoots[19],
&gRoots[20], &gRoots[21], &gRoots[22], &gRoots[23],
&gRoots[24], &gRoots[25], &gRoots[26], &gRoots[27],
&gRoots[28], &gRoots[29], &gRoots[30], &gRoots[31],
&gRoots[32], &gRoots[33], &gRoots[34], &gRoots[35],
&gRoots[36], &gRoots[37], &gRoots[38], &gRoots[39],
&gRoots[40], &gRoots[41], &gRoots[42], &gRoots[43],
&gRoots[44], &gRoots[45], &gRoots[46], &gRoots[47],
&gRoots[48], &gRoots[49], &gRoots[50], &gRoots[51],
&gRoots[52], &gRoots[53], &gRoots[54], &gRoots[55],
&gRoots[56], &gRoots[57], &gRoots[58], &gRoots[59],
&gRoots[60], &gRoots[61], &gRoots[62], &gRoots[63],
};
int CProfileManager::FrameCounter = 0;
unsigned long int CProfileManager::ResetTime = 0;
CProfileIterator * CProfileManager::Get_Iterator( void )
{
int threadIndex = btGetCurrentThreadIndex();
return new CProfileIterator( &gRoots[threadIndex]);
}
void CProfileManager::CleanupMemory(void)
{
for (int i=0;i<BT_MAX_THREAD_COUNT;i++)
{
gRoots[i].CleanupMemory();
}
}
/***********************************************************************************************
* CProfileManager::Start_Profile -- Begin a named profile *
@@ -458,19 +545,12 @@ unsigned long int CProfileManager::ResetTime = 0;
*=============================================================================================*/
void CProfileManager::Start_Profile( const char * name )
{
#if BT_THREADSAFE
// profile system is not designed for profiling multiple threads
// disable collection on all but the main thread
if ( !btIsMainThread() )
{
return;
}
#endif //#if BT_THREADSAFE
if (name != CurrentNode->Get_Name()) {
CurrentNode = CurrentNode->Get_Sub_Node( name );
int threadIndex = btGetCurrentThreadIndex();
if (name != gCurrentNodes[threadIndex]->Get_Name()) {
gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name );
}
CurrentNode->Call();
gCurrentNodes[threadIndex]->Call();
}
@@ -479,22 +559,26 @@ void CProfileManager::Start_Profile( const char * name )
*=============================================================================================*/
void CProfileManager::Stop_Profile( void )
{
#if BT_THREADSAFE
// profile system is not designed for profiling multiple threads
// disable collection on all but the main thread
if ( !btIsMainThread() )
{
return;
}
#endif //#if BT_THREADSAFE
int threadIndex = btGetCurrentThreadIndex();
// Return will indicate whether we should back up to our parent (we may
// be profiling a recursive function)
if (CurrentNode->Return()) {
CurrentNode = CurrentNode->Get_Parent();
if (gCurrentNodes[threadIndex]->Return()) {
gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent();
}
}
void btEnterProfileZoneDefault(const char* name)
{
CProfileManager::Start_Profile( name );
}
void btLeaveProfileZoneDefault()
{
CProfileManager::Stop_Profile();
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
@@ -503,8 +587,8 @@ void CProfileManager::Stop_Profile( void )
void CProfileManager::Reset( void )
{
gProfileClock.reset();
Root.Reset();
Root.Call();
gRoots[btGetCurrentThreadIndex()].Reset();
gRoots[btGetCurrentThreadIndex()].Call();
FrameCounter = 0;
Profile_Get_Ticks(&ResetTime);
}
@@ -592,6 +676,56 @@ void CProfileManager::dumpAll()
}
#else
void btEnterProfileZoneDefault(const char* name)
{
}
void btLeaveProfileZoneDefault()
{
}
#endif //BT_NO_PROFILE
static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault;
static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault;
void btEnterProfileZone(const char* name)
{
(bts_enterFunc)(name);
}
void btLeaveProfileZone()
{
(bts_leaveFunc)();
}
btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc()
{
return bts_enterFunc ;
}
btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc()
{
return bts_leaveFunc;
}
void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc)
{
bts_enterFunc = enterFunc;
}
void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc)
{
bts_leaveFunc = leaveFunc;
}
CProfileSample::CProfileSample( const char * name )
{
btEnterProfileZone(name);
}
CProfileSample::~CProfileSample( void )
{
btLeaveProfileZone();
}

59
src/LinearMath/btQuickprof.h
View File

@@ -36,12 +36,14 @@ public:
/// Returns the time in ms since the last call to reset or since
/// the btClock was created.
unsigned long int getTimeMilliseconds();
unsigned long long int getTimeMilliseconds();
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long int getTimeMicroseconds();
unsigned long long int getTimeMicroseconds();
unsigned long long int getTimeNanoseconds();
/// Returns the time in s since the last call to reset or since
/// the Clock was created.
btScalar getTimeSeconds();
@@ -52,9 +54,18 @@ private:
#endif //USE_BT_CLOCK
typedef void (btEnterProfileZoneFunc)(const char* msg);
typedef void (btLeaveProfileZoneFunc)();
btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc();
btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();
void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
//To disable built-in profiling, please comment out next line
#define BT_NO_PROFILE 1
//#define BT_NO_PROFILE 1
#ifndef BT_NO_PROFILE
#include <stdio.h>//@todo remove this, backwards compatibility
@@ -151,21 +162,21 @@ public:
static void Start_Profile( const char * name );
static void Stop_Profile( void );
static void CleanupMemory(void)
{
Root.CleanupMemory();
}
static void CleanupMemory(void);
// {
// Root.CleanupMemory();
// }
static void Reset( void );
static void Increment_Frame_Counter( void );
static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; }
static float Get_Time_Since_Reset( void );
static CProfileIterator * Get_Iterator( void )
{
return new CProfileIterator( &Root );
}
static CProfileIterator * Get_Iterator( void );
// {
//
// return new CProfileIterator( &Root );
// }
static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
static void dumpRecursive(CProfileIterator* profileIterator, int spacing);
@@ -173,37 +184,27 @@ public:
static void dumpAll();
private:
static CProfileNode Root;
static CProfileNode * CurrentNode;
static int FrameCounter;
static unsigned long int ResetTime;
};
#endif //#ifndef BT_NO_PROFILE
///ProfileSampleClass is a simple way to profile a function's scope
///Use the BT_PROFILE macro at the start of scope to time
class CProfileSample {
public:
CProfileSample( const char * name )
{
CProfileManager::Start_Profile( name );
}
CProfileSample( const char * name );
~CProfileSample( void )
{
CProfileManager::Stop_Profile();
}
~CProfileSample( void );
};
#define BT_PROFILE( name ) CProfileSample __profile( name )
#else
#define BT_PROFILE( name )
#endif //#ifndef BT_NO_PROFILE
#endif //BT_QUICK_PROF_H

20
src/LinearMath/btThreads.cpp
View File

@@ -226,5 +226,23 @@ bool btSpinMutex::tryLock()
return true;
}
#endif // #else // #if BT_THREADSAFE
unsigned int btGetCurrentThreadIndex()
{
const unsigned int kNullIndex = ~0U;
#ifdef _WIN32
__declspec( thread ) static unsigned int sThreadIndex = kNullIndex;
#else
static __thread unsigned int sThreadIndex = kNullIndex;
#endif
static int gThreadCounter=0;
if ( sThreadIndex == kNullIndex )
{
sThreadIndex = gThreadCounter++;
}
return sThreadIndex;
}
#endif // #if BT_THREADSAFE

3
src/LinearMath/btThreads.h
View File

@@ -69,7 +69,8 @@ const unsigned int BT_MAX_THREAD_COUNT = 64;
SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {}
SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {}
SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;}
unsigned int btGetCurrentThreadIndex();
const unsigned int BT_MAX_THREAD_COUNT = 64;
#endif