diff --git a/build_visual_studio.bat b/build_visual_studio.bat
index 56ee9f610..0da0089b1 100644
--- a/build_visual_studio.bat
+++ b/build_visual_studio.bat
@@ -14,6 +14,6 @@ rem cd vs2010
rem rename 0_Bullet3Solution.sln 0_client.sln
rem cd ..
rem rename vs2010 vs2010_client
-start vs2010/0_Bullet3Solution.sln
+rem start vs2010/0_Bullet3Solution.sln
pause
diff --git a/data/kuka_iiwa/meshes/coarse/link_0.stl b/data/kuka_iiwa/meshes/coarse/link_0.stl
deleted file mode 100644
index 84b8ea5d2..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_0.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_1.stl b/data/kuka_iiwa/meshes/coarse/link_1.stl
deleted file mode 100644
index ffe3ec981..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_1.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_2.stl b/data/kuka_iiwa/meshes/coarse/link_2.stl
deleted file mode 100644
index 4a51b27ef..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_2.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_3.stl b/data/kuka_iiwa/meshes/coarse/link_3.stl
deleted file mode 100644
index 32d6d5282..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_3.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_4.stl b/data/kuka_iiwa/meshes/coarse/link_4.stl
deleted file mode 100644
index 35d192181..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_4.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_5.stl b/data/kuka_iiwa/meshes/coarse/link_5.stl
deleted file mode 100644
index 35aa1245d..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_5.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_6.stl b/data/kuka_iiwa/meshes/coarse/link_6.stl
deleted file mode 100644
index bce349eb2..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_6.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/coarse/link_7.stl b/data/kuka_iiwa/meshes/coarse/link_7.stl
deleted file mode 100644
index 2d5d6ecfb..000000000
Binary files a/data/kuka_iiwa/meshes/coarse/link_7.stl and /dev/null differ
diff --git a/data/kuka_iiwa/meshes/link_0.stl b/data/kuka_iiwa/meshes/link_0.stl
index 84b8ea5d2..ed1447056 100644
Binary files a/data/kuka_iiwa/meshes/link_0.stl and b/data/kuka_iiwa/meshes/link_0.stl differ
diff --git a/data/kuka_iiwa/meshes/link_1.stl b/data/kuka_iiwa/meshes/link_1.stl
index e8e37de9f..d9d043e69 100644
Binary files a/data/kuka_iiwa/meshes/link_1.stl and b/data/kuka_iiwa/meshes/link_1.stl differ
diff --git a/data/kuka_iiwa/meshes/link_2.stl b/data/kuka_iiwa/meshes/link_2.stl
index 47c7885fc..282aa787b 100644
Binary files a/data/kuka_iiwa/meshes/link_2.stl and b/data/kuka_iiwa/meshes/link_2.stl differ
diff --git a/data/kuka_iiwa/meshes/link_3.stl b/data/kuka_iiwa/meshes/link_3.stl
index 027eb2211..50d856cb8 100644
Binary files a/data/kuka_iiwa/meshes/link_3.stl and b/data/kuka_iiwa/meshes/link_3.stl differ
diff --git a/data/kuka_iiwa/meshes/link_4.stl b/data/kuka_iiwa/meshes/link_4.stl
index c0c1213c1..ade9b0cf1 100644
Binary files a/data/kuka_iiwa/meshes/link_4.stl and b/data/kuka_iiwa/meshes/link_4.stl differ
diff --git a/data/kuka_iiwa/meshes/link_5.stl b/data/kuka_iiwa/meshes/link_5.stl
index 82a9337a6..663ece5cd 100644
Binary files a/data/kuka_iiwa/meshes/link_5.stl and b/data/kuka_iiwa/meshes/link_5.stl differ
diff --git a/data/kuka_iiwa/meshes/link_6.stl b/data/kuka_iiwa/meshes/link_6.stl
index 10b558dc5..7fb9fcad2 100644
Binary files a/data/kuka_iiwa/meshes/link_6.stl and b/data/kuka_iiwa/meshes/link_6.stl differ
diff --git a/data/kuka_iiwa/meshes/link_7.stl b/data/kuka_iiwa/meshes/link_7.stl
index 5909e7e01..e4e301b53 100644
Binary files a/data/kuka_iiwa/meshes/link_7.stl and b/data/kuka_iiwa/meshes/link_7.stl differ
diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf
index 56123c36d..d8fe60293 100644
--- a/data/kuka_iiwa/model.urdf
+++ b/data/kuka_iiwa/model.urdf
@@ -77,7 +77,7 @@
-
+
@@ -106,7 +106,7 @@
-
+
@@ -135,7 +135,7 @@
-
+
@@ -164,7 +164,7 @@
-
+
@@ -193,7 +193,7 @@
-
+
@@ -222,7 +222,7 @@
-
+
@@ -251,7 +251,7 @@
-
+
@@ -280,7 +280,7 @@
-
+
diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h
index f3fb20047..14d19ec3d 100644
--- a/examples/CommonInterfaces/CommonExampleInterface.h
+++ b/examples/CommonInterfaces/CommonExampleInterface.h
@@ -46,6 +46,8 @@ public:
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=0;
+ virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]) {}
+ virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
};
class ExampleEntries
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp
index 64503dcc3..e9c5e38fa 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp
@@ -16,9 +16,9 @@ subject to the following restrictions:
///todo: make this configurable in the gui
-bool useShadowMap=true;//false;//true;
+bool useShadowMap = true;// true;//false;//true;
int shadowMapWidth=4096;//8192;
-int shadowMapHeight=4096;
+int shadowMapHeight= 4096;
float shadowMapWorldSize=25;
#define MAX_POINTS_IN_BATCH 1024
@@ -363,7 +363,7 @@ void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcI
void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
{
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
+ //glFlush();
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
//b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
@@ -393,7 +393,7 @@ void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, fl
orientations [objectIndex*4+3] = orientation[3];
glUnmapBuffer( GL_ARRAY_BUFFER);
- glFlush();
+ //glFlush();
}
@@ -500,7 +500,7 @@ void GLInstancingRenderer::writeTransforms()
glUnmapBuffer( GL_ARRAY_BUFFER);
//if this glFinish is removed, the animation is not always working/blocks
//@todo: figure out why
- glFlush();
+ //glFlush();
#endif
@@ -1037,7 +1037,7 @@ void GLInstancingRenderer::renderScene()
//avoid some Intel driver on a Macbook Pro to lock-up
//todo: figure out what is going on on that machine
- glFlush();
+ //glFlush();
if (useShadowMap)
{
@@ -1539,7 +1539,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
B3_PROFILE("glFlush2");
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
+ //glFlush();
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1730,7 +1730,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
B3_PROFILE("glFlush");
- glFlush();
+ //glFlush();
}
if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE)
{
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 5a728b0cb..1ff24b66c 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -115,6 +115,15 @@ int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, doub
return 0;
}
+int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_physSimParamArgs.m_allowRealTimeSimulation = enableRealTimeSimulation;
+ command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
+ return 0;
+}
+
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index dcbe9c30c..a3b6dbd5c 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -81,6 +81,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 5c82e3381..420fe37b6 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -374,6 +374,8 @@ struct PhysicsServerCommandProcessorInternalData
///end handle management
+ bool m_allowRealTimeSimulation;
+ bool m_hasGround;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -417,7 +419,8 @@ struct PhysicsServerCommandProcessorInternalData
TinyRendererVisualShapeConverter m_visualConverter;
PhysicsServerCommandProcessorInternalData()
- :
+ :m_hasGround(false),
+ m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@@ -496,6 +499,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
}
}
m_data->m_guiHelper = guiHelper;
+
+
+
}
@@ -504,6 +510,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
+
}
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -983,7 +990,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
-
+
bool hasStatus = false;
{
@@ -1768,6 +1775,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
}
+ if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
+ {
+ m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation;
+ }
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
{
btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
@@ -1873,7 +1884,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
-
+ m_data->m_hasGround = false;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -2307,6 +2318,26 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
+void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
+{
+ if (m_data->m_allowRealTimeSimulation)
+ {
+ if (!m_data->m_hasGround)
+ {
+ m_data->m_hasGround = true;
+
+ int bodyId = 0;
+ btAlignedObjectArray bufferServerToClient;
+ bufferServerToClient.resize(32768);
+
+
+ loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
+ }
+
+ m_data->m_dynamicsWorld->stepSimulation(dtInSec);
+ }
+}
+
void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
{
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index bc74a0a27..7cbc87ab4 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h
@@ -53,7 +53,8 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
- void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+ void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+ void stepSimulationRealTime(double dtInSec);
void applyJointDamping(int bodyUniqueId);
};
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index a4ac05f67..6ae9a654d 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -9,11 +9,12 @@
#include "PhysicsServerSharedMemory.h"
#include "SharedMemoryCommon.h"
-
+#include "Bullet3Common/b3Matrix3x3.h"
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
+
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
#define MAX_MOTION_NUM_THREADS 1
@@ -80,13 +81,23 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
struct MotionArgs
{
MotionArgs()
- :m_physicsServerPtr(0)
+ :m_physicsServerPtr(0),
+ m_isPicking(false),
+ m_isDragging(false),
+ m_isReleasing(false)
{
}
b3CriticalSection* m_cs;
PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray m_positions;
+
+ btVector3 m_pos;
+ btQuaternion m_orn;
+ bool m_isPicking;
+ bool m_isDragging;
+ bool m_isReleasing;
+
};
struct MotionThreadLocalStorage
@@ -113,23 +124,58 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_cs->setSharedParam(0,eMotionIsInitialized);
args->m_cs->unlock();
+
do
{
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
-#if 0
+
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
- if (deltaTimeInSeconds<(1./260.))
+ if (deltaTimeInSeconds<(1./5000.))
{
skip++;
- if (deltaTimeInSeconds<.001)
- continue;
+ } else
+ {
+ //process special controller commands, such as
+ //VR controller button press/release and controller motion
+
+ btVector3 from = args->m_pos;
+ btMatrix3x3 mat(args->m_orn);
+
+ btVector3 toX = args->m_pos+mat.getColumn(0);
+ btVector3 toY = args->m_pos+mat.getColumn(1);
+ btVector3 toZ = args->m_pos+mat.getColumn(2)*50.;
+
+
+ if (args->m_isPicking)
+ {
+ args->m_isPicking = false;
+ args->m_isDragging = true;
+ args->m_physicsServerPtr->pickBody(from,-toZ);
+ //printf("PICK!\n");
+ }
+
+ if (args->m_isDragging)
+ {
+ args->m_physicsServerPtr->movePickedBody(from,-toZ);
+ // printf(".");
+ }
+
+ if (args->m_isReleasing)
+ {
+ args->m_isDragging = false;
+ args->m_isReleasing = false;
+ args->m_physicsServerPtr->removePickingConstraint();
+ //printf("Release pick\n");
+ }
+
+ //don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
+ btClamp(deltaTimeInSeconds,0.,0.1);
+ args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+ clock.reset();
}
- clock.reset();
-#endif
args->m_physicsServerPtr->processClientCommands();
-
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
} else
{
@@ -375,7 +421,7 @@ class PhysicsServerExample : public SharedMemoryCommon
btClock m_clock;
bool m_replay;
int m_options;
-
+
public:
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
@@ -417,6 +463,9 @@ public:
btVector3 getRayTo(int x,int y);
+ virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]);
+ virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]);
+
virtual bool mouseMoveCallback(float x,float y)
{
if (m_replay)
@@ -720,6 +769,28 @@ void PhysicsServerExample::renderScene()
//m_args[0].m_cs->lock();
m_physicsServer.renderScene();
+
+ if (m_args[0].m_isPicking || m_args[0].m_isDragging)
+ {
+ btVector3 from = m_args[0].m_pos;
+ btMatrix3x3 mat(m_args[0].m_orn);
+
+ btVector3 toX = m_args[0].m_pos+mat.getColumn(0);
+ btVector3 toY = m_args[0].m_pos+mat.getColumn(1);
+ btVector3 toZ = m_args[0].m_pos+mat.getColumn(2);
+
+ int width = 2;
+
+
+ btVector4 color;
+ color=btVector4(1,0,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
+ color=btVector4(0,1,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
+ color=btVector4(0,0,1,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
+
+ }
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
@@ -856,4 +927,17 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
}
-B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
\ No newline at end of file
+void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
+{
+ m_args[0].m_isPicking = (state!=0);
+ m_args[0].m_isReleasing = (state==0);
+ m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
+ m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
+}
+
+void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4])
+{
+ m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
+ m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
+}
+B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
index d0b583969..111ac07f5 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
@@ -236,8 +236,10 @@ void PhysicsServerSharedMemory::releaseSharedMemory()
}
-
-
+void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
+{
+ m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
+}
void PhysicsServerSharedMemory::processClientCommands()
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h
index 2c08124d0..f96e7ca4b 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.h
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.h
@@ -26,6 +26,8 @@ public:
virtual void processClientCommands();
+ virtual void stepSimulationRealTime(double dtInSec);
+
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index d5b50d2ba..64ae61991 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -215,6 +215,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_GRAVITY=2,
SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
+ SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
};
///Controlling a robot involves sending the desired state to its joint motor controllers.
@@ -225,6 +226,7 @@ struct SendPhysicsSimulationParameters
double m_gravityAcceleration[3];
int m_numSimulationSubSteps;
int m_numSolverIterations;
+ bool m_allowRealTimeSimulation;
};
struct RequestActualStateArgs
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index dcb7aa22f..630c2d3e8 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -4,6 +4,8 @@
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "../OpenGLWindow/OpenGLInclude.h"
#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+
#include "../ExampleBrowser/OpenGLGuiHelper.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
@@ -28,6 +30,8 @@ int gSharedMemoryKey = -1;
#include "pathtools.h"
CommonExampleInterface* sExample;
+int sIsPressed=-1;
+int sPrevPacketNum=0;
GUIHelperInterface* sGuiPtr = 0;
@@ -82,7 +86,7 @@ public:
bool BInit();
bool BInitGL();
bool BInitCompositor();
-
+ void getControllerTransform(int unDevice, b3Transform& tr);
void SetupRenderModels();
void Shutdown();
@@ -619,6 +623,26 @@ void CMainApplication::Shutdown()
}
+
+void CMainApplication::getControllerTransform(int unDevice, b3Transform& tr)
+{
+ const Matrix4 & matOrg = m_rmat4DevicePose[unDevice];
+ tr.setIdentity();
+ tr.setOrigin(b3MakeVector3(matOrg[12],matOrg[13],matOrg[14]));//pos[1]));
+ b3Matrix3x3 bmat;
+ for (int i=0;i<3;i++)
+ {
+ for (int j=0;j<3;j++)
+ {
+ bmat[i][j] = matOrg[i+4*j];
+ }
+ }
+ tr.setBasis(bmat);
+ b3Transform y2z;
+ y2z.setIdentity();
+ y2z.setRotation(b3Quaternion(0,B3_HALF_PI,0));
+ tr = y2z*tr;
+}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
@@ -639,14 +663,61 @@ bool CMainApplication::HandleInput()
vr::VRControllerState_t state;
if( m_pHMD->GetControllerState( unDevice, &state ) )
{
- if (state.ulButtonPressed)
+ uint64_t trigger = vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Trigger);
+
+ if (sPrevPacketNum != state.unPacketNum)
{
- b3Printf("state.ulButtonPressed=%d\n",state.ulButtonPressed);
- sExample->exitPhysics();
- m_app->m_instancingRenderer->removeAllInstances();
- sExample->initPhysics();
+
+ static int counter=0;
+ sPrevPacketNum = state.unPacketNum;
+ //b3Printf(".");
+ bool isTrigger = (state.ulButtonPressed&trigger)!=0;
+ if (isTrigger)
+ {
+ //printf("unDevice=%d\n",unDevice);
+ b3Transform tr;
+ getControllerTransform(unDevice,tr);
+ float pos[3] = {tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2]};
+ b3Quaternion born = tr.getRotation();
+ float orn[4] = {born[0],born[1],born[2],born[3]};
+
+
+ if (sIsPressed!=unDevice)
+ {
+ b3Printf("trigger pressed %d, %d\n",counter++, unDevice);
+ sIsPressed=unDevice;
+
+ sExample->vrControllerButtonCallback(unDevice,1,1,pos, orn);
+
+ //
+ //virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]) {}
+ //virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
+
+ //sExample->start
+ } else
+ {
+ sExample->vrControllerMoveCallback(unDevice,pos,orn);
+ }
+ //sExample->exitPhysics();
+ //m_app->m_instancingRenderer->removeAllInstances();
+ ///sExample->initPhysics();
+ } else
+ {
+ if (unDevice==sIsPressed)
+ {
+ b3Transform tr;
+ getControllerTransform(unDevice,tr);
+ float pos[3] = {tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2]};
+ b3Quaternion born = tr.getRotation();
+ float orn[4] = {born[0],born[1],born[2],born[3]};
+
+ sIsPressed=-1;
+ printf("device released: %d",unDevice);
+ sExample->vrControllerButtonCallback(unDevice,1,0,pos, orn);
+ }
+ }
+ }
- }
m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
}
}
@@ -1480,7 +1551,7 @@ void CMainApplication::RenderStereoTargets()
rotYtoZ.rotateX(-90);
}
- RenderScene( vr::Eye_Left );
+
// Left Eye
{
@@ -1527,7 +1598,7 @@ void CMainApplication::RenderStereoTargets()
m_app->m_window->startRendering();
-
+ RenderScene( vr::Eye_Left );
@@ -1560,7 +1631,7 @@ void CMainApplication::RenderStereoTargets()
// Right Eye
- RenderScene( vr::Eye_Right );
+
{
Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ;
@@ -1573,7 +1644,7 @@ void CMainApplication::RenderStereoTargets()
m_app->m_window->startRendering();
-
+ RenderScene( vr::Eye_Right );
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
//m_app->m_renderer->renderScene();
diff --git a/examples/Utils/b3ResourcePath.cpp b/examples/Utils/b3ResourcePath.cpp
index 44952d61a..bc50ef5cb 100644
--- a/examples/Utils/b3ResourcePath.cpp
+++ b/examples/Utils/b3ResourcePath.cpp
@@ -33,8 +33,10 @@ int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
#else
#ifdef _WIN32
//https://msdn.microsoft.com/en-us/library/windows/desktop/ms683197(v=vs.85).aspx
+
HMODULE hModule = GetModuleHandle(NULL);
- numBytes = GetModuleFileName(hModule, path, maxPathLenInBytes);
+ numBytes = GetModuleFileNameA(hModule, path, maxPathLenInBytes);
+
#else
///http://stackoverflow.com/questions/933850/how-to-find-the-location-of-the-executable-in-c
numBytes = (int)readlink("/proc/self/exe", path, maxPathLenInBytes-1);
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 2a2d9b2d1..be5994c16 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -410,6 +410,41 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
+
+static PyObject *
+pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
+{
+ if (0 == sm)
+ {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int enableRealTimeSimulation = 0;
+ int ret;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation))
+ {
+ PyErr_SetString(SpamError, "setRealTimeSimulation expected a single value (integer).");
+ return NULL;
+ }
+ ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+
+
// Set the gravity of the world with (x, y, z) arguments
static PyObject *
pybullet_setGravity(PyObject* self, PyObject* args)
@@ -1453,6 +1488,9 @@ static PyMethodDef SpamMethods[] = {
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
"Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"},
+ { "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS,
+ "Enable or disable real time simulation (using the real time clock, RTC) in the physics server. Expects one integer argument, 0 or 1" },
+
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
diff --git a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
index 264c7b9f0..369f1d750 100644
--- a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
+++ b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
@@ -636,10 +636,10 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
FILETIME modtimeBinary;
- CreateDirectory(sCachedBinaryPath,0);
+ CreateDirectoryA(sCachedBinaryPath,0);
{
- HANDLE binaryFileHandle = CreateFile(binaryFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ HANDLE binaryFileHandle = CreateFileA(binaryFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if (binaryFileHandle ==INVALID_HANDLE_VALUE)
{
DWORD errorCode;
@@ -677,7 +677,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
if (binaryFileValid)
{
- HANDLE srcFileHandle = CreateFile(clFileNameForCaching,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ HANDLE srcFileHandle = CreateFileA(clFileNameForCaching,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if (srcFileHandle==INVALID_HANDLE_VALUE)
{
@@ -686,7 +686,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],clFileNameForCaching);
- srcFileHandle = CreateFile(relativeFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ srcFileHandle = CreateFileA(relativeFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
}
}