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

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 <new>
+#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 );
+}

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

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			 \

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"

src/ide/vc8/bullet_lib.vcproj

@@ -478,6 +478,10 @@
 						RelativePath="..\..\bullet\src\BulletDynamics\ConstraintSolver\btTypedConstraint.cpp"
 						>
 					</File>
+					<File
+						RelativePath="..\..\bullet\src\BulletDynamics\ConstraintSolver\btUprightConstraint.cpp"
+						>
+					</File>
 				</Filter>
 				<Filter
 					Name="Dynamics"
@@ -1060,6 +1064,10 @@
 						RelativePath="..\..\bullet\src\BulletDynamics\ConstraintSolver\btTypedConstraint.h"
 						>
 					</File>
+					<File
+						RelativePath="..\..\bullet\src\BulletDynamics\ConstraintSolver\btUprightConstraint.h"
+						>
+					</File>
 				</Filter>
 				<Filter
 					Name="Dynamics"

src/kart.cpp

@@ -219,8 +219,17 @@ void Kart::createPhysics(ssgEntity *obj)
         wheel.m_frictionSlip             = m_kart_properties->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<m_kart_chassis.getNumChildShapes(); i++)
     {
         delete m_kart_chassis.getChildShape(i);
@@ -718,6 +728,10 @@ float Kart::handleWheelie(float dt)
     if ( m_controls.wheelie && 
          m_speed >= 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);

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;

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
 
 //-----------------------------------------------------------------------------