Back to index

supertuxkart  0.5+dfsg1
btUprightConstraint.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Bullet Continuous Collision Detection and Physics Library
00004 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00005 
00006 This software is provided 'as-is', without any express or implied warranty.
00007 In no event will the authors be held liable for any damages arising from the use of this software.
00008 Permission is granted to anyone to use this software for any purpose,
00009 including commercial applications, and to alter it and redistribute it freely,
00010 subject to the following restrictions:
00011  
00012 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.
00013 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00014 3. This notice may not be removed or altered from any source distribution.
00015 
00016 */
00017 
00018 #include "btUprightConstraint.h"
00019 #include "BulletDynamics/Dynamics/btRigidBody.h"
00020 #include "LinearMath/btTransformUtil.h"
00021 #include <new>
00022 #include "stdio.h"
00023 
00027 
00028 void btUprightConstraint::solveAngularLimit(
00029                      btUprightConstraintLimit *limit,
00030             btScalar timeStep, btScalar jacDiagABInv,
00031             btRigidBody * body0 )
00032 {
00033        
00034        // Work out if limit is violated
00035     if(limit->m_angle>=m_loLimit && limit->m_angle<=m_hiLimit) return;
00036 
00037     limit->m_currentLimitError = (limit->m_angle<m_loLimit) 
00038                                ? limit->m_angle - m_loLimit
00039                                : limit->m_angle - m_hiLimit;
00040 
00041        btScalar targetVelocity       = -m_ERP*limit->m_currentLimitError/(3.1415f/8.0f*timeStep);
00042        btScalar maxMotorForce        = m_maxLimitForce;
00043 
00044     maxMotorForce *= timeStep;
00045 
00046     // current velocity difference
00047     btVector3 angularVelocity       = body0->getAngularVelocity();
00048     btScalar  axisAngularVelocity   = limit->m_axis.dot( angularVelocity );
00049  
00050      // correction velocity
00051     btScalar motorVelocity          = m_limitSoftness*(targetVelocity  - m_damping*axisAngularVelocity);
00052 
00053     // correction impulse
00054     btScalar unclippedMotorImpulse = (1+m_bounce)*motorVelocity*jacDiagABInv;
00055 
00056        // clip correction impulse
00057     btScalar clippedMotorImpulse = unclippedMotorImpulse;
00058 
00059     //todo: should clip against accumulated impulse
00060 
00061     if (unclippedMotorImpulse>0.0f)
00062     {
00063         clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
00064     }
00065     else
00066     {
00067         clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
00068     }
00069 
00070        // sort with accumulated impulses
00071     btScalar      lo = btScalar(-1e30);
00072     btScalar      hi = btScalar(1e30);
00073 
00074     btScalar oldaccumImpulse = limit->m_accumulatedImpulse;
00075 
00076     btScalar sum = oldaccumImpulse + clippedMotorImpulse;
00077 
00078        limit->m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00079 
00080     clippedMotorImpulse = limit->m_accumulatedImpulse - oldaccumImpulse;
00081 
00082     btVector3 motorImp = clippedMotorImpulse * limit->m_axis;
00083     body0->applyTorqueImpulse(motorImp);
00084 }
00085 
00089 
00090 btUprightConstraint::btUprightConstraint(btRigidBody& rbA, const btTransform& frameInA )
00091         : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA)
00092         , m_frameInA(frameInA)
00093 
00094 {
00095       m_ERP                           = 1.0f;
00096       m_bounce                        = 0.0f;
00097       m_damping                       = 1.0f;
00098       m_limitSoftness                 = 1.0f;
00099       m_maxLimitForce                 = 3000.0f;
00100       m_disable_time                  = 0.0f;
00101       m_limit[0].m_accumulatedImpulse = 0.0f;
00102       m_limit[1].m_accumulatedImpulse = 0.0f;
00103          m_limit[ 0 ].m_axis                    = btVector3( 1, 0, 0 );
00104          m_limit[ 1 ].m_axis                    = btVector3( 0, 1, 0 );
00105          setLimit( SIMD_PI * 0.4f );
00106 }
00107  
00111 
00112 void btUprightConstraint::buildJacobian()
00113 {
00114       btTransform worldTransform = m_rbA.getCenterOfMassTransform() * m_frameInA;
00115       btVector3   upAxis         = worldTransform.getBasis().getColumn(2);
00116       m_limit[ 0 ].m_angle          =  btAtan2( upAxis.getZ(), upAxis.getY() )-SIMD_PI/2.0f;
00117       m_limit[ 1 ].m_angle          = -btAtan2( upAxis.getZ(), upAxis.getX() )+SIMD_PI/2.0f;
00118 
00119          for ( int i = 0; i < 2; i++ )
00120          {
00121                 if ( m_limit[ i ].m_angle < -SIMD_PI )
00122                             m_limit[ i ].m_angle += 2 * SIMD_PI;
00123                 if ( m_limit[ i ].m_angle > SIMD_PI )
00124                             m_limit[ i ].m_angle -= 2 * SIMD_PI;
00125 
00126                 new (&m_jacAng[ i ])      btJacobianEntry(  m_limit[ i ].m_axis,
00127                                                                                              m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00128                                                                                              m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00129                                                                                              m_rbA.getInvInertiaDiagLocal(),
00130                                                                                              m_rbB.getInvInertiaDiagLocal());
00131          }
00132 }
00133 
00137 
00138 void btUprightConstraint::solveConstraint(btScalar    timeStep)
00139 {
00140     m_timeStep = timeStep;
00141 
00142     // Update disable time and return if constraint is still disabled
00143     if(m_disable_time>0.0f)
00144     {
00145         m_disable_time -= timeStep;
00146         if(m_disable_time>0.0f) return;
00147     }
00148 
00149        solveAngularLimit( &m_limit[ 0 ], m_timeStep, btScalar(1.) / m_jacAng[ 0 ].getDiagonal(), &m_rbA );
00150        solveAngularLimit( &m_limit[ 1 ], m_timeStep, btScalar(1.) / m_jacAng[ 1 ].getDiagonal(), &m_rbA );
00151 }
00152