Back to index

supertuxkart  0.5+dfsg1
btPoint2PointConstraint.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btPoint2PointConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include <new>
00020 
00021 
00022 
00023 btPoint2PointConstraint::btPoint2PointConstraint()
00024 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
00025 {
00026 }
00027 
00028 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
00029 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
00030 {
00031 
00032 }
00033 
00034 
00035 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
00036 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
00037 {
00038        
00039 }
00040 
00041 void   btPoint2PointConstraint::buildJacobian()
00042 {
00043        m_appliedImpulse = btScalar(0.);
00044 
00045        btVector3     normal(0,0,0);
00046 
00047        for (int i=0;i<3;i++)
00048        {
00049               normal[i] = 1;
00050               new (&m_jac[i]) btJacobianEntry(
00051                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00052                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00053                      m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
00054                      m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
00055                      normal,
00056                      m_rbA.getInvInertiaDiagLocal(),
00057                      m_rbA.getInvMass(),
00058                      m_rbB.getInvInertiaDiagLocal(),
00059                      m_rbB.getInvMass());
00060               normal[i] = 0;
00061        }
00062 
00063 }
00064 
00065 void   btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
00066 {
00067        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
00068        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
00069 
00070 
00071        btVector3 normal(0,0,0);
00072        
00073 
00074 //     btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
00075 //     btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
00076 
00077        for (int i=0;i<3;i++)
00078        {             
00079               normal[i] = 1;
00080               btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
00081 
00082               btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
00083               btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00084               //this jacobian entry could be re-used for all iterations
00085               
00086               btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
00087               btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
00088               btVector3 vel = vel1 - vel2;
00089               
00090               btScalar rel_vel;
00091               rel_vel = normal.dot(vel);
00092 
00093        /*
00094               //velocity error (first order error)
00095               btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
00096                                                                                                   m_rbB.getLinearVelocity(),angvelB);
00097        */
00098        
00099               //positional error (zeroth order error)
00100               btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
00101               
00102               btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
00103               m_appliedImpulse+=impulse;
00104               btVector3 impulse_vector = normal * impulse;
00105               m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
00106               m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
00107               
00108               normal[i] = 0;
00109        }
00110 }
00111 
00112 void   btPoint2PointConstraint::updateRHS(btScalar      timeStep)
00113 {
00114        (void)timeStep;
00115 
00116 }
00117