← Back to branch summary

~ubuntu-branches/ubuntu/intrepid/blender/intrepid-updates

« back to all changes in this revision

Viewing changes to extern/bullet/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Cyril Brulebois
  • Date: 2008-08-08 02:45:40 UTC
  • mfrom: (12.1.14 intrepid)
  • Revision ID: james.westby@ubuntu.com-20080808024540-kkjp7ekfivzhuw3l
Tags: 2.46+dfsg-4
http://bugs.debian.org/492280
* Fix python syntax warning in import_dxf.py, which led to nasty output
  in installation/upgrade logs during byte-compilation, using a patch
  provided by the script author (Closes: #492280):
   - debian/patches/45_fix_python_syntax_warning

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.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 "Solve2LinearConstraint.h"
19
 
 
20
 
#include "Dynamics/RigidBody.h"
21
 
#include "SimdVector3.h"
22
 
#include "JacobianEntry.h"
23
 
 
24
 
 
25
 
void Solve2LinearConstraint::resolveUnilateralPairConstraint(
26
 
                                                                                                   RigidBody* body1,
27
 
                RigidBody* body2,
28
 
 
29
 
                                                const SimdMatrix3x3& world2A,
30
 
                                                const SimdMatrix3x3& world2B,
31
 
                                                
32
 
                                                const SimdVector3& invInertiaADiag,
33
 
                                                const SimdScalar invMassA,
34
 
                                                const SimdVector3& linvelA,const SimdVector3& angvelA,
35
 
                                                const SimdVector3& rel_posA1,
36
 
                                                const SimdVector3& invInertiaBDiag,
37
 
                                                const SimdScalar invMassB,
38
 
                                                const SimdVector3& linvelB,const SimdVector3& angvelB,
39
 
                                                const SimdVector3& rel_posA2,
40
 
 
41
 
                                          SimdScalar depthA, const SimdVector3& normalA, 
42
 
                                          const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
43
 
                                          SimdScalar depthB, const SimdVector3& normalB, 
44
 
                                          SimdScalar& imp0,SimdScalar& imp1)
45
 
{
46
 
 
47
 
        imp0 = 0.f;
48
 
        imp1 = 0.f;
49
 
 
50
 
        SimdScalar 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
 
        JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
59
 
                invInertiaBDiag,invMassB);
60
 
        JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
61
 
                invInertiaBDiag,invMassB);
62
 
        
63
 
        //const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
64
 
        //const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
65
 
 
66
 
        const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
67
 
        const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
68
 
 
69
 
//      SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
70
 
        SimdScalar massTerm = 1.f / (invMassA + invMassB);
71
 
 
72
 
 
73
 
        // calculate rhs (or error) terms
74
 
        const SimdScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
75
 
        const SimdScalar 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
 
        SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
90
 
        SimdScalar      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 Solve2LinearConstraint::resolveBilateralPairConstraint(
109
 
                                                RigidBody* body1,
110
 
                                                RigidBody* body2,
111
 
                                                const SimdMatrix3x3& world2A,
112
 
                                                const SimdMatrix3x3& world2B,
113
 
                                                
114
 
                                                const SimdVector3& invInertiaADiag,
115
 
                                                const SimdScalar invMassA,
116
 
                                                const SimdVector3& linvelA,const SimdVector3& angvelA,
117
 
                                                const SimdVector3& rel_posA1,
118
 
                                                const SimdVector3& invInertiaBDiag,
119
 
                                                const SimdScalar invMassB,
120
 
                                                const SimdVector3& linvelB,const SimdVector3& angvelB,
121
 
                                                const SimdVector3& rel_posA2,
122
 
 
123
 
                                          SimdScalar depthA, const SimdVector3& normalA, 
124
 
                                          const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
125
 
                                          SimdScalar depthB, const SimdVector3& normalB, 
126
 
                                          SimdScalar& imp0,SimdScalar& imp1)
127
 
{
128
 
 
129
 
        imp0 = 0.f;
130
 
        imp1 = 0.f;
131
 
 
132
 
        SimdScalar 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
 
        JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
141
 
                invInertiaBDiag,invMassB);
142
 
        JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
143
 
                invInertiaBDiag,invMassB);
144
 
        
145
 
        //const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
146
 
        //const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
147
 
 
148
 
        const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
149
 
        const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
150
 
 
151
 
        // calculate rhs (or error) terms
152
 
        const SimdScalar dv0 = depthA  * m_tau - vel0 * m_damping;
153
 
        const SimdScalar 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
 
        SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
167
 
        SimdScalar      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 Solve2LinearConstraint::resolveAngularConstraint(  const SimdMatrix3x3& invInertiaAWS,
226
 
                                                                                        const SimdScalar invMassA,
227
 
                                                                                        const SimdVector3& linvelA,const SimdVector3& angvelA,
228
 
                                                                                        const SimdVector3& rel_posA1,
229
 
                                                                                        const SimdMatrix3x3& invInertiaBWS,
230
 
                                                                                        const SimdScalar invMassB,
231
 
                                                                                        const SimdVector3& linvelB,const SimdVector3& angvelB,
232
 
                                                                                        const SimdVector3& rel_posA2,
233
 
 
234
 
                                                                                        SimdScalar depthA, const SimdVector3& normalA, 
235
 
                                                                                        const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
236
 
                                                                                        SimdScalar depthB, const SimdVector3& normalB, 
237
 
                                                                                        SimdScalar& imp0,SimdScalar& imp1)
238
 
{
239
 
 
240
 
}
241