Bullet Collision Detection & Physics Library
btSolve2LinearConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16
17
19
22#include "btJacobianEntry.h"
23
24
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
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
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/*
238void 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
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
#define SIMD_EPSILON
Definition: btScalar.h:521
#define btAssert(x)
Definition: btScalar.h:131
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
btScalar getDiagonal() const
btScalar getNonDiagonal(const btJacobianEntry &jacB, const btScalar massInvA) const
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
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)
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)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235