btContactConstraint.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 "btContactConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btVector3.h"
00020 #include "btJacobianEntry.h"
00021 #include "btContactSolverInfo.h"
00022 #include "LinearMath/btMinMax.h"
00023 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
00024 
00025 
00026 
00027 btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
00028 :btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
00029         m_contactManifold(*contactManifold)
00030 {
00031 
00032 }
00033 
00034 btContactConstraint::~btContactConstraint()
00035 {
00036 
00037 }
00038 
00039 void    btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
00040 {
00041         m_contactManifold = *contactManifold;
00042 }
00043 
00044 void btContactConstraint::getInfo1 (btConstraintInfo1* info)
00045 {
00046 
00047 }
00048 
00049 void btContactConstraint::getInfo2 (btConstraintInfo2* info)
00050 {
00051 
00052 }
00053 
00054 void    btContactConstraint::buildJacobian()
00055 {
00056 
00057 }
00058 
00059 
00060 
00061 
00062 
00063 #include "btContactConstraint.h"
00064 #include "BulletDynamics/Dynamics/btRigidBody.h"
00065 #include "LinearMath/btVector3.h"
00066 #include "btJacobianEntry.h"
00067 #include "btContactSolverInfo.h"
00068 #include "LinearMath/btMinMax.h"
00069 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
00070 
00071 
00072 
00073 //response  between two dynamic objects without friction and no restitution, assuming 0 penetration depth
00074 btScalar resolveSingleCollision(
00075         btRigidBody* body1,
00076         btCollisionObject* colObj2,
00077                 const btVector3& contactPositionWorld,
00078                 const btVector3& contactNormalOnB,
00079         const btContactSolverInfo& solverInfo,
00080                 btScalar distance)
00081 {
00082         btRigidBody* body2 = btRigidBody::upcast(colObj2);
00083     
00084         
00085     const btVector3& normal = contactNormalOnB;
00086 
00087     btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); 
00088     btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
00089     
00090     btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
00091         btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
00092     btVector3 vel = vel1 - vel2;
00093     btScalar rel_vel;
00094     rel_vel = normal.dot(vel);
00095     
00096     btScalar combinedRestitution = 0.f;
00097     btScalar restitution = combinedRestitution* -rel_vel;
00098 
00099     btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
00100     btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
00101         btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
00102         btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
00103         btScalar relaxation = 1.f;
00104         btScalar jacDiagABInv = relaxation/(denom0+denom1);
00105 
00106     btScalar penetrationImpulse = positionalError * jacDiagABInv;
00107     btScalar velocityImpulse = velocityError * jacDiagABInv;
00108 
00109     btScalar normalImpulse = penetrationImpulse+velocityImpulse;
00110     normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
00111 
00112         body1->applyImpulse(normal*(normalImpulse), rel_pos1);
00113     if (body2)
00114                 body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
00115     
00116     return normalImpulse;
00117 }
00118 
00119 
00120 //bilateral constraint between two dynamic objects
00121 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
00122                       btRigidBody& body2, const btVector3& pos2,
00123                       btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
00124 {
00125         (void)timeStep;
00126         (void)distance;
00127 
00128 
00129         btScalar normalLenSqr = normal.length2();
00130         btAssert(btFabs(normalLenSqr) < btScalar(1.1));
00131         if (normalLenSqr > btScalar(1.1))
00132         {
00133                 impulse = btScalar(0.);
00134                 return;
00135         }
00136         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
00137         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
00138         //this jacobian entry could be re-used for all iterations
00139         
00140         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00141         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00142         btVector3 vel = vel1 - vel2;
00143         
00144 
00145            btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
00146                 body2.getCenterOfMassTransform().getBasis().transpose(),
00147                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
00148                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
00149 
00150         btScalar jacDiagAB = jac.getDiagonal();
00151         btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
00152         
00153           btScalar rel_vel = jac.getRelativeVelocity(
00154                 body1.getLinearVelocity(),
00155                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
00156                 body2.getLinearVelocity(),
00157                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
00158         btScalar a;
00159         a=jacDiagABInv;
00160 
00161 
00162         rel_vel = normal.dot(vel);
00163         
00164         //todo: move this into proper structure
00165         btScalar contactDamping = btScalar(0.2);
00166 
00167 #ifdef ONLY_USE_LINEAR_MASS
00168         btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
00169         impulse = - contactDamping * rel_vel * massTerm;
00170 #else   
00171         btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
00172         impulse = velocityImpulse;
00173 #endif
00174 }
00175 
00176 
00177 
00178