Bullet Collision Detection & Physics Library
btContactConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 
17 #include "btContactConstraint.h"
19 #include "LinearMath/btVector3.h"
20 #include "btJacobianEntry.h"
21 #include "btContactSolverInfo.h"
22 #include "LinearMath/btMinMax.h"
24 
25 
26 
29  m_contactManifold(*contactManifold)
30 {
31 
32 }
33 
35 {
36 
37 }
38 
40 {
41  m_contactManifold = *contactManifold;
42 }
43 
45 {
46 
47 }
48 
50 {
51 
52 }
53 
55 {
56 
57 }
58 
59 
60 
61 
62 
63 #include "btContactConstraint.h"
65 #include "LinearMath/btVector3.h"
66 #include "btJacobianEntry.h"
67 #include "btContactSolverInfo.h"
68 #include "LinearMath/btMinMax.h"
70 
71 
72 
73 //response between two dynamic objects without friction and no restitution, assuming 0 penetration depth
75  btRigidBody* body1,
76  btCollisionObject* colObj2,
77  const btVector3& contactPositionWorld,
78  const btVector3& contactNormalOnB,
79  const btContactSolverInfo& solverInfo,
80  btScalar distance)
81 {
82  btRigidBody* body2 = btRigidBody::upcast(colObj2);
83 
84 
85  const btVector3& normal = contactNormalOnB;
86 
87  btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
88  btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
89 
90  btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
91  btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
92  btVector3 vel = vel1 - vel2;
93  btScalar rel_vel;
94  rel_vel = normal.dot(vel);
95 
96  btScalar combinedRestitution = 0.f;
97  btScalar restitution = combinedRestitution* -rel_vel;
98 
99  btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
100  btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
101  btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
102  btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
103  btScalar relaxation = 1.f;
104  btScalar jacDiagABInv = relaxation/(denom0+denom1);
105 
106  btScalar penetrationImpulse = positionalError * jacDiagABInv;
107  btScalar velocityImpulse = velocityError * jacDiagABInv;
108 
109  btScalar normalImpulse = penetrationImpulse+velocityImpulse;
110  normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
111 
112  body1->applyImpulse(normal*(normalImpulse), rel_pos1);
113  if (body2)
114  body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
115 
116  return normalImpulse;
117 }
118 
119 
120 //bilateral constraint between two dynamic objects
122  btRigidBody& body2, const btVector3& pos2,
123  btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
124 {
125  (void)timeStep;
126  (void)distance;
127 
128 
129  btScalar normalLenSqr = normal.length2();
130  btAssert(btFabs(normalLenSqr) < btScalar(1.1));
131  if (normalLenSqr > btScalar(1.1))
132  {
133  impulse = btScalar(0.);
134  return;
135  }
136  btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
137  btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
138  //this jacobian entry could be re-used for all iterations
139 
140  btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
141  btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
142  btVector3 vel = vel1 - vel2;
143 
144 
147  rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
148  body2.getInvInertiaDiagLocal(),body2.getInvMass());
149 
150  btScalar jacDiagAB = jac.getDiagonal();
151  btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
152 
153  btScalar rel_vel = jac.getRelativeVelocity(
154  body1.getLinearVelocity(),
156  body2.getLinearVelocity(),
158 
159 
160 
161  rel_vel = normal.dot(vel);
162 
163  //todo: move this into proper structure
164  btScalar contactDamping = btScalar(0.2);
165 
166 #ifdef ONLY_USE_LINEAR_MASS
167  btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
168  impulse = - contactDamping * rel_vel * massTerm;
169 #else
170  btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
171  impulse = velocityImpulse;
172 #endif
173 }
174 
175 
176 
177 
btScalar getInvMass() const
Definition: btRigidBody.h:273
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don&#39;t store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:203
virtual void buildJacobian()
obsolete methods
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don&#39;t use them directly
#define btAssert(x)
Definition: btScalar.h:131
btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:403
btContactConstraint(btPersistentManifold *contactManifold, btRigidBody &rbA, btRigidBody &rbB)
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
btTransform & getWorldTransform()
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
void resolveSingleBilateral(btRigidBody &body1, const btVector3 &pos1, btRigidBody &body2, const btVector3 &pos2, btScalar distance, const btVector3 &normal, btScalar &impulse, btScalar timeStep)
resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects ...
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btScalar getDiagonal() const
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:365
btCollisionObject can be used to manage collision detection objects.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:354
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
void setContactManifold(btPersistentManifold *contactManifold)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1030
btScalar resolveSingleCollision(btRigidBody *body1, btCollisionObject *colObj2, const btVector3 &contactPositionWorld, const btVector3 &contactNormalOnB, const btContactSolverInfo &solverInfo, btScalar distance)
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:297
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:362
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:334
btPersistentManifold m_contactManifold
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btFabs(btScalar x)
Definition: btScalar.h:475