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:521
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btJacobianEntry::getNonDiagonal
btScalar getNonDiagonal(const btJacobianEntry &jacB, const btScalar massInvA) const
Definition: btJacobianEntry.h:109
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:30
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btRigidBody.h
btJacobianEntry.h
btVector3.h
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btJacobianEntry::getDiagonal
btScalar getDiagonal() const
Definition: btJacobianEntry.h:106
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
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
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
btSolve2LinearConstraint.h
Generated by   doxygen 1.8.17