From b6541195c22ba50f089b52c15e23110ea5de84ec Mon Sep 17 00:00:00 2001 From: hikerstk Date: Tue, 22 Apr 2008 14:20:52 +0000 Subject: [PATCH] Added an 'upright constraint' which prevents the karts from toppling over after a jump (e.g. ramp, or paper plane). git-svn-id: svn+ssh://svn.code.sf.net/p/supertuxkart/code/trunk/supertuxkart@1721 178a84e3-b1eb-0310-8ba1-8eac791a3b58 --- .../ConstraintSolver/btUprightConstraint.cpp | 156 ++++++++++++++++++ .../ConstraintSolver/btUprightConstraint.h | 128 ++++++++++++++ src/bullet/src/Makefile.am | 2 + src/bullet/src/btBulletDynamicsCommon.h | 1 + src/ide/vc8/bullet_lib.vcproj | 8 + src/kart.cpp | 24 ++- src/kart.hpp | 2 + src/physics.cpp | 3 +- 8 files changed, 320 insertions(+), 4 deletions(-) create mode 100644 src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.cpp create mode 100644 src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.h diff --git a/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.cpp b/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.cpp new file mode 100644 index 000000000..4195ebb62 --- /dev/null +++ b/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.cpp @@ -0,0 +1,156 @@ +/* + +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 "btUprightConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include +#include "stdio.h" + +//! +//! +//! + +void btUprightConstraint::solveAngularLimit( + btUprightConstraintLimit *limit, + btScalar timeStep, btScalar jacDiagABInv, + btRigidBody * body0 ) +{ + + // Work out if limit is violated + + limit->m_currentLimitError = 0; + if ( limit->m_angle < m_loLimit ) + { + limit->m_currentLimitError = limit->m_angle - m_loLimit; + } + if ( limit->m_angle > m_hiLimit ) + { + limit->m_currentLimitError = limit->m_angle - m_hiLimit; + } + if( limit->m_currentLimitError == 0.0f ) + { + return; + } + + btScalar targetVelocity = -m_ERP*limit->m_currentLimitError/(timeStep); + btScalar maxMotorForce = m_maxLimitForce; + + maxMotorForce *= timeStep; + + // current velocity difference + btVector3 angularVelocity = body0->getAngularVelocity(); + btScalar axisAngularVelocity = limit->m_axis.dot( angularVelocity ); + + // correction velocity + btScalar motorVelocity = m_limitSoftness*(targetVelocity - m_damping*axisAngularVelocity); + + // correction impulse + btScalar unclippedMotorImpulse = (1+m_bounce)*motorVelocity*jacDiagABInv; + + // clip correction impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse; + + //todo: should clip against accumulated impulse + + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } + + // sort with accumulated impulses + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + + btScalar oldaccumImpulse = limit->m_accumulatedImpulse; + + btScalar sum = oldaccumImpulse + clippedMotorImpulse; + + limit->m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + + clippedMotorImpulse = limit->m_accumulatedImpulse - oldaccumImpulse; + + btVector3 motorImp = clippedMotorImpulse * limit->m_axis; + body0->applyTorqueImpulse(motorImp); + printf("angle %f target Vel %f unclimpulse %f impulse %f %f%f\n", + limit->m_angle, targetVelocity,unclippedMotorImpulse, + motorImp.getX(), motorImp.getY(), motorImp.getZ() + ); +} + +//! +//! +//! + +btUprightConstraint::btUprightConstraint(btRigidBody& rbA, const btTransform& frameInA ) + : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA) + , m_frameInA(frameInA) + +{ + m_ERP = 1.0f; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 1.0f; + m_maxLimitForce = 3000.0f; + m_limit[0].m_accumulatedImpulse = 0.0f; + m_limit[1].m_accumulatedImpulse = 0.0f; + m_limit[ 0 ].m_axis = btVector3( 1, 0, 0 ); + m_limit[ 1 ].m_axis = btVector3( 0, 1, 0 ); + setLimit( SIMD_PI * 0.4f ); +} + +//! +//! +//! + +void btUprightConstraint::buildJacobian() +{ + btTransform worldTransform = m_rbA.getCenterOfMassTransform() * m_frameInA; + btVector3 upAxis = worldTransform.getBasis().getColumn(2); + m_limit[ 0 ].m_angle = btAtan2( upAxis.getZ(), upAxis.getY() )-SIMD_PI/2.0f; + m_limit[ 1 ].m_angle = -btAtan2( upAxis.getZ(), upAxis.getX() )+SIMD_PI/2.0f; + + for ( int i = 0; i < 2; i++ ) + { + if ( m_limit[ i ].m_angle < -SIMD_PI ) + m_limit[ i ].m_angle += 2 * SIMD_PI; + if ( m_limit[ i ].m_angle > SIMD_PI ) + m_limit[ i ].m_angle -= 2 * SIMD_PI; + + new (&m_jacAng[ i ]) btJacobianEntry( m_limit[ i ].m_axis, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + } +} + +//! +//! +//! + +void btUprightConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; + + solveAngularLimit( &m_limit[ 0 ], m_timeStep, btScalar(1.) / m_jacAng[ 0 ].getDiagonal(), &m_rbA ); + solveAngularLimit( &m_limit[ 1 ], m_timeStep, btScalar(1.) / m_jacAng[ 1 ].getDiagonal(), &m_rbA ); +} \ No newline at end of file diff --git a/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.h b/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.h new file mode 100644 index 000000000..5b3adceeb --- /dev/null +++ b/src/bullet/src/BulletDynamics/ConstraintSolver/btUprightConstraint.h @@ -0,0 +1,128 @@ +/* + +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 UPRIGHT_CONSTRAINT_H +#define UPRIGHT_CONSTRAINT_H + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + +//! +//! +//! + +class btUprightConstraintLimit +{ +public: + btVector3 m_axis; + btScalar m_angle; + btScalar m_accumulatedImpulse; + btScalar m_currentLimitError; +}; + +class btUprightConstraint : public btTypedConstraint +{ +protected: + + //! relative_frames + + //!@{ + btTransform m_frameInA;//!< the constraint space w.r.t body A + //!@} + + //! Jacobians + //!@{ + btJacobianEntry m_jacAng[ 2 ];//!< angular constraint + //!@} + +protected: + + //! temporal variables + //!@{ + btScalar m_timeStep; + btScalar m_ERP; + btScalar m_bounce; + btScalar m_damping; + btScalar m_maxLimitForce; + btScalar m_limitSoftness; + btScalar m_hiLimit; + btScalar m_loLimit; + + btUprightConstraintLimit m_limit[ 2 ]; + + //!@} + + btUprightConstraint& operator=(btUprightConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } + + void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); + + void solveAngularLimit( + btUprightConstraintLimit *limit, + btScalar timeStep, btScalar jacDiagABInv, + btRigidBody * body0 ); + +public: + + btUprightConstraint(btRigidBody& rbA, const btTransform& frameInA ); + + // -PI,+PI is the full range + // 0,0 is no rotation around x or z + // -PI*0.2,+PI*0.2 is a nice bit of tilt + void setLimit( btScalar range ) + { + m_loLimit = -range; + m_hiLimit = +range; + } + + // Error correction scaling + // 0 - 1 + // + void setErp( btScalar erp ) + { + m_ERP = erp; + } + void setBounce( btScalar bounce ) + { + m_bounce = bounce; + } + void setMaxLimitForce( btScalar force ) + { + m_maxLimitForce = force; + } + void setLimitSoftness( btScalar softness ) + { + m_limitSoftness = softness; + } + void setDamping( btScalar damping ) + { + m_damping = damping; + } + + virtual void buildJacobian(); + virtual void solveConstraint(btScalar timeStep); +}; + + + +#endif //UPRIGHT_CONSTRAINT_H \ No newline at end of file diff --git a/src/bullet/src/Makefile.am b/src/bullet/src/Makefile.am index 6b5348ce5..1b2a11e78 100644 --- a/src/bullet/src/Makefile.am +++ b/src/bullet/src/Makefile.am @@ -180,6 +180,8 @@ libbulletdynamics_a_SOURCES = \ BulletDynamics/ConstraintSolver/btSolverBody.h \ BulletDynamics/ConstraintSolver/btSolverConstraint.h \ BulletDynamics/ConstraintSolver/btTypedConstraint.cpp \ + BulletDynamics/ConstraintSolver/btUprightConstraint.h \ + BulletDynamics/ConstraintSolver/btUprightConstraint.cpp \ BulletDynamics/ConstraintSolver/btTypedConstraint.h \ BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp \ BulletDynamics/Dynamics/btContinuousDynamicsWorld.h \ diff --git a/src/bullet/src/btBulletDynamicsCommon.h b/src/bullet/src/btBulletDynamicsCommon.h index 300d05bec..66c4754a7 100644 --- a/src/bullet/src/btBulletDynamicsCommon.h +++ b/src/bullet/src/btBulletDynamicsCommon.h @@ -29,6 +29,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btUprightConstraint.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" diff --git a/src/ide/vc8/bullet_lib.vcproj b/src/ide/vc8/bullet_lib.vcproj index 7a093c02c..142e5e93b 100755 --- a/src/ide/vc8/bullet_lib.vcproj +++ b/src/ide/vc8/bullet_lib.vcproj @@ -478,6 +478,10 @@ RelativePath="..\..\bullet\src\BulletDynamics\ConstraintSolver\btTypedConstraint.cpp" > + + + + getFrictionSlip(); wheel.m_rollInfluence = m_kart_properties->getRollInfluence(); } + // Obviously these allocs have to be properly managed/freed + btTransform *t=new btTransform(); + t->setIdentity(); + m_uprightConstraint=new btUprightConstraint(*m_body, *t); + m_uprightConstraint->setLimit(M_PI/8.0f); + m_uprightConstraint->setBounce(0.0f); + m_uprightConstraint->setMaxLimitForce(300.0f); + m_uprightConstraint->setErp(1.0f); + m_uprightConstraint->setLimitSoftness(1.0f); + m_uprightConstraint->setDamping(0.0f); world->getPhysics()->addKart(this, m_vehicle); - } // createPhysics // ----------------------------------------------------------------------------- @@ -244,10 +253,11 @@ Kart::~Kart() if(m_skidmark_left ) delete m_skidmark_left ; if(m_skidmark_right) delete m_skidmark_right; + world->getPhysics()->removeKart(this); delete m_vehicle; delete m_tuning; delete m_vehicle_raycaster; - world->getPhysics()->removeKart(this); + delete m_uprightConstraint; for(int i=0; i= getMaxSpeed()*getWheelieMaxSpeedRatio()) { + // Disable the upright constraint, since it will otherwise + // work against the wheelie + m_uprightConstraint->setLimit(2.0f*M_PI); + if ( m_wheelie_angle < getWheelieMaxPitch() ) m_wheelie_angle += getWheeliePitchRate() * dt; else @@ -728,7 +742,11 @@ float Kart::handleWheelie(float dt) m_wheelie_angle -= getWheelieRestoreRate() * dt; if ( m_wheelie_angle <= 0.0f ) m_wheelie_angle = 0.0f ; } - if(m_wheelie_angle <=0.0f) return 0.0f; + if(m_wheelie_angle <=0.0f) + { + m_uprightConstraint->setLimit(M_PI/8.0f); + return 0.0f; + } const btTransform& chassisTrans = getTrans(); btVector3 targetUp(0.0f, 0.0f, 1.0f); diff --git a/src/kart.hpp b/src/kart.hpp index bc8a5f94a..bb701dca1 100644 --- a/src/kart.hpp +++ b/src/kart.hpp @@ -79,6 +79,7 @@ protected: btCompoundShape m_kart_chassis; btVehicleRaycaster *m_vehicle_raycaster; btRaycastVehicle *m_vehicle; + btUprightConstraint *m_uprightConstraint; float m_kart_height; private: @@ -191,6 +192,7 @@ public: float getKartHeight () const {return m_kart_height; } float getWheelieAngle () const {return m_wheelie_angle; } btRaycastVehicle *getVehicle () const {return m_vehicle; } + btUprightConstraint *getUprightConstraint() const {return m_uprightConstraint;} void updateBulletPhysics(float dt); void draw (); bool isInRest () const; diff --git a/src/physics.cpp b/src/physics.cpp index df2364c03..2d9ef1b67 100644 --- a/src/physics.cpp +++ b/src/physics.cpp @@ -68,7 +68,7 @@ void Physics::addKart(const Kart *kart, btRaycastVehicle *vehicle) { m_dynamics_world->addRigidBody(kart->getBody()); m_dynamics_world->addVehicle(vehicle); - + m_dynamics_world->addConstraint(kart->getUprightConstraint()); } // addKart //----------------------------------------------------------------------------- @@ -79,6 +79,7 @@ void Physics::removeKart(const Kart *kart) { m_dynamics_world->removeRigidBody(kart->getBody()); m_dynamics_world->removeVehicle(kart->getVehicle()); + m_dynamics_world->removeConstraint(kart->getUprightConstraint()); } // removeKart //-----------------------------------------------------------------------------