diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 242c63ccd..674807de8 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -66,9 +66,9 @@ struct CommonGraphicsApp m_mouseYpos(0.f), m_mouseInitialized(false) { - m_backgroundColorRGB[0] = 0.9; - m_backgroundColorRGB[1] = 0.9; - m_backgroundColorRGB[2] = 1; + m_backgroundColorRGB[0] = 0.7; + m_backgroundColorRGB[1] = 0.7; + m_backgroundColorRGB[2] = 0.8; } virtual ~CommonGraphicsApp() { diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6a8c87355..be280c877 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false; int gDebugDrawFlags = 0; static bool pauseSimulation=false; +static bool singleStepSimulation = false; int midiBaseIndex = 176; extern bool gDisableDeactivation; @@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state) { pauseSimulation = !pauseSimulation; } + if (key == 'o' && state) + { + singleStepSimulation = true; + } + + #ifndef NO_OPENGL3 if (key=='s' && state) { @@ -490,7 +497,7 @@ void MyComboBoxCallback(int comboId, const char* item) void MyGuiPrintf(const char* msg) { printf("b3Printf: %s\n",msg); - if (gui2) + if (!gDisableDemoSelection) { gui2->textOutput(msg); gui2->forceUpdateScrollBars(); @@ -502,7 +509,7 @@ void MyGuiPrintf(const char* msg) void MyStatusBarPrintf(const char* msg) { printf("b3Printf: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = true; gui2->setStatusBarMessage(msg,isLeft); @@ -513,7 +520,7 @@ void MyStatusBarPrintf(const char* msg) void MyStatusBarError(const char* msg) { printf("Warning: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = false; gui2->setStatusBarMessage(msg,isLeft); @@ -1103,15 +1110,33 @@ void OpenGLExampleBrowser::update(float deltaTime) s_app->drawText(bla,10,10); } + if (gPngFileName) + { + + static int skip = 0; + skip--; + if (skip<0) + { + skip=gPngSkipFrames; + //printf("gPngFileName=%s\n",gPngFileName); + static int s_frameCount = 100; + + sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); + //b3Printf("Made screenshot %s",staticPngFileName); + s_app->dumpNextFrameToPng(staticPngFileName); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + } + if (sCurrentDemo) { - if (!pauseSimulation) + if (!pauseSimulation || singleStepSimulation) { + singleStepSimulation = false; //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); - - + if (gFixedTimeStep>0) { sCurrentDemo->stepSimulation(gFixedTimeStep); @@ -1145,24 +1170,7 @@ void OpenGLExampleBrowser::update(float deltaTime) } } - if (gPngFileName) - { - - static int skip = 0; - skip--; - if (skip<0) - { - skip=gPngSkipFrames; - //printf("gPngFileName=%s\n",gPngFileName); - static int s_frameCount = 100; - - sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); - //b3Printf("Made screenshot %s",staticPngFileName); - s_app->dumpNextFrameToPng(staticPngFileName); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - } - } - + { diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index d4aa246dc..e49369547 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -84,7 +84,10 @@ public: virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) { - drawLine(PointOnB,PointOnB+normalOnB,color); + drawLine(PointOnB,PointOnB+normalOnB*distance,color); + btVector3 red(0.3, 1., 0.3); + drawLine(PointOnB, PointOnB + normalOnB*0.01, red); + } diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 46b69cc08..de25c1a66 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase struct ImportUrdfInternalData* m_data; bool m_useMultiBody; btAlignedObjectArray m_nameMemory; - + btScalar m_grav; + int m_upAxis; public: ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName); virtual ~ImportUrdfSetup(); @@ -87,7 +88,9 @@ struct ImportUrdfInternalData ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName) - :CommonMultiBodyBase(helper) + :CommonMultiBodyBase(helper), + m_grav(0), + m_upAxis(2) { m_data = new ImportUrdfInternalData; @@ -186,9 +189,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName) void ImportUrdfSetup::initPhysics() { - int upAxis = 2; - m_guiHelper->setUpAxis(upAxis); - + + m_guiHelper->setUpAxis(m_upAxis); + this->createEmptyDynamicsWorld(); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics() );//+btIDebugDraw::DBG_DrawConstraintLimits); - btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; - - m_dynamicsWorld->setGravity(gravity); + if (m_guiHelper->getParameterInterface()) + { + SliderParams slider("Gravity", &m_grav); + slider.m_minVal = -10; + slider.m_maxVal = 10; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + BulletURDFImporter u2b(m_guiHelper, 0); @@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics() if (createGround) { btVector3 groundHalfExtents(20,20,20); - groundHalfExtents[upAxis]=1.f; + groundHalfExtents[m_upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); m_collisionShapes.push_back(box); box->initializePolyhedralFeatures(); @@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics() m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-2.5; + groundOrigin[m_upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); @@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { if (m_dynamicsWorld) { + btVector3 gravity(0, 0, 0); + gravity[m_upAxis] = m_grav; + m_dynamicsWorld->setGravity(gravity); + for (int i=0;im_numMotors;i++) { if (m_data->m_jointMotors[i]) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 367e674da..8503dd41d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { //b3Printf("Fixed joint\n"); - btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); if (enableConstraints) world1->addConstraint(dof6,true); @@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); @@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); if (enableConstraints) @@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter mb->setHasSelfCollision(false); mb->finalizeMultiDof(); - mb->setBaseWorldTransform(rootTransformInWorldSpace); + btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex]; + + mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot); btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; mb->forwardKinematics(scratch_q,scratch_m); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 84b5fc7db..7db87b26a 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -58,7 +58,7 @@ public: virtual void physicsDebugDraw(int debugDrawMode) { - + m_robotSim.debugDraw(debugDrawMode); } virtual void initPhysics() { @@ -81,7 +81,16 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_small.urdf"; + args.m_startPosition.setValue(0, 0, .107); + args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + } + { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; @@ -129,15 +138,7 @@ public: } } } - { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; - args.m_startPosition.setValue(0,0,.107); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_useMultiBody = false; - m_robotSim.loadFile(args,results); - } + if (1) { b3RobotSimLoadFileArgs args(""); @@ -179,19 +180,20 @@ public: m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } - m_robotSim.stepSimulation(); + m_robotSim.stepSimulation(); } virtual void renderScene() { m_robotSim.renderScene(); //m_app->m_renderer->renderScene(); + } virtual void physicsDebugDraw() { - + } virtual bool mouseMoveCallback(float x,float y) { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 15cc08a21..4c825acab 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -7,15 +7,15 @@ //#include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/PhysicsDirect.h" + #include - - - - - - #include "../Utils/b3Clock.h" + + #include "../MultiThreading/b3ThreadSupportInterface.h" @@ -406,24 +406,27 @@ public: - - - struct b3RobotSimAPI_InternalData { //GUIHelperInterface* m_guiHelper; PhysicsServerSharedMemory m_physicsServer; b3PhysicsClientHandle m_physicsClient; + b3ThreadSupportInterface* m_threadSupport; RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; + PhysicsDirect* m_clientServerDirect; + bool m_useMultiThreading; bool m_connected; b3RobotSimAPI_InternalData() - :m_multiThreadedHelper(0), + :m_threadSupport(0), + m_multiThreadedHelper(0), + m_clientServerDirect(0), m_physicsClient(0), + m_useMultiThreading(false), m_connected(false) { } @@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI() void b3RobotSimAPI::processMultiThreadedGraphicsRequests() { + if (0==m_data->m_multiThreadedHelper) + return; + switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) { case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: @@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() } } - - - - #if 0 - if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) - { - btClock rtc; - btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); - - while (rtc.getTimeMilliseconds()m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - - - - m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); - - for (int i=0;im_threadSupport->getNumTasks();i++) + if (m_data->m_useMultiThreading) { - RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i); - b3Assert(storage); - storage->threadId = i; - //storage->m_sharedMem = data->m_sharedMem; - } + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); + + MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); - - for (int w=0;wm_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); - m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized); - int numMoving = 0; - m_data->m_args[w].m_positions.resize(numMoving); - m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; - int index = 0; - - m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w); - while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized) + + m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); + + for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++) { + RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i); + b3Assert(storage); + storage->threadId = i; + //storage->m_sharedMem = data->m_sharedMem; } + + + + + for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++) + { + m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); + m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized); + int numMoving = 0; + m_data->m_args[w].m_positions.resize(numMoving); + m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; + int index = 0; + + m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w); + while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized) + { + } + } + + m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); + + bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); + b3Assert(serverConnected); + + m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + } + else + { + m_data->m_clientServerDirect = new PhysicsDirect(); + bool connected = m_data->m_clientServerDirect->connect(guiHelper); + m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; + } - m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); - - m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper); - - b3Assert(m_data->m_connected); - - m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient); - b3Assert(canSubmit); - return m_data->m_connected && canSubmit; + //client connected + m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient); + + b3Assert(m_data->m_connected); + return m_data->m_connected && m_data->m_connected; } void b3RobotSimAPI::disconnect() { - - for (int i=0;im_useMultiThreading) { - m_data->m_args[i].m_cs->lock(); - m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim); - m_data->m_args[i].m_cs->unlock(); + for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++) + { + m_data->m_args[i].m_cs->lock(); + m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim); + m_data->m_args[i].m_cs->unlock(); + } + int numActiveThreads = MAX_ROBOT_NUM_THREADS; + + while (numActiveThreads) + { + int arg0, arg1; + if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0)) + { + numActiveThreads--; + printf("numActiveThreads = %d\n", numActiveThreads); + + } + else + { + } + }; + + printf("stopping threads\n"); + + delete m_data->m_threadSupport; + m_data->m_threadSupport = 0; } - int numActiveThreads = MAX_ROBOT_NUM_THREADS; - - while (numActiveThreads) - { - int arg0,arg1; - if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0)) - { - numActiveThreads--; - printf("numActiveThreads = %d\n",numActiveThreads); - - } else - { - } - }; - - printf("stopping threads\n"); - - delete m_data->m_threadSupport; - m_data->m_threadSupport = 0; - b3DisconnectSharedMemory(m_data->m_physicsClient); m_data->m_physicsServer.disconnectSharedMemory(true); + m_data->m_connected = false; } + +void b3RobotSimAPI::debugDraw(int debugDrawMode) +{ + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->debugDraw(debugDrawMode); + } +} void b3RobotSimAPI::renderScene() { - if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + if (m_data->m_useMultiThreading) { - m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } } + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->renderScene(); + } m_data->m_physicsServer.renderScene(); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 9a4a14831..d435b2925 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -95,6 +95,7 @@ public: void setGravity(const b3Vector3& gravityAcceleration); void renderScene(); + void debugDraw(int debugDrawMode); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 8dc2f962d..034215bd2 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; - static double startPosY = 1; + static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); startPosY += 2.f; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 16b435f61..85c90bbd7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -81,7 +81,6 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const return bodyJoints->m_jointInfo.size(); } - btAssert(0); return 0; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 05c8c0eed..948b90b90 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect() bool PhysicsDirect::connect() { m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + return true; } +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper) +{ + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return true; +} + +void PhysicsDirect::renderScene() +{ + m_data->m_commandProcessor->renderScene(); +} +void PhysicsDirect::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + ////todo: rename to 'disconnect' void PhysicsDirect::disconnectSharedMemory() { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index aaa2d8639..7e3ae2c94 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -29,9 +29,10 @@ public: virtual ~PhysicsDirect(); - // return true if connection succesfull, can also check 'isConnected' + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper virtual bool connect(); - + ////todo: rename to 'disconnect' virtual void disconnectSharedMemory(); @@ -62,7 +63,12 @@ public: virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(b3CameraImageData* cameraData); - + + //those 2 APIs are for internal use for visualization + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + }; #endif //PHYSICS_DIRECT_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5e2970b83..96dc083a7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -696,10 +696,12 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) if (supportsJointMotor(mb,mbLinkIndex)) { - float maxMotorImpulse = 0.f; + float maxMotorImpulse = 10000.f; int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); + motor->setPositionTarget(0, 0); + motor->setVelocityTarget(0, 1); //motor->setMaxAppliedImpulse(0); mb->getLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); @@ -1416,7 +1418,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_TORQUE"); } // mb->clearForcesAndTorques(); - int torqueIndex = 0; + int torqueIndex = 6; if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { for (int link=0;linkgetNumLinks();link++) @@ -1602,6 +1604,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; + if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + break; + } //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed @@ -2437,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) 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); + m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.); } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 59e81ac4c..489d9526f 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene() static int frameCount=0; frameCount++; char bla[1024]; - sprintf(bla,"VR sub-title text test, frame %d", frameCount/2); + + static btScalar prevTime = m_clock.getTimeSeconds(); + btScalar curTime = m_clock.getTimeSeconds(); + static btScalar deltaTime = 0.f; + static int count = 10; + if (count-- < 0) + { + count = 10; + deltaTime = curTime - prevTime; + } + if (deltaTime == 0) + deltaTime = 1000; + + prevTime = curTime; + + sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2); float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); btTransform viewTr; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f7a9812ca..5e2bd41c5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -27,7 +27,7 @@ #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 64 +#define MAX_DEGREE_OF_FREEDOM 128 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024 diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index b81ce57ed..3ed2de4b9 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; - int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0); + int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0; TGAImage& frame = renderData.m_rgbColorBuffer; diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 7482c302d..8e1884526 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -239,7 +239,7 @@ void b3Clock::usleep(int microSeconds) Sleep(millis); #else - usleep(microSeconds); + ::usleep(microSeconds); //struct timeval tv; //tv.tv_sec = microSeconds/1000000L; //tv.tv_usec = microSeconds%1000000L; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 915de51f1..fdc753443 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { int size; int bodyIndex, jointIndex, controlMode; - double targetValue=0; + double targetPosition=0; double targetVelocity=0; double maxForce=100000; + double appliedForce = 0; double kp=0.1; double kd=1.0; int valid = 0; @@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) size= PySequence_Size(args); if (size==4) { - // for CONTROL_MODE_VELOCITY targetValue -> velocity - // for CONTROL_MODE_TORQUE targetValue -> force torque + double targetValue = 0; + // see switch statement below for convertsions dependent on controlMode if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } - valid = 1; + valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + appliedForce = targetValue; + break; + } + default: + { + valid = 0; + } + } + } if (size==5) { - // for CONTROL_MODE_VELOCITY targetValue -> velocity - // for CONTROL_MODE_TORQUE targetValue -> force torque + double targetValue = 0; + //See switch statement for conversions if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } valid = 1; + + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } } if (size==6) { - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd)) + double gain; + double targetValue = 0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } - valid = 1; + valid = 1; + + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } } if (size==8) { @@ -353,16 +427,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) valid = 1; } - if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD) - { - PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD"); - return NULL; - } - if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8) - { - PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity"); - return NULL; - } + if (valid) { @@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce); break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } default: @@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) Py_INCREF(Py_None); return Py_None; } - PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl."); + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); return NULL; } @@ -532,7 +597,7 @@ pybullet_setTimeStep(PyObject* self, PyObject* args) // Internal function used to get the base position and orientation // Orientation is returned in quaternions -static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) +static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) { basePosition[0] = 0.; basePosition[1] = 0.; @@ -552,7 +617,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); const int status_type = b3GetStatusType(status_handle); - + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return 0; + } const double* actualStateQ; // const double* jointReactionForces[]; int i; @@ -579,6 +648,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double } } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion @@ -606,7 +676,11 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) return NULL; } - pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation); + if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation)) + { + PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?)."); + return NULL; + } { @@ -848,42 +922,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - pyListJointInfo = PyTuple_New(jointInfoSize); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, - PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, - PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, - PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, - PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, - PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; + if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) + { + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } } } @@ -934,12 +1010,19 @@ pybullet_getJointState(PyObject* self, PyObject* args) { if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { - + int status_type = 0; b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } + pyListJointState = PyTuple_New(sensorStateSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize); diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 60e49b750..83fe4969f 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -134,6 +134,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) { + m_ghostObject = ghostObject; m_up.setValue(0.0f, 0.0f, 1.0f); m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); setUp(up); @@ -141,8 +142,7 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho m_addedMargin = 0.02; m_walkDirection.setValue(0.0,0.0,0.0); m_AngVel.setValue(0.0, 0.0, 0.0); - m_useGhostObjectSweepTest = true; - m_ghostObject = ghostObject; + m_useGhostObjectSweepTest = true; m_turnAngle = btScalar(0.0); m_convexShape=convexShape; m_useWalkDirection = true; // use walk direction by default, legacy behavior @@ -520,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -529,7 +529,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) { //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -537,11 +537,11 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld } btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - bool has_hit = false; + bool has_hit; if (bounce_fix == true) has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); else - has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); + has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject); btScalar stepHeight = 0.0f; if (m_verticalVelocity < 0.0) @@ -556,7 +556,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_targetPosition = orig_position; downVelocity = stepHeight; - btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests @@ -564,7 +564,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld break; } - if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true) { // we dropped a fraction of the height -> hit floor btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index 58c9838c4..a020c3f4e 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -166,9 +166,9 @@ class btIDebugDraw virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); - drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f)); } virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,