angular-only hinge updated
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSolve2LinearConstraint.cpp
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
47         imp0 = 0.f;
48         imp1 = 0.f;
49
50         btScalar len = fabs(normalA.length())-1.f;
51         if (fabs(len) >= SIMD_EPSILON)
52                 return;
53
54         ASSERT(len < SIMD_EPSILON);
55
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 = 1.f / (invMassA + invMassB);
71
72
73         // calculate rhs (or error) terms
74         const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
75         const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
76
77
78         // dC/dv * dv = -C
79         
80         // jacobian * impulse = -error
81         //
82
83         //impulse = jacobianInverse * -error
84
85         // inverting 2x2 symmetric system (offdiagonal are equal!)
86         // 
87
88
89         btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
90         btScalar        invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
91         
92         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
93         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
94
95         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
96         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
97
98         //[a b]                                                           [d -c]
99         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
100
101         //[jA nD] * [imp0] = [dv0]
102         //[nD jB]   [imp1]   [dv1]
103
104 }
105
106
107
108 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
109                                                 btRigidBody* body1,
110                                                 btRigidBody* body2,
111                                                 const btMatrix3x3& world2A,
112                                                 const btMatrix3x3& world2B,
113                                                 
114                                                 const btVector3& invInertiaADiag,
115                                                 const btScalar invMassA,
116                                                 const btVector3& linvelA,const btVector3& angvelA,
117                                                 const btVector3& rel_posA1,
118                                                 const btVector3& invInertiaBDiag,
119                                                 const btScalar invMassB,
120                                                 const btVector3& linvelB,const btVector3& angvelB,
121                                                 const btVector3& rel_posA2,
122
123                                           btScalar depthA, const btVector3& normalA, 
124                                           const btVector3& rel_posB1,const btVector3& rel_posB2,
125                                           btScalar depthB, const btVector3& normalB, 
126                                           btScalar& imp0,btScalar& imp1)
127 {
128
129         imp0 = 0.f;
130         imp1 = 0.f;
131
132         btScalar len = fabs(normalA.length())-1.f;
133         if (fabs(len) >= SIMD_EPSILON)
134                 return;
135
136         ASSERT(len < SIMD_EPSILON);
137
138
139         //this jacobian entry could be re-used for all iterations
140         btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
141                 invInertiaBDiag,invMassB);
142         btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
143                 invInertiaBDiag,invMassB);
144         
145         //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
146         //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
147
148         const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
149         const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
150
151         // calculate rhs (or error) terms
152         const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
153         const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
154
155         // dC/dv * dv = -C
156         
157         // jacobian * impulse = -error
158         //
159
160         //impulse = jacobianInverse * -error
161
162         // inverting 2x2 symmetric system (offdiagonal are equal!)
163         // 
164
165
166         btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
167         btScalar        invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
168         
169         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
170         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
171
172         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
173         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
174
175         //[a b]                                                           [d -c]
176         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
177
178         //[jA nD] * [imp0] = [dv0]
179         //[nD jB]   [imp1]   [dv1]
180
181         if ( imp0 > 0.0f)
182         {
183                 if ( imp1 > 0.0f )
184                 {
185                         //both positive
186                 }
187                 else
188                 {
189                         imp1 = 0.f;
190
191                         // now imp0>0 imp1<0
192                         imp0 = dv0 / jacA.getDiagonal();
193                         if ( imp0 > 0.0f )
194                         {
195                         } else
196                         {
197                                 imp0 = 0.f;
198                         }
199                 }
200         }
201         else
202         {
203                 imp0 = 0.f;
204
205                 imp1 = dv1 / jacB.getDiagonal();
206                 if ( imp1 <= 0.0f )
207                 {
208                         imp1 = 0.f;
209                         // now imp0>0 imp1<0
210                         imp0 = dv0 / jacA.getDiagonal();
211                         if ( imp0 > 0.0f )
212                         {
213                         } else
214                         {
215                                 imp0 = 0.f;
216                         }
217                 } else
218                 {
219                 }
220         }
221 }
222
223
224
225 void btSolve2LinearConstraint::resolveAngularConstraint(        const btMatrix3x3& invInertiaAWS,
226                                                                                         const btScalar invMassA,
227                                                                                         const btVector3& linvelA,const btVector3& angvelA,
228                                                                                         const btVector3& rel_posA1,
229                                                                                         const btMatrix3x3& invInertiaBWS,
230                                                                                         const btScalar invMassB,
231                                                                                         const btVector3& linvelB,const btVector3& angvelB,
232                                                                                         const btVector3& rel_posA2,
233
234                                                                                         btScalar depthA, const btVector3& normalA, 
235                                                                                         const btVector3& rel_posB1,const btVector3& rel_posB2,
236                                                                                         btScalar depthB, const btVector3& normalB, 
237                                                                                         btScalar& imp0,btScalar& imp1)
238 {
239
240 }
241