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