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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btSolve2LinearConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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#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/*
223void 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*/
btJacobianEntry.h
btRigidBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:497
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
btAssert
#define btAssert(x)
Definition btScalar.h:153
btSolve2LinearConstraint.h
btVector3.h
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition btJacobianEntry.h:31
btJacobianEntry::getDiagonal
btScalar getDiagonal() const
Definition btJacobianEntry.h:104
btJacobianEntry::getNonDiagonal
btScalar getNonDiagonal(const btJacobianEntry &jacB, const btScalar massInvA) const
Definition btJacobianEntry.h:107
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition btRigidBody.h:460
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::m_tau
btScalar m_tau
Definition btSolve2LinearConstraint.h:27
btSolve2LinearConstraint::m_damping
btScalar m_damping
Definition btSolve2LinearConstraint.h:28
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
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::length
btScalar length() const
Return the length of the vector.
Definition btVector3.h:257
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229