Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @ 9032

Last change on this file since 9032 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
  • Property svn:eol-style set to native
File size: 7.5 KB
Line 
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
18#include "btSolve2LinearConstraint.h"
19
20#include "BulletDynamics/Dynamics/btRigidBody.h"
21#include "LinearMath/btVector3.h"
22#include "btJacobianEntry.h"
23
24
25void 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
114void 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/*
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
Note: See TracBrowser for help on using the repository browser.