From 3e9f35c0fd1363d423709ab6b8e84144edc94774 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Thu, 13 Sep 2012 22:40:39 +0000 Subject: [PATCH] Add btGearConstraint, with a demo in Bullet/Demos/ConstraintDemo Thanks to Dimitris Papavasiliou for the idea. --- Demos/ConstraintDemo/ConstraintDemo.cpp | 82 +++++++++++++++++++ Demos/ConstraintDemo/ConstraintDemo.h | 3 + Demos/ConstraintDemo/main.cpp | 4 +- src/BulletDynamics/CMakeLists.txt | 2 + .../ConstraintSolver/btGearConstraint.cpp | 54 ++++++++++++ .../ConstraintSolver/btGearConstraint.h | 56 +++++++++++++ .../ConstraintSolver/btTypedConstraint.h | 1 + src/Makefile.am | 3 + src/btBulletDynamicsCommon.h | 1 + 9 files changed, 204 insertions(+), 2 deletions(-) create mode 100644 src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp create mode 100644 src/BulletDynamics/ConstraintSolver/btGearConstraint.h diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index afabc114f..74ee92762 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -28,6 +28,10 @@ subject to the following restrictions: #include "GL_ShapeDrawer.h" #include "GlutStuff.h" +#include "GLDebugDrawer.h" +static GLDebugDrawer gDebugDrawer; + + const int numObjects = 3; @@ -85,6 +89,11 @@ void ConstraintDemo::setupEmptyDynamicsWorld() } +void ConstraintDemo::clientResetScene() +{ + exitPhysics(); + initPhysics(); +} void ConstraintDemo::initPhysics() { setTexturing(true); @@ -95,6 +104,9 @@ void ConstraintDemo::initPhysics() setupEmptyDynamicsWorld(); + m_dynamicsWorld->setDebugDrawer(&gDebugDrawer); + + //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.))); btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),40); @@ -115,6 +127,76 @@ void ConstraintDemo::initPhysics() float mass = 1.f; +#if ENABLE_ALL_DEMOS +///gear constraint demo + +#define THETA SIMD_PI/4.f +#define L_1 (2 - tan(THETA)) +#define L_2 (1 / cos(THETA)) +#define RATIO L_2 / L_1 + + btRigidBody* bodyA=0; + btRigidBody* bodyB=0; + + { + btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.25,0.2)); + btCollisionShape* cylB = new btCylinderShape(btVector3(L_1,0.025,L_1)); + btCompoundShape* cyl0 = new btCompoundShape(); + cyl0->addChildShape(btTransform::getIdentity(),cylA); + cyl0->addChildShape(btTransform::getIdentity(),cylB); + + btScalar mass = 6.28; + btVector3 localInertia; + cyl0->calculateLocalInertia(mass,localInertia); + btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia); + ci.m_startWorldTransform.setOrigin(btVector3(-8,1,-8)); + + btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia); + m_dynamicsWorld->addRigidBody(body); + body->setLinearFactor(btVector3(0,0,0)); + body->setAngularFactor(btVector3(0,1,0)); + bodyA = body; + } + + { + btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.26,0.2)); + btCollisionShape* cylB = new btCylinderShape(btVector3(L_2,0.025,L_2)); + btCompoundShape* cyl0 = new btCompoundShape(); + cyl0->addChildShape(btTransform::getIdentity(),cylA); + cyl0->addChildShape(btTransform::getIdentity(),cylB); + + btScalar mass = 6.28; + btVector3 localInertia; + cyl0->calculateLocalInertia(mass,localInertia); + btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia); + ci.m_startWorldTransform.setOrigin(btVector3(-10,2,-8)); + + + btQuaternion orn(btVector3(0,0,1),-THETA); + ci.m_startWorldTransform.setRotation(orn); + + btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia); + body->setLinearFactor(btVector3(0,0,0)); + btHingeConstraint* hinge = new btHingeConstraint(*body,btVector3(0,0,0),btVector3(0,1,0),true); + m_dynamicsWorld->addConstraint(hinge); + bodyB= body; + body->setAngularVelocity(btVector3(0,3,0)); + + m_dynamicsWorld->addRigidBody(body); + } + + btVector3 axisA(0,1,0); + btVector3 axisB(0,1,0); + btQuaternion orn(btVector3(0,0,1),-THETA); + btMatrix3x3 mat(orn); + axisB = mat.getRow(1); + + btGearConstraint* gear = new btGearConstraint(*bodyA,*bodyB, axisA,axisB,RATIO); + m_dynamicsWorld->addConstraint(gear,true); + + +#endif + #if ENABLE_ALL_DEMOS //point to point constraint with a breaking threshold diff --git a/Demos/ConstraintDemo/ConstraintDemo.h b/Demos/ConstraintDemo/ConstraintDemo.h index 6b73e0784..5037cb91e 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.h +++ b/Demos/ConstraintDemo/ConstraintDemo.h @@ -39,6 +39,9 @@ class ConstraintDemo : public PlatformDemoApplication void setupEmptyDynamicsWorld(); + void clientResetScene(); + + public: diff --git a/Demos/ConstraintDemo/main.cpp b/Demos/ConstraintDemo/main.cpp index c5e853846..0cec5f6cb 100644 --- a/Demos/ConstraintDemo/main.cpp +++ b/Demos/ConstraintDemo/main.cpp @@ -1,18 +1,18 @@ #include "ConstraintDemo.h" #include "GlutStuff.h" #include "GLDebugDrawer.h" + #include "btBulletDynamicsCommon.h" int main(int argc,char** argv) { - GLDebugDrawer gDebugDrawer; + ConstraintDemo* constraintDemo = new ConstraintDemo(); constraintDemo->initPhysics(); - constraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); 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/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 3a88e354a..100cd7a88 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -6,6 +6,7 @@ SET(BulletDynamics_SRCS Character/btKinematicCharacterController.cpp ConstraintSolver/btConeTwistConstraint.cpp ConstraintSolver/btContactConstraint.cpp + ConstraintSolver/btGearConstraint.cpp ConstraintSolver/btGeneric6DofConstraint.cpp ConstraintSolver/btGeneric6DofSpringConstraint.cpp ConstraintSolver/btHinge2Constraint.cpp @@ -33,6 +34,7 @@ SET(ConstraintSolver_HDRS ConstraintSolver/btConstraintSolver.h ConstraintSolver/btContactConstraint.h ConstraintSolver/btContactSolverInfo.h + ConstraintSolver/btGearConstraint.h ConstraintSolver/btGeneric6DofConstraint.h ConstraintSolver/btGeneric6DofSpringConstraint.h ConstraintSolver/btHinge2Constraint.h diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp new file mode 100644 index 000000000..446e69de7 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. 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. +*/ + +/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. + +#include "btGearConstraint.h" + +btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) +:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), +m_axisInA(axisInA), +m_axisInB(axisInB), +m_ratio(ratio) +{ +} + +btGearConstraint::~btGearConstraint () +{ +} + +void btGearConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 1; + info->nub = 1; +} + +void btGearConstraint::getInfo2 (btConstraintInfo2* info) +{ + btVector3 globalAxisA, globalAxisB; + + globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; + globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; + + info->m_J1angularAxis[0] = globalAxisA[0]; + info->m_J1angularAxis[1] = globalAxisA[1]; + info->m_J1angularAxis[2] = globalAxisA[2]; + + info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; + info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; + info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; + +} + diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h new file mode 100644 index 000000000..b76889476 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -0,0 +1,56 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. 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. +*/ + + + +#ifndef BT_GEAR_CONSTRAINT_H +#define BT_GEAR_CONSTRAINT_H + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. +///See Bullet/Demos/ConstraintDemo for an example use. +class btGearConstraint : public btTypedConstraint +{ +protected: + btVector3 m_axisInA; + btVector3 m_axisInB; + bool m_useFrameA; + btScalar m_ratio; + +public: + btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); + virtual ~btGearConstraint (); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo1 (btConstraintInfo1* info); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void setParam(int num, btScalar value, int axis = -1) + { + btAssert(0); + }; + + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const + { + btAssert(0); + return 0.f; + } + +}; + +#endif //BT_GEAR_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index f5330b51d..594fbcca6 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -33,6 +33,7 @@ enum btTypedConstraintType SLIDER_CONSTRAINT_TYPE, CONTACT_CONSTRAINT_TYPE, D6_SPRING_CONSTRAINT_TYPE, + GEAR_CONSTRAINT_TYPE, MAX_CONSTRAINT_TYPE }; diff --git a/src/Makefile.am b/src/Makefile.am index aaa43aa85..ec39e7a05 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -313,6 +313,7 @@ libBulletDynamics_la_SOURCES = \ BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp \ BulletDynamics/Dynamics/Bullet-C-API.cpp \ BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp \ + BulletDynamics/ConstraintSolver/btGearConstraint.cpp \ BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp \ BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp \ BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp \ @@ -345,6 +346,7 @@ libBulletDynamics_la_SOURCES = \ BulletDynamics/ConstraintSolver/btJacobianEntry.h \ BulletDynamics/ConstraintSolver/btSolverConstraint.h \ BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h \ + BulletDynamics/ConstraintSolver/btGearConstraint.h \ BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h \ BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h \ BulletDynamics/ConstraintSolver/btSliderConstraint.h \ @@ -403,6 +405,7 @@ nobase_bullet_include_HEADERS += \ BulletDynamics/ConstraintSolver/btConstraintSolver.h \ BulletDynamics/ConstraintSolver/btContactConstraint.h \ BulletDynamics/ConstraintSolver/btContactSolverInfo.h \ + BulletDynamics/ConstraintSolver/btGearConstraint.h \ BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h \ BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h \ BulletDynamics/ConstraintSolver/btJacobianEntry.h \ diff --git a/src/btBulletDynamicsCommon.h b/src/btBulletDynamicsCommon.h index ccfad19b8..dbd175c3f 100644 --- a/src/btBulletDynamicsCommon.h +++ b/src/btBulletDynamicsCommon.h @@ -32,6 +32,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h" #include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h" #include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h" +#include "BulletDynamics/ConstraintSolver/btGearConstraint.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"