From 54744b6ab90ab82887399c50cfc429b1e6acd967 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Fri, 14 Sep 2012 21:39:48 +0000 Subject: [PATCH] added gyroscopic force option for btRigidBody, body->setFlags(BT_ENABLE_GYROPSCOPIC_FORCE); Note that it can easily introduce instability at regular (60Hertz) simulation steps so it is generally best to not use the option. If needed, use a very small internal step, such as 1000 Hertz (world->stepSimulation(dt,100,1./1000.f); or stepSimulation(1./1000.,0); --- Demos/CMakeLists.txt | 7 +- Demos/GyroscopicDemo/CMakeLists.txt | 72 ++++++ Demos/GyroscopicDemo/GyroscopicDemo.cpp | 240 ++++++++++++++++++ Demos/GyroscopicDemo/GyroscopicDemo.h | 71 ++++++ Demos/GyroscopicDemo/Win32GyroscopicDemo.cpp | 25 ++ Demos/GyroscopicDemo/main.cpp | 20 ++ Demos/premake4.lua | 1 + .../ConstraintSolver/btContactSolverInfo.h | 2 + .../btSequentialImpulseConstraintSolver.cpp | 7 +- src/BulletDynamics/Dynamics/btRigidBody.cpp | 17 ++ src/BulletDynamics/Dynamics/btRigidBody.h | 9 +- 11 files changed, 464 insertions(+), 7 deletions(-) create mode 100644 Demos/GyroscopicDemo/CMakeLists.txt create mode 100644 Demos/GyroscopicDemo/GyroscopicDemo.cpp create mode 100644 Demos/GyroscopicDemo/GyroscopicDemo.h create mode 100644 Demos/GyroscopicDemo/Win32GyroscopicDemo.cpp create mode 100644 Demos/GyroscopicDemo/main.cpp diff --git a/Demos/CMakeLists.txt b/Demos/CMakeLists.txt index 90b97af0c..b197c37aa 100644 --- a/Demos/CMakeLists.txt +++ b/Demos/CMakeLists.txt @@ -12,7 +12,7 @@ IF (USE_GLUT) SET(SharedDemoSubdirs OpenGL AllBulletDemos ConvexDecompositionDemo CcdPhysicsDemo ConstraintDemo SliderConstraintDemo GenericJointDemo Raytracer - RagdollDemo ForkLiftDemo BasicDemo RaytestDemo VoronoiFractureDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo + RagdollDemo ForkLiftDemo BasicDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo UserCollisionAlgorithm CharacterDemo SoftDemo CollisionInterfaceDemo ConcaveConvexcastDemo SimplexDemo DynamicControlDemo ConvexHullDistance @@ -47,10 +47,11 @@ ELSE (USE_GLUT) Box2dDemo CollisionInterfaceDemo ConcaveDemo - ConstraintDemo + ConstraintDemo ConvexDecompositionDemo InternalEdgeDemo - GimpactTestDemo + GimpactTestDemo + GyroscopicDemo GenericJointDemo SerializeDemo SoftDemo diff --git a/Demos/GyroscopicDemo/CMakeLists.txt b/Demos/GyroscopicDemo/CMakeLists.txt new file mode 100644 index 000000000..ca6856637 --- /dev/null +++ b/Demos/GyroscopicDemo/CMakeLists.txt @@ -0,0 +1,72 @@ +# This is basically the overall name of the project in Visual Studio this is the name of the Solution File + + +# For every executable you have with a main method you should have an add_executable line below. +# For every add executable line you should list every .cpp and .h file you have associated with that executable. + + + +# You shouldn't have to modify anything below this line +######################################################## + + + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src +${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL +${BULLET_PHYSICS_SOURCE_DIR}/Extras/Serialize/BulletFileLoader +${BULLET_PHYSICS_SOURCE_DIR}/Extras/Serialize/BulletWorldImporter +) + + +IF (USE_GLUT) + LINK_LIBRARIES( + OpenGLSupport BulletWorldImporter BulletDynamics BulletCollision LinearMath BulletFileLoader ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + + ADD_EXECUTABLE(AppGyroscopicDemo + GyroscopicDemo.cpp + GyroscopicDemo.h + main.cpp + ) + IF (WIN32) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (CMAKE_CL_64) + ADD_CUSTOM_COMMAND( + TARGET AppGyroscopicDemo + POST_BUILD + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR} + ) + ELSE(CMAKE_CL_64) + ADD_CUSTOM_COMMAND( + TARGET AppGyroscopicDemo + POST_BUILD + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Debug + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Release + + ) + ENDIF(CMAKE_CL_64) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + ENDIF(WIN32) +ELSE (USE_GLUT) + LINK_LIBRARIES( + OpenGLSupport BulletWorldImporter BulletDynamics BulletCollision LinearMath BulletFileLoader ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + + ADD_EXECUTABLE(AppGyroscopicDemo + WIN32 + ../OpenGL/Win32AppMain.cpp + Win32GyroscopicDemo.cpp + GyroscopicDemo.cpp + GyroscopicDemo.h + ) +ENDIF (USE_GLUT) + + + +IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(AppGyroscopicDemo PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(AppGyroscopicDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(AppGyroscopicDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) \ No newline at end of file diff --git a/Demos/GyroscopicDemo/GyroscopicDemo.cpp b/Demos/GyroscopicDemo/GyroscopicDemo.cpp new file mode 100644 index 000000000..7dfac9399 --- /dev/null +++ b/Demos/GyroscopicDemo/GyroscopicDemo.cpp @@ -0,0 +1,240 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btIDebugDraw.h" + +#include "GLDebugDrawer.h" + +#include "GLDebugFont.h" +#include //printf debugging + +#include "GyroscopicDemo.h" +#include "GL_ShapeDrawer.h" +#include "GlutStuff.h" + + +#include "GLDebugDrawer.h" +static GLDebugDrawer gDebugDrawer; + + + + + +void GyroscopicDemo::setupEmptyDynamicsWorld() +{ + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_overlappingPairCache = new btDbvtBroadphase(); + m_constraintSolver = new btSequentialImpulseConstraintSolver(); + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration); +} + +void GyroscopicDemo::clientResetScene() +{ + exitPhysics(); + initPhysics(); +} +void GyroscopicDemo::initPhysics() +{ + m_azi=90; + m_ele = 20; + + setTexturing(true); + setShadows(true); + setCameraUp(btVector3(0,0,1)); + setCameraForwardAxis(1); + m_sundirection.setValue(0,-1,-1); + setCameraDistance(7.f); + + setupEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(btVector3(0,0,-9.8)); + m_dynamicsWorld->setDebugDrawer(&gDebugDrawer); + + + //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(0.5))); + btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1),0); + + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,0,0)); + btRigidBody* groundBody; + groundBody= localCreateRigidBody(0, groundTransform, groundShape); + groundBody->setFriction(btSqrt(2)); + btVector3 positions[2] = { + btVector3(0.8,-2,2), + btVector3(0.8,2,2) + }; + bool gyro[2] = { + true, + false + }; + + for (int i=0;i<2;i++) + { + btCylinderShapeZ* top = new btCylinderShapeZ(btVector3(1,1,0.125)); + btCapsuleShapeZ* pin = new btCapsuleShapeZ(0.05,1.5); + top->setMargin(0.01); + pin->setMargin(0.01); + btCompoundShape* compound = new btCompoundShape(); + compound->addChildShape(btTransform::getIdentity(),top); + compound->addChildShape(btTransform::getIdentity(),pin); + btVector3 localInertia; + top->calculateLocalInertia(1,localInertia); + btRigidBody* body = new btRigidBody(1,0,compound,localInertia); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(positions[i]); + body->setCenterOfMassTransform(tr); + body->setAngularVelocity(btVector3(0,0,1000)); + body->setLinearVelocity(btVector3(0,.2,0)); + body->setFriction(btSqrt(1)); + m_dynamicsWorld->addRigidBody(body); + if (gyro[i]) + { + body->setFlags(BT_ENABLE_GYROPSCOPIC_FORCE); + } else + { + body->setFlags(0); + } + body->setDamping(0.00001f,0.0001f); + + + } + +} + +void GyroscopicDemo::exitPhysics() +{ + + int i; + + //removed/delete constraints + for (i=m_dynamicsWorld->getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i); + m_dynamicsWorld->removeConstraint(constraint); + delete constraint; + } + + //remove the rigidbodies from the dynamics world and delete them + for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } + + + + + //delete collision shapes + for (int j=0;jgetDebugDrawer() && once) + { + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits); + once=false; + } + } + + + { + //during idle mode, just run 1 simulation step maximum + + int numSimSteps = m_dynamicsWorld->stepSimulation(dt,100,1./1000.f); + + //optional but useful: debug drawing + m_dynamicsWorld->debugDrawWorld(); + + + } + renderme(); + + + glFlush(); + swapBuffers(); +} + + + + +void GyroscopicDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (m_dynamicsWorld) + m_dynamicsWorld->debugDrawWorld(); + + + renderme(); + + glFlush(); + swapBuffers(); +} + + diff --git a/Demos/GyroscopicDemo/GyroscopicDemo.h b/Demos/GyroscopicDemo/GyroscopicDemo.h new file mode 100644 index 000000000..8925fa5d1 --- /dev/null +++ b/Demos/GyroscopicDemo/GyroscopicDemo.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_GYROSCOPIC_DEMO_H +#define BT_GYROSCOPIC_DEMO_H + +#ifdef _WINDOWS +#include "Win32DemoApplication.h" +#define PlatformDemoApplication Win32DemoApplication +#else +#include "GlutDemoApplication.h" +#define PlatformDemoApplication GlutDemoApplication +#endif + +///GyroscopicDemo shows how to create a constraint, like Hinge or btGenericD6constraint +class GyroscopicDemo : public PlatformDemoApplication +{ + //keep track of variables to delete memory at the end + btAlignedObjectArray m_collisionShapes; + + class btBroadphaseInterface* m_overlappingPairCache; + + class btCollisionDispatcher* m_dispatcher; + + class btConstraintSolver* m_constraintSolver; + + class btDefaultCollisionConfiguration* m_collisionConfiguration; + + void setupEmptyDynamicsWorld(); + + void clientResetScene(); + + + public: + + GyroscopicDemo(); + + virtual ~GyroscopicDemo(); + + void initPhysics(); + + void exitPhysics(); + + virtual void clientMoveAndDisplay(); + + virtual void displayCallback(); + + static DemoApplication* Create() + { + GyroscopicDemo* demo = new GyroscopicDemo(); + demo->myinit(); + demo->initPhysics(); + return demo; + } + + +}; + +#endif //BT_GYROSCOPIC_DEMO_H + diff --git a/Demos/GyroscopicDemo/Win32GyroscopicDemo.cpp b/Demos/GyroscopicDemo/Win32GyroscopicDemo.cpp new file mode 100644 index 000000000..37c36ff85 --- /dev/null +++ b/Demos/GyroscopicDemo/Win32GyroscopicDemo.cpp @@ -0,0 +1,25 @@ +#ifdef _WINDOWS +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "GyroscopicDemo.h" + +///The 'createDemo' function is called from Bullet/Demos/OpenGL/Win32AppMain.cpp to instantiate this particular demo +DemoApplication* createDemo() +{ + return new GyroscopicDemo(); +} + +#endif diff --git a/Demos/GyroscopicDemo/main.cpp b/Demos/GyroscopicDemo/main.cpp new file mode 100644 index 000000000..ca2caaa9a --- /dev/null +++ b/Demos/GyroscopicDemo/main.cpp @@ -0,0 +1,20 @@ +#include "GyroscopicDemo.h" +#include "GlutStuff.h" +#include "GLDebugDrawer.h" + +#include "btBulletDynamicsCommon.h" + +int main(int argc,char** argv) +{ + + + + GyroscopicDemo* constraintDemo = new GyroscopicDemo(); + + + constraintDemo->initPhysics(); + constraintDemo->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits); + + return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo); +} + diff --git a/Demos/premake4.lua b/Demos/premake4.lua index e7f75972e..1fa80a049 100644 --- a/Demos/premake4.lua +++ b/Demos/premake4.lua @@ -65,6 +65,7 @@ end "GenericJointDemo", "GimpactTestDemo", "GjkConvexCastDemo", + "GyroscopicDemo", "InternalEdgeDemo", "MovingConcaveDemo", "MultiMaterialDemo", diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 19a2203f2..916a26b33 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -56,6 +56,7 @@ struct btContactSolverInfoData int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; + btScalar m_maxGyroscopicForce; }; @@ -86,6 +87,7 @@ struct btContactSolverInfo : public btContactSolverInfoData //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit + m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag) } }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 8a0042aef..23c24a0ea 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -837,8 +837,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (body && body->getInvMass()) { btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) + { + gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); + } solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep; - solverBody.m_angularVelocity += body->getTotalTorque()*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index e9cfe852f..93b3a4a21 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -256,6 +256,23 @@ void btRigidBody::updateInertiaTensor() } +btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const +{ + btVector3 inertiaLocal; + inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0]; + inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1]; + inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2]; + btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); + btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 gf = getAngularVelocity().cross(tmp); + btScalar l2 = gf.length2(); + if (l2>maxGyroscopicForce*maxGyroscopicForce) + { + gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; + } + return gf; +} + void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 910b8835c..a62066381 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -40,7 +40,11 @@ extern bool gDisableDeactivation; enum btRigidBodyFlags { - BT_DISABLE_WORLD_GRAVITY = 1 + BT_DISABLE_WORLD_GRAVITY = 1, + ///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability + ///So generally it is best to not enable it. + ///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use + BT_ENABLE_GYROPSCOPIC_FORCE = 2 }; @@ -519,8 +523,7 @@ public: return m_rigidbodyFlags; } - - + btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const; ///////////////////////////////////////////////