Blender  V3.3
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 
17 
19 #include "LinearMath/btVector3.h"
20 #include "btJacobianEntry.h"
21 
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 
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 
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 
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 */
btScalar m_damping
btJacobianEntry
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
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)
SyclQueue void void size_t num_bytes void
int len
Definition: draw_manager.c:108