Bullet Collision Detection & Physics Library: btSolve2LinearConstraint.cpp Source File

Bullet Collision Detection & Physics Library

btSolve2LinearConstraint.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 #include "btSolve2LinearConstraint.h"
17
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btVector3.h"
20 #include "btJacobianEntry.h"
21
22 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
23 btRigidBody* body1,
24 btRigidBody* body2,
25
26 const btMatrix3x3& world2A,
27 const btMatrix3x3& world2B,
28
29 const btVector3& invInertiaADiag,
30 const btScalar invMassA,
31 const btVector3& linvelA, const btVector3& angvelA,
32 const btVector3& rel_posA1,
33 const btVector3& invInertiaBDiag,
34 const btScalar invMassB,
35 const btVector3& linvelB, const btVector3& angvelB,
36 const btVector3& rel_posA2,
37
38 btScalar depthA, const btVector3& normalA,
39 const btVector3& rel_posB1, const btVector3& rel_posB2,
40 btScalar depthB, const btVector3& normalB,
41 btScalar& imp0, btScalar& imp1)
42 {
43 (void)linvelA;
44 (void)linvelB;
45 (void)angvelB;
46 (void)angvelA;
47
48 imp0 = btScalar(0.);
49 imp1 = btScalar(0.);
50
51 btScalar len = btFabs(normalA.length()) - btScalar(1.);
52 if (btFabs(len) >= SIMD_EPSILON)
53 return;
54
55 btAssert(len < SIMD_EPSILON);
56
57 //this jacobian entry could be re-used for all iterations
58 btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
59 invInertiaBDiag, invMassB);
60 btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
61 invInertiaBDiag, invMassB);
62
63 //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
64 //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
65
66 const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
67 const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
68
69 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
70 btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
71
72 // calculate rhs (or error) terms
73 const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
74 const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
75
76 // dC/dv * dv = -C
77
78 // jacobian * impulse = -error
79 //
80
81 //impulse = jacobianInverse * -error
82
83 // inverting 2x2 symmetric system (offdiagonal are equal!)
84 //
85
86 btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
87 btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
88
89 //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
90 //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
91
92 imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
93 imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
94
95 //[a b] [d -c]
96 //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
97
98 //[jA nD] * [imp0] = [dv0]
99 //[nD jB] [imp1] [dv1]
100 }
101
102 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
103 btRigidBody* body1,
104 btRigidBody* body2,
105 const btMatrix3x3& world2A,
106 const btMatrix3x3& world2B,
107
108 const btVector3& invInertiaADiag,
109 const btScalar invMassA,
110 const btVector3& linvelA, const btVector3& angvelA,
111 const btVector3& rel_posA1,
112 const btVector3& invInertiaBDiag,
113 const btScalar invMassB,
114 const btVector3& linvelB, const btVector3& angvelB,
115 const btVector3& rel_posA2,
116
117 btScalar depthA, const btVector3& normalA,
118 const btVector3& rel_posB1, const btVector3& rel_posB2,
119 btScalar depthB, const btVector3& normalB,
120 btScalar& imp0, btScalar& imp1)
121 {
122 (void)linvelA;
123 (void)linvelB;
124 (void)angvelA;
125 (void)angvelB;
126
127 imp0 = btScalar(0.);
128 imp1 = btScalar(0.);
129
130 btScalar len = btFabs(normalA.length()) - btScalar(1.);
131 if (btFabs(len) >= SIMD_EPSILON)
132 return;
133
134 btAssert(len < SIMD_EPSILON);
135
136 //this jacobian entry could be re-used for all iterations
137 btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
138 invInertiaBDiag, invMassB);
139 btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
140 invInertiaBDiag, invMassB);
141
142 //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
143 //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
144
145 const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
146 const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
147
148 // calculate rhs (or error) terms
149 const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
150 const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
151
152 // dC/dv * dv = -C
153
154 // jacobian * impulse = -error
155 //
156
157 //impulse = jacobianInverse * -error
158
159 // inverting 2x2 symmetric system (offdiagonal are equal!)
160 //
161
162 btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
163 btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
164
165 //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
166 //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
167
168 imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
169 imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
170
171 //[a b] [d -c]
172 //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
173
174 //[jA nD] * [imp0] = [dv0]
175 //[nD jB] [imp1] [dv1]
176
177 if (imp0 > btScalar(0.0))
178 {
179 if (imp1 > btScalar(0.0))
180 {
181 //both positive
182 }
183 else
184 {
185 imp1 = btScalar(0.);
186
187 // now imp0>0 imp1<0
188 imp0 = dv0 / jacA.getDiagonal();
189 if (imp0 > btScalar(0.0))
190 {
191 }
192 else
193 {
194 imp0 = btScalar(0.);
195 }
196 }
197 }
198 else
199 {
200 imp0 = btScalar(0.);
201
202 imp1 = dv1 / jacB.getDiagonal();
203 if (imp1 <= btScalar(0.0))
204 {
205 imp1 = btScalar(0.);
206 // now imp0>0 imp1<0
207 imp0 = dv0 / jacA.getDiagonal();
208 if (imp0 > btScalar(0.0))
209 {
210 }
211 else
212 {
213 imp0 = btScalar(0.);
214 }
215 }
216 else
217 {
218 }
219 }
220 }
221
222 /*
223 void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
224 const btScalar invMassA,
225 const btVector3& linvelA,const btVector3& angvelA,
226 const btVector3& rel_posA1,
227 const btMatrix3x3& invInertiaBWS,
228 const btScalar invMassB,
229 const btVector3& linvelB,const btVector3& angvelB,
230 const btVector3& rel_posA2,
231
232 btScalar depthA, const btVector3& normalA,
233 const btVector3& rel_posB1,const btVector3& rel_posB2,
234 btScalar depthB, const btVector3& normalB,
235 btScalar& imp0,btScalar& imp1)
236 {
237
238 }
239 */
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btJacobianEntry::getNonDiagonal
btScalar getNonDiagonal(const btJacobianEntry &jacB, const btScalar massInvA) const
Definition: btJacobianEntry.h:107
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:31
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btRigidBody.h
btJacobianEntry.h
btVector3.h
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btJacobianEntry::getDiagonal
btScalar getDiagonal() const
Definition: btJacobianEntry.h:104
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btSolve2LinearConstraint::m_damping
btScalar m_damping
Definition: btSolve2LinearConstraint.h:28
btSolve2LinearConstraint::m_tau
btScalar m_tau
Definition: btSolve2LinearConstraint.h:27
btSolve2LinearConstraint::resolveBilateralPairConstraint
void resolveBilateralPairConstraint(btRigidBody *body0, btRigidBody *body1, const btMatrix3x3 &world2A, const btMatrix3x3 &world2B, const btVector3 &invInertiaADiag, const btScalar invMassA, const btVector3 &linvelA, const btVector3 &angvelA, const btVector3 &rel_posA1, const btVector3 &invInertiaBDiag, const btScalar invMassB, const btVector3 &linvelB, const btVector3 &angvelB, const btVector3 &rel_posA2, btScalar depthA, const btVector3 &normalA, const btVector3 &rel_posB1, const btVector3 &rel_posB2, btScalar depthB, const btVector3 &normalB, btScalar &imp0, btScalar &imp1)
Definition: btSolve2LinearConstraint.cpp:102
btSolve2LinearConstraint::resolveUnilateralPairConstraint
void resolveUnilateralPairConstraint(btRigidBody *body0, btRigidBody *body1, const btMatrix3x3 &world2A, const btMatrix3x3 &world2B, const btVector3 &invInertiaADiag, const btScalar invMassA, const btVector3 &linvelA, const btVector3 &angvelA, const btVector3 &rel_posA1, const btVector3 &invInertiaBDiag, const btScalar invMassB, const btVector3 &linvelB, const btVector3 &angvelB, const btVector3 &rel_posA2, btScalar depthA, const btVector3 &normalA, const btVector3 &rel_posB1, const btVector3 &rel_posB2, btScalar depthB, const btVector3 &normalB, btScalar &imp0, btScalar &imp1)
Definition: btSolve2LinearConstraint.cpp:22
btSolve2LinearConstraint.h