From b221d2ad18bfec48efeccbe24302aac206948d7f Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 11:09:12 -0700 Subject: [PATCH 01/12] add targetPosition, targetValue, kp & kd to pybullet_setJointMotorControl --- examples/pybullet/pybullet.c | 72 +++++++++++++++++++++--------------- 1 file changed, 42 insertions(+), 30 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index be5994c16..2bd48e963 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -294,27 +294,29 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - //todo(erwincoumans): set max forces, kp, kd + //TODO(matkelcey): should this just be three methods? int size; int bodyIndex, jointIndex, controlMode; double targetValue=0; + double targetPosition=0; + double targetVelocity=0; double maxForce=100000; - double gains=0.1; + double kp=0.1; + double kd=0.1; int valid = 0; - - + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + size= PySequence_Size(args); - if (size==4) { - + // for CONTROL_MODE_VELOCITY targetValue -> velocity + // for CONTROL_MODE_TORQUE targetValue -> force torque if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { PyErr_SetString(SpamError, "Error parsing arguments"); @@ -324,7 +326,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } if (size==5) { - + // for CONTROL_MODE_VELOCITY targetValue -> velocity + // for CONTROL_MODE_TORQUE targetValue -> force torque if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) { PyErr_SetString(SpamError, "Error parsing arguments"); @@ -332,20 +335,31 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } valid = 1; } - if (size==6) + if (size==8) { - - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains)) + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } 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) { + int numJoints; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -357,7 +371,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } - + if ((controlMode != CONTROL_MODE_VELOCITY) && (controlMode != CONTROL_MODE_TORQUE) && (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) @@ -365,19 +379,18 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Illegral control mode."); return NULL; } - + commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - + switch (controlMode) { case CONTROL_MODE_VELOCITY: { - double kd = gains; b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - b3JointControlSetKd(commandHandle,info.m_uIndex,kd); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } @@ -386,31 +399,30 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); break; } - + case CONTROL_MODE_POSITION_VELOCITY_PD: { - double kp = gains; - b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); - b3JointControlSetKp(commandHandle,info.m_uIndex,kp); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + 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); break; } default: { } }; - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - + Py_INCREF(Py_None); return Py_None; } - PyErr_SetString(SpamError, "error in setJointControl."); + PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl."); return NULL; } - - static PyObject * pybullet_setRealTimeSimulation(PyObject* self, PyObject* args) { From 4b75c5d9d0a95763152f0f9f8bc2d0d4876d7cb2 Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 11:36:34 -0700 Subject: [PATCH 02/12] add another combo of args to allow kd to be configured in the CONTROL_MODE_VELOCITY case --- examples/pybullet/pybullet.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2bd48e963..0a1a205bb 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -335,6 +335,15 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } valid = 1; } + if (size==6) + { + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } if (size==8) { // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. From 98c6181ba85aa90cd98a5c8a87f40b9cfc922766 Mon Sep 17 00:00:00 2001 From: "Erwin Coumans (Google)" Date: Tue, 26 Jul 2016 15:36:21 -0700 Subject: [PATCH 03/12] fix screen width/height issue on Intel/Linux add command-line option to set png_skip_frames when taking screenshot series --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 40 ++++++++++--------- .../InverseKinematicsExample.cpp | 2 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 6 ++- examples/OpenGLWindow/X11OpenGLWindow.cpp | 3 ++ 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 577d24c85..d0fae2254 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -157,6 +157,7 @@ void deleteDemo() } const char* gPngFileName = 0; +int gPngSkipFrames = 0; @@ -763,7 +764,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) loadCurrentSettings(startFileName, args); args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep); - + args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames); ///The OpenCL rigid body pipeline is experimental and ///most OpenCL drivers and OpenCL compilers have issues with our kernels. ///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers @@ -1101,24 +1102,6 @@ void OpenGLExampleBrowser::update(float deltaTime) //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); - if (gPngFileName) - { - - static int skip = 0; - skip++; - if (skip>4) - { - skip=0; - //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 (gFixedTimeStep>0) { @@ -1153,6 +1136,25 @@ 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); + } + } + + { if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera()) diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 8f902b008..4c36037a6 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -84,7 +84,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian) // Update target positions void UpdateTargets( double T2, Tree & treeY) { - double T = T2 / 20.; + double T = T2 / 5.; target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T)); } diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 1423f2841..971df3734 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -753,8 +753,10 @@ void SimpleOpenGL3App::swapBuffer() m_window->endRendering(); if (m_data->m_frameDumpPngFileName) { - writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(), - (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName, + int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); + int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(); + writeTextureToFile(width, + height,m_data->m_frameDumpPngFileName, m_data->m_ffmpegFile); m_data->m_renderTexture->disable(); if (m_data->m_ffmpegFile==0) diff --git a/examples/OpenGLWindow/X11OpenGLWindow.cpp b/examples/OpenGLWindow/X11OpenGLWindow.cpp index 68aa22720..789be7803 100644 --- a/examples/OpenGLWindow/X11OpenGLWindow.cpp +++ b/examples/OpenGLWindow/X11OpenGLWindow.cpp @@ -516,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) m_data->m_dpy = MyXOpenDisplay(NULL); + m_data->m_glWidth = ci.m_width; + m_data->m_glHeight = ci.m_height; + if(m_data->m_dpy == NULL) { printf("\n\tcannot connect to X server\n\n"); exit(0); From b4cfee87453f22b82647d1563aff34a3f59ee4ec Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 16:15:08 -0700 Subject: [PATCH 04/12] set dft for kd to be 1.0. note: this is only applicable to CONTROL_MODE_VELOCITY --- examples/pybullet/pybullet.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0a1a205bb..d2b7a496c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -294,8 +294,6 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - //TODO(matkelcey): should this just be three methods? - int size; int bodyIndex, jointIndex, controlMode; double targetValue=0; @@ -303,7 +301,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) double targetVelocity=0; double maxForce=100000; double kp=0.1; - double kd=0.1; + double kd=1.0; int valid = 0; if (0==sm) From daf91de77878858611c601ed89ff14651eb3fb44 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 27 Jul 2016 09:11:08 -0700 Subject: [PATCH 05/12] fix ffmpeg stream to create quicktime compatible videos --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 971df3734..7cfc75652 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -750,7 +750,7 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char* void SimpleOpenGL3App::swapBuffer() { - m_window->endRendering(); + if (m_data->m_frameDumpPngFileName) { int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); @@ -764,7 +764,8 @@ void SimpleOpenGL3App::swapBuffer() m_data->m_frameDumpPngFileName = 0; } } - m_window->startRendering(); + m_window->endRendering(); + m_window->startRendering(); } // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/ @@ -780,7 +781,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) #else sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - "-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName); + "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); #endif //sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " From 626a913866158e5956cfa2ae7f5f6350e99d783e Mon Sep 17 00:00:00 2001 From: MiCroN3000 Date: Thu, 28 Jul 2016 20:15:38 +0200 Subject: [PATCH 06/12] The kinematic character controller with various fixes and a few new features like, being able to set any vector for gravity/up, jumping in a certain direction, possibility to use collision masks, angular & linear velocity, angular & linear damping. --- .../btCharacterControllerInterface.h | 2 +- .../btKinematicCharacterController.cpp | 461 +++++++++++++----- .../btKinematicCharacterController.h | 60 ++- 3 files changed, 395 insertions(+), 128 deletions(-) diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h index dffb06dfe..c3a3ac6c8 100644 --- a/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -37,7 +37,7 @@ public: virtual void preStep ( btCollisionWorld* collisionWorld) = 0; virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; virtual bool canJump () const = 0; - virtual void jump () = 0; + virtual void jump(const btVector3& dir = btVector3()) = 0; virtual bool onGround () const = 0; virtual void setUpInterpolate (bool value) = 0; diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 31faf1df5..60e49b750 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector return direction - parallelComponent(direction, normal); } -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) +btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) { - m_upAxis = upAxis; + m_up.setValue(0.0f, 0.0f, 1.0f); + m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); + setUp(up); + setStepHeight(stepHeight); m_addedMargin = 0.02; - m_walkDirection.setValue(0,0,0); + 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_stepHeight = stepHeight; m_turnAngle = btScalar(0.0); m_convexShape=convexShape; m_useWalkDirection = true; // use walk direction by default, legacy behavior m_velocityTimeInterval = 0.0; m_verticalVelocity = 0.0; m_verticalOffset = 0.0; - m_gravity = 9.8 * 3 ; // 3G acceleration. + m_gravity = 9.8 * 3.0 ; // 3G acceleration. m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. m_jumpSpeed = 10.0; // ? + m_SetjumpSpeed = m_jumpSpeed; m_wasOnGround = false; m_wasJumping = false; m_interpolateUp = true; setMaxSlope(btRadians(45.0)); - m_currentStepOffset = 0; + m_currentStepOffset = 0.0; + m_maxPenetrationDepth = 0.2; full_drop = false; bounce_fix = false; + m_linearDamping = btScalar(0.0); + m_angularDamping = btScalar(0.0); } btKinematicCharacterController::~btKinematicCharacterController () @@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - btScalar maxPen = btScalar(0.0); +// btScalar maxPen = btScalar(0.0); for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) { m_manifoldArray.resize(0); @@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) continue; + + if (!needsCollision(obj0, obj1)) + continue; if (collisionPair->m_algorithm) collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); @@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btScalar dist = pt.getDistance(); - if (dist < 0.0) + if (dist < -m_maxPenetrationDepth) { - if (dist < maxPen) - { - maxPen = dist; - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + // TODO: cause problems on slopes, not sure if it is needed + //if (dist < maxPen) + //{ + // maxPen = dist; + // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - } + //} m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); penetration = true; } else { @@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* void btKinematicCharacterController::stepUp ( btCollisionWorld* world) { + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + // phase 1: up btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); start.setIdentity (); end.setIdentity (); /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); + start.setOrigin(m_currentPosition); + + m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f)); + m_currentPosition = m_targetPosition; + end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) } else { - world->convexSweepTest (m_convexShape, start, end, callback); + world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); } - - if (callback.hasHit()) + + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // Only modify the position if the hit was a slope and not a wall or ceiling. - if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) + if (callback.m_hitNormalWorld.dot(m_up) > 0.0) { // we moved up only a fraction of the step height - m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + m_currentStepOffset = stepHeight * callback.m_closestHitFraction; if (m_interpolateUp == true) m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); else m_currentPosition = m_targetPosition; } - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; + + btTransform& xform = m_ghostObject->getWorldTransform(); + xform.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(xform); + + // fix penetration if we hit a ceiling for example + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(world)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + m_targetPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_currentPosition = m_targetPosition; + + if (m_verticalOffset > 0) + { + m_verticalOffset = 0.0; + m_verticalVelocity = 0.0; + m_currentStepOffset = m_stepHeight; + } } else { - m_currentStepOffset = m_stepHeight; + m_currentStepOffset = stepHeight; m_currentPosition = m_targetPosition; } } +bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) +{ + bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0; + collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask); + return collides; +} + void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { btVector3 movementDirection = m_targetPosition - m_currentPosition; @@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); // phase 2: forward and strafe btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; start.setIdentity (); @@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); // printf("distance2=%f\n",distance2); - if (m_touchingContact) - { - if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) - { - //interferes with step movement - //updateTargetPositionBasedOnCollision (m_touchingNormal); - } - } - int maxIter = 10; while (fraction > btScalar(0.01) && maxIter-- > 0) @@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co end.setOrigin (m_targetPosition); btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar margin = m_convexShape->getMargin(); m_convexShape->setMargin(margin + m_addedMargin); - - if (m_useGhostObjectSweepTest) + if (!(start == end)) { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + else + { + collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } } - m_convexShape->setMargin(margin); fraction -= callback.m_closestHitFraction; - if (callback.hasHit()) + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // we moved only a fraction //btScalar hitDistance; @@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co } } else { - // we moved whole way - m_currentPosition = m_targetPosition; +// if (!m_wasJumping) + // we moved whole way + m_currentPosition = m_targetPosition; } + m_currentPosition = m_targetPosition; // if (callback.m_closestHitFraction == 0.f) // break; @@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld // phase 3: down /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); + btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep); btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; + btVector3 gravity_drop = m_up * downVelocity; m_targetPosition -= (step_drop + gravity_drop);*/ btVector3 orig_position = m_targetPosition; btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if (m_verticalVelocity > 0.0) + return; + if(downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) downVelocity = m_fallSpeed; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine); callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld start.setOrigin (m_currentPosition); end.setOrigin (m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + //set double test for 2x the step drop, to check for a large drop vs small drop end_double.setOrigin (m_targetPosition - step_drop); @@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) + if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { //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); @@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) - { - //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); - } + if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + { + //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); + } } btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; bool has_hit = false; if (bounce_fix == true) - has_hit = callback.hasHit() || callback2.hasHit(); + has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); else - has_hit = callback2.hasHit(); + has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); - if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + + if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; - downVelocity = m_stepHeight; + downVelocity = stepHeight; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests @@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld break; } - if (callback.hasHit() || runonce == true) + if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // we dropped a fraction of the height -> hit floor - btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); @@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld if (bounce_fix == true) { if (full_drop == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else - //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + else + //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); } else m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); @@ -528,7 +589,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_wasJumping = false; } else { // we dropped the full height - + full_drop = true; if (bounce_fix == true) @@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; - step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } @@ -579,21 +640,63 @@ btScalar timeInterval m_velocityTimeInterval += timeInterval; } +void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity) +{ + m_AngVel = velocity; +} + +const btVector3& btKinematicCharacterController::getAngularVelocity() const +{ + return m_AngVel; +} + +void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity) +{ + m_walkDirection = velocity; + + // HACK: if we are moving in the direction of the up, treat it as a jump :( + if (m_walkDirection.length2() > 0) + { + btVector3 w = velocity.normalized(); + btScalar c = w.dot(m_up); + if (c != 0) + { + //there is a component in walkdirection for vertical velocity + btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length()); + m_walkDirection -= upComponent; + m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length(); + + if (c > 0.0f) + { + m_wasJumping = true; + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + } + } + } + else + m_verticalVelocity = 0.0f; +} + +btVector3 btKinematicCharacterController::getLinearVelocity() const +{ + return m_walkDirection + (m_verticalVelocity * m_up); +} + void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) { - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - m_wasOnGround = false; - m_wasJumping = false; - m_walkDirection.setValue(0,0,0); - m_velocityTimeInterval = 0.0; + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasOnGround = false; + m_wasJumping = false; + m_walkDirection.setValue(0,0,0); + m_velocityTimeInterval = 0.0; - //clear pair cache - btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); - while (cache->getOverlappingPairArray().size() > 0) - { - cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); - } + //clear pair cache + btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); + while (cache->getOverlappingPairArray().size() > 0) + { + cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); + } } void btKinematicCharacterController::warp (const btVector3& origin) @@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin) void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) { - - int numPenetrationLoops = 0; - m_touchingContact = false; - while (recoverFromPenetration (collisionWorld)) - { - numPenetrationLoops++; - m_touchingContact = true; - if (numPenetrationLoops > 4) - { - //printf("character could not recover from penetration = %d\n", numPenetrationLoops); - break; - } - } - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); m_targetPosition = m_currentPosition; + + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); - - } -#include - void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) { // printf("playerStep(): "); // printf(" dt = %f", dt); + if (m_AngVel.length2() > 0.0f) + { + m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt); + } + + // integrate for angular velocity + if (m_AngVel.length2() > 0.0f) + { + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + + btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt); + + btQuaternion orn = rot * xform.getRotation(); + + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; + } + // quick check... - if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) { + if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) { // printf("\n"); return; // no motion } m_wasOnGround = onGround(); + //btVector3 lvel = m_walkDirection; + btScalar c = 0.0f; + + if (m_walkDirection.length2() > 0) + { + // apply damping + m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt); + } + + m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt); + // Update fall velocity. m_verticalVelocity -= m_gravity * dt; - if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) { m_verticalVelocity = m_jumpSpeed; } - if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) { m_verticalVelocity = -btFabs(m_fallSpeed); } m_verticalOffset = m_verticalVelocity * dt; - btTransform xform; - xform = m_ghostObject->getWorldTransform (); + xform = m_ghostObject->getWorldTransform(); // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); // printf("walkSpeed=%f\n",walkSpeed); - stepUp (collisionWorld); + stepUp(collisionWorld); + //todo: Experimenting with behavior of controller when it hits a ceiling.. + //bool hitUp = stepUp (collisionWorld); + //if (hitUp) + //{ + // m_verticalVelocity -= m_gravity * dt; + // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + // { + // m_verticalVelocity = m_jumpSpeed; + // } + // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + // { + // m_verticalVelocity = -btFabs(m_fallSpeed); + // } + // m_verticalOffset = m_verticalVelocity * dt; + + // xform = m_ghostObject->getWorldTransform(); + //} + if (m_useWalkDirection) { stepForwardAndStrafe (collisionWorld, m_walkDirection); } else { @@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } stepDown (collisionWorld, dt); + //todo: Experimenting with max jump height + //if (m_wasJumping) + //{ + // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis]; + // if (ds > m_maxJumpHeight) + // { + // // substract the overshoot + // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight; + + // // max height was reached, so potential energy is at max + // // and kinematic energy is 0, thus velocity is 0. + // if (m_verticalVelocity > 0.0) + // m_verticalVelocity = 0.0; + // } + //} // printf("\n"); xform.setOrigin (m_currentPosition); m_ghostObject->setWorldTransform (xform); + + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(collisionWorld)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } } void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) @@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) { m_jumpSpeed = jumpSpeed; + m_SetjumpSpeed = m_jumpSpeed; } void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) @@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const return onGround(); } -void btKinematicCharacterController::jump () +void btKinematicCharacterController::jump(const btVector3& v) { - if (!canJump()) - return; - + m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length(); m_verticalVelocity = m_jumpSpeed; m_wasJumping = true; + m_jumpAxis = v.length2() == 0 ? m_up : v.normalized(); + + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + #if 0 currently no jumping. btTransform xform; @@ -727,14 +898,16 @@ void btKinematicCharacterController::jump () #endif } -void btKinematicCharacterController::setGravity(btScalar gravity) +void btKinematicCharacterController::setGravity(const btVector3& gravity) { - m_gravity = gravity; + if (gravity.length2() > 0) setUpVector(-gravity); + + m_gravity = gravity.length(); } -btScalar btKinematicCharacterController::getGravity() const +btVector3 btKinematicCharacterController::getGravity() const { - return m_gravity; + return -m_gravity * m_up; } void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) @@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const return m_maxSlopeRadians; } -bool btKinematicCharacterController::onGround () const +void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d) { - return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; + m_maxPenetrationDepth = d; } +btScalar btKinematicCharacterController::getMaxPenetrationDepth() const +{ + return m_maxPenetrationDepth; +} + +bool btKinematicCharacterController::onGround () const +{ + return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON); +} + +void btKinematicCharacterController::setStepHeight(btScalar h) +{ + m_stepHeight = h; +} btVector3* btKinematicCharacterController::getUpAxisDirections() { @@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value) { m_interpolateUp = value; } + +void btKinematicCharacterController::setUp(const btVector3& up) +{ + if (up.length2() > 0 && m_gravity > 0.0f) + { + setGravity(-m_gravity * up.normalized()); + return; + } + + setUpVector(up); +} + +void btKinematicCharacterController::setUpVector(const btVector3& up) +{ + if (m_up == up) + return; + + btVector3 u = m_up; + + if (up.length2() > 0) + m_up = up.normalized(); + else + m_up = btVector3(0.0, 0.0, 0.0); + + if (!m_ghostObject) return; + btQuaternion rot = getRotation(m_up, u); + + //set orientation with new up + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + btQuaternion orn = rot.inverse() * xform.getRotation(); + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); +} + +btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const +{ + if (v0.length2() == 0.0f || v1.length2() == 0.0f) + { + btQuaternion q; + return q; + } + + return shortestArcQuatNormalize2(v0, v1); +} + diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h index add6f30a6..3d677e647 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -43,10 +43,12 @@ protected: btPairCachingGhostObject* m_ghostObject; btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + btScalar m_maxPenetrationDepth; btScalar m_verticalVelocity; btScalar m_verticalOffset; btScalar m_fallSpeed; btScalar m_jumpSpeed; + btScalar m_SetjumpSpeed; btScalar m_maxJumpHeight; btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) @@ -61,24 +63,34 @@ protected: ///this is the desired walk direction, set by the user btVector3 m_walkDirection; btVector3 m_normalizedDirection; + btVector3 m_AngVel; + + btVector3 m_jumpPosition; //some internal variables btVector3 m_currentPosition; btScalar m_currentStepOffset; btVector3 m_targetPosition; + btQuaternion m_currentOrientation; + btQuaternion m_targetOrientation; + ///keep track of the contact manifolds btManifoldArray m_manifoldArray; bool m_touchingContact; btVector3 m_touchingNormal; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_wasOnGround; bool m_wasJumping; bool m_useGhostObjectSweepTest; bool m_useWalkDirection; btScalar m_velocityTimeInterval; - int m_upAxis; + btVector3 m_up; + btVector3 m_jumpAxis; static btVector3* getUpAxisDirections(); bool m_interpolateUp; @@ -94,11 +106,18 @@ protected: void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); void stepDown (btCollisionWorld* collisionWorld, btScalar dt); + + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + void setUpVector(const btVector3& up); + + btQuaternion getRotation(btVector3& v0, btVector3& v1) const; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); + btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0)); ~btKinematicCharacterController (); @@ -112,14 +131,9 @@ public: ///btActionInterface interface void debugDraw(btIDebugDraw* debugDrawer); - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } + void setUp(const btVector3& up); + + const btVector3& getUp() { return m_up; } /// This should probably be called setPositionIncrementPerSimulatorStep. /// This is neither a direction nor a velocity, but the amount to @@ -136,27 +150,47 @@ public: virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval); + virtual void setAngularVelocity(const btVector3& velocity); + virtual const btVector3& getAngularVelocity() const; + + virtual void setLinearVelocity(const btVector3& velocity); + virtual btVector3 getLinearVelocity() const; + + void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getLinearDamping() const { return m_linearDamping; } + void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getAngularDamping() const { return m_angularDamping; } + void reset ( btCollisionWorld* collisionWorld ); void warp (const btVector3& origin); void preStep ( btCollisionWorld* collisionWorld); void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); + void setStepHeight(btScalar h); + btScalar getStepHeight() const { return m_stepHeight; } void setFallSpeed (btScalar fallSpeed); + btScalar getFallSpeed() const { return m_fallSpeed; } void setJumpSpeed (btScalar jumpSpeed); + btScalar getJumpSpeed() const { return m_jumpSpeed; } void setMaxJumpHeight (btScalar maxJumpHeight); bool canJump () const; - void jump (); + void jump(const btVector3& v = btVector3()); - void setGravity(btScalar gravity); - btScalar getGravity() const; + void applyImpulse(const btVector3& v) { jump(v); } + + void setGravity(const btVector3& gravity); + btVector3 getGravity() const; /// The max slope determines the maximum angle that the controller can walk up. /// The slope angle is measured in radians. void setMaxSlope(btScalar slopeRadians); btScalar getMaxSlope() const; + void setMaxPenetrationDepth(btScalar d); + btScalar getMaxPenetrationDepth() const; + btPairCachingGhostObject* getGhostObject(); void setUseGhostSweepTest(bool useGhostObjectSweepTest) { From a7a9182953a5524c3ec49b6a96058583b9b8b0ef Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Thu, 28 Jul 2016 16:47:05 -0700 Subject: [PATCH 07/12] fix warning: extra tokens at end of #endif directive --- examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 2 +- examples/ThirdPartyLibs/BussIK/VectorRn.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 544dd921e..1d3eda690 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -399,4 +399,4 @@ inline void MatrixRmn::AddArrayScale( long length, const double* from, long from -#endif MATRIX_RMN_H \ No newline at end of file +#endif //MATRIX_RMN_H diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.h b/examples/ThirdPartyLibs/BussIK/VectorRn.h index 8357f9997..99bd67da5 100644 --- a/examples/ThirdPartyLibs/BussIK/VectorRn.h +++ b/examples/ThirdPartyLibs/BussIK/VectorRn.h @@ -235,4 +235,4 @@ inline double Dot( const VectorRn& u, const VectorRn& v ) return res; } -#endif VECTOR_RN_H \ No newline at end of file +#endif //VECTOR_RN_H From 72e329962ec94dec0e6ee324a157f31073195fcd Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 28 Jul 2016 18:06:03 -0700 Subject: [PATCH 08/12] fix ffmpeg mp4 generation under Windows as well. remove static variables, make them local, to avoid multithreading issues. --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 7 +++- .../Featherstone/btMultiBody.cpp | 42 +++++++++---------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 7cfc75652..25d283119 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -776,8 +776,11 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) char cmd[8192]; #ifdef _WIN32 - sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); + sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " + "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); + + //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " + // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); #else sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index c683a179e..d56f1db14 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i, m_links[i].m_eVector = parentComToThisComOffset; // - static btVector3 vecNonParallelToRotAxis(1, 0, 0); + btVector3 vecNonParallelToRotAxis(1, 0, 0); if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) vecNonParallelToRotAxis.setValue(0, 1, 0); // @@ -714,15 +714,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btScalar * Y = &scratch_r[0]; // //aux variables - static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) - static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child - static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp - static btSpatialTransformationMatrix fromWorld; + btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + btSpatialTransformationMatrix fromWorld; fromWorld.m_trnVec.setZero(); ///////////////// @@ -919,8 +919,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar case btMultibodyLink::eSpherical: case btMultibodyLink::ePlanar: { - static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); //unroll the loop? for(int row = 0; row < 3; ++row) @@ -1323,11 +1323,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar btScalar * Y = r_ptr; //////////////// //aux variables - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; ///////////////// // First 'upward' loop. @@ -1522,8 +1522,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd btScalar *pBaseQuat = pq ? pq : m_baseQuat; btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // - static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); pQuatUpdateFun(baseOmega, baseQuat, true, dt); pBaseQuat[0] = baseQuat.x(); pBaseQuat[1] = baseQuat.y(); @@ -1557,8 +1557,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::eSpherical: { - static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); pQuatUpdateFun(jointVel, jointOri, false, dt); pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); break; From 80dfec170b5d28885f2841629a22aa7b04f42379 Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Fri, 29 Jul 2016 14:43:55 -0700 Subject: [PATCH 09/12] fix setidentity bug --- examples/ThirdPartyLibs/BussIK/LinearR4.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h index 51f57a666..b0c542249 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -580,8 +580,11 @@ inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A) inline void Matrix4x4::SetIdentity ( ) { - m11 = m22 = m33 = m44 = 1.0; - m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0; + m12 = m13 = m14 = + m21 = m23 = m24 = + m31 = m32 = m34 = + m41 = m42 = m43 = 0.0; + m11 = m22 = m33 = m44 = 1.0; } inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v, From b759ab8a913126697cbd4bc76dbb3cca4b245ecf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:45:14 -0700 Subject: [PATCH 10/12] fix overflow visual shape in COLLADA .dae file importer. --- .../ImportColladaDemo/LoadMeshFromCollada.cpp | 76 ++++++++++--------- 1 file changed, 41 insertions(+), 35 deletions(-) diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp index b9a9479e0..8b47f5552 100644 --- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: #include "btMatrix4x4.h" +#define MAX_VISUAL_SHAPES 512 struct VertexSource @@ -288,42 +289,47 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray; - visualShape.m_indices = new b3AlignedObjectArray; - int indexBase = 0; + if (shapeIndex; + visualShape.m_indices = new b3AlignedObjectArray; + int indexBase = 0; - btAssert(vertexNormals.size()==vertexPositions.size()); - for (int v=0;vpush_back(vtx); - } + btAssert(vertexNormals.size()==vertexPositions.size()); + for (int v=0;vpush_back(vtx); + } - for (int index=0;indexpush_back(indices[index]+indexBase); - } - - - //b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size()); - indexBase=visualShape.m_vertices->size(); - visualShape.m_numIndices = visualShape.m_indices->size(); - visualShape.m_numvertices = visualShape.m_vertices->size(); - } - //b3Printf("geometry name=%s\n",geometryName); - name2Shape.insert(geometryName,shapeIndex); - + for (int index=0;indexpush_back(indices[index]+indexBase); + } + + + //b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size()); + indexBase=visualShape.m_vertices->size(); + visualShape.m_numIndices = visualShape.m_indices->size(); + visualShape.m_numvertices = visualShape.m_vertices->size(); + } + //b3Printf("geometry name=%s\n",geometryName); + name2Shape.insert(geometryName,shapeIndex); + } else + { + b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES); + } }//for each geometry } @@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray name2ShapeIndex; From a608f9bfddc21f47166e39c0ada5d93558e10dff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:46:35 -0700 Subject: [PATCH 11/12] fix trailing space issue in tinyobjloader/mtl files. --- examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index 8479992d5..16b0522ea 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -366,6 +366,8 @@ std::string LoadMtl ( continue; } + linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1); + // Skip leading space. const char* token = linebuf.c_str(); token += strspn(token, " \t"); From 04cd9e18d6ec0023e8aca6a894ee26bf3ad7f7a3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:53:38 -0700 Subject: [PATCH 12/12] add [] in error message of texture loading, to show leading/trailing spaces --- examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 67b64fd26..c042c42c0 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -67,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& } else { - b3Warning("not found %s\n",relativeFileName); + b3Warning("not found [%s]\n",relativeFileName); } } }