Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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;
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
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
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
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