← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.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
#include "btMinkowskiPenetrationDepthSolver.h"
 
17
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 
18
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 
19
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 
20
#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
21
 
 
22
#define NUM_UNITSPHERE_POINTS 42
 
23
 
 
24
 
 
25
bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
 
26
                                                                                                   const btConvexShape* convexA,const btConvexShape* convexB,
 
27
                                                                                                   const btTransform& transA,const btTransform& transB,
 
28
                                                                                                   btVector3& v, btVector3& pa, btVector3& pb,
 
29
                                                                                                   class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
 
30
                                                                                                   )
 
31
{
 
32
 
 
33
        (void)stackAlloc;
 
34
        (void)v;
 
35
        
 
36
        bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
 
37
 
 
38
        struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
 
39
        {
 
40
 
 
41
                btIntermediateResult():m_hasResult(false)
 
42
                {
 
43
                }
 
44
                
 
45
                btVector3 m_normalOnBInWorld;
 
46
                btVector3 m_pointInWorld;
 
47
                btScalar m_depth;
 
48
                bool    m_hasResult;
 
49
 
 
50
                virtual void setShapeIdentifiersA(int partId0,int index0)
 
51
                {
 
52
                        (void)partId0;
 
53
                        (void)index0;
 
54
                }
 
55
                virtual void setShapeIdentifiersB(int partId1,int index1)
 
56
                {
 
57
                        (void)partId1;
 
58
                        (void)index1;
 
59
                }
 
60
                void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 
61
                {
 
62
                        m_normalOnBInWorld = normalOnBInWorld;
 
63
                        m_pointInWorld = pointInWorld;
 
64
                        m_depth = depth;
 
65
                        m_hasResult = true;
 
66
                }
 
67
        };
 
68
 
 
69
        //just take fixed number of orientation, and sample the penetration depth in that direction
 
70
        btScalar minProj = btScalar(BT_LARGE_FLOAT);
 
71
        btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
 
72
        btVector3 minA,minB;
 
73
        btVector3 seperatingAxisInA,seperatingAxisInB;
 
74
        btVector3 pInA,qInB,pWorld,qWorld,w;
 
75
 
 
76
#ifndef __SPU__
 
77
#define USE_BATCHED_SUPPORT 1
 
78
#endif
 
79
#ifdef USE_BATCHED_SUPPORT
 
80
 
 
81
        btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 
82
        btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 
83
        btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 
84
        btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 
85
        int i;
 
86
 
 
87
        int numSampleDirections = NUM_UNITSPHERE_POINTS;
 
88
 
 
89
        for (i=0;i<numSampleDirections;i++)
 
90
        {
 
91
                btVector3 norm = getPenetrationDirections()[i];
 
92
                seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
 
93
                seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
 
94
        }
 
95
 
 
96
        {
 
97
                int numPDA = convexA->getNumPreferredPenetrationDirections();
 
98
                if (numPDA)
 
99
                {
 
100
                        for (int i=0;i<numPDA;i++)
 
101
                        {
 
102
                                btVector3 norm;
 
103
                                convexA->getPreferredPenetrationDirection(i,norm);
 
104
                                norm  = transA.getBasis() * norm;
 
105
                                getPenetrationDirections()[numSampleDirections] = norm;
 
106
                                seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 
107
                                seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 
108
                                numSampleDirections++;
 
109
                        }
 
110
                }
 
111
        }
 
112
 
 
113
        {
 
114
                int numPDB = convexB->getNumPreferredPenetrationDirections();
 
115
                if (numPDB)
 
116
                {
 
117
                        for (int i=0;i<numPDB;i++)
 
118
                        {
 
119
                                btVector3 norm;
 
120
                                convexB->getPreferredPenetrationDirection(i,norm);
 
121
                                norm  = transB.getBasis() * norm;
 
122
                                getPenetrationDirections()[numSampleDirections] = norm;
 
123
                                seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 
124
                                seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 
125
                                numSampleDirections++;
 
126
                        }
 
127
                }
 
128
        }
 
129
 
 
130
 
 
131
 
 
132
 
 
133
        convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
 
134
        convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
 
135
 
 
136
        for (i=0;i<numSampleDirections;i++)
 
137
        {
 
138
                btVector3 norm = getPenetrationDirections()[i];
 
139
                if (check2d)
 
140
                {
 
141
                        norm[2] = 0.f;
 
142
                }
 
143
                if (norm.length2()>0.01)
 
144
                {
 
145
 
 
146
                        seperatingAxisInA = seperatingAxisInABatch[i];
 
147
                        seperatingAxisInB = seperatingAxisInBBatch[i];
 
148
 
 
149
                        pInA = supportVerticesABatch[i];
 
150
                        qInB = supportVerticesBBatch[i];
 
151
 
 
152
                        pWorld = transA(pInA);  
 
153
                        qWorld = transB(qInB);
 
154
                        if (check2d)
 
155
                        {
 
156
                                pWorld[2] = 0.f;
 
157
                                qWorld[2] = 0.f;
 
158
                        }
 
159
 
 
160
                        w       = qWorld - pWorld;
 
161
                        btScalar delta = norm.dot(w);
 
162
                        //find smallest delta
 
163
                        if (delta < minProj)
 
164
                        {
 
165
                                minProj = delta;
 
166
                                minNorm = norm;
 
167
                                minA = pWorld;
 
168
                                minB = qWorld;
 
169
                        }
 
170
                }
 
171
        }       
 
172
#else
 
173
 
 
174
        int numSampleDirections = NUM_UNITSPHERE_POINTS;
 
175
 
 
176
#ifndef __SPU__
 
177
        {
 
178
                int numPDA = convexA->getNumPreferredPenetrationDirections();
 
179
                if (numPDA)
 
180
                {
 
181
                        for (int i=0;i<numPDA;i++)
 
182
                        {
 
183
                                btVector3 norm;
 
184
                                convexA->getPreferredPenetrationDirection(i,norm);
 
185
                                norm  = transA.getBasis() * norm;
 
186
                                getPenetrationDirections()[numSampleDirections] = norm;
 
187
                                numSampleDirections++;
 
188
                        }
 
189
                }
 
190
        }
 
191
 
 
192
        {
 
193
                int numPDB = convexB->getNumPreferredPenetrationDirections();
 
194
                if (numPDB)
 
195
                {
 
196
                        for (int i=0;i<numPDB;i++)
 
197
                        {
 
198
                                btVector3 norm;
 
199
                                convexB->getPreferredPenetrationDirection(i,norm);
 
200
                                norm  = transB.getBasis() * norm;
 
201
                                getPenetrationDirections()[numSampleDirections] = norm;
 
202
                                numSampleDirections++;
 
203
                        }
 
204
                }
 
205
        }
 
206
#endif // __SPU__
 
207
 
 
208
        for (int i=0;i<numSampleDirections;i++)
 
209
        {
 
210
                const btVector3& norm = getPenetrationDirections()[i];
 
211
                seperatingAxisInA = (-norm)* transA.getBasis();
 
212
                seperatingAxisInB = norm* transB.getBasis();
 
213
                pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 
214
                qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
215
                pWorld = transA(pInA);  
 
216
                qWorld = transB(qInB);
 
217
                w       = qWorld - pWorld;
 
218
                btScalar delta = norm.dot(w);
 
219
                //find smallest delta
 
220
                if (delta < minProj)
 
221
                {
 
222
                        minProj = delta;
 
223
                        minNorm = norm;
 
224
                        minA = pWorld;
 
225
                        minB = qWorld;
 
226
                }
 
227
        }
 
228
#endif //USE_BATCHED_SUPPORT
 
229
 
 
230
        //add the margins
 
231
 
 
232
        minA += minNorm*convexA->getMarginNonVirtual();
 
233
        minB -= minNorm*convexB->getMarginNonVirtual();
 
234
        //no penetration
 
235
        if (minProj < btScalar(0.))
 
236
                return false;
 
237
 
 
238
        btScalar extraSeparation = 0.5f;///scale dependent
 
239
        minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 
240
 
 
241
 
 
242
 
 
243
 
 
244
 
 
245
//#define DEBUG_DRAW 1
 
246
#ifdef DEBUG_DRAW
 
247
        if (debugDraw)
 
248
        {
 
249
                btVector3 color(0,1,0);
 
250
                debugDraw->drawLine(minA,minB,color);
 
251
                color = btVector3 (1,1,1);
 
252
                btVector3 vec = minB-minA;
 
253
                btScalar prj2 = minNorm.dot(vec);
 
254
                debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
 
255
 
 
256
        }
 
257
#endif //DEBUG_DRAW
 
258
 
 
259
        
 
260
 
 
261
        btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
 
262
 
 
263
        btScalar offsetDist = minProj;
 
264
        btVector3 offset = minNorm * offsetDist;
 
265
        
 
266
 
 
267
 
 
268
        btGjkPairDetector::ClosestPointInput input;
 
269
                
 
270
        btVector3 newOrg = transA.getOrigin() + offset;
 
271
 
 
272
        btTransform displacedTrans = transA;
 
273
        displacedTrans.setOrigin(newOrg);
 
274
 
 
275
        input.m_transformA = displacedTrans;
 
276
        input.m_transformB = transB;
 
277
        input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
 
278
        
 
279
        btIntermediateResult res;
 
280
        gjkdet.setCachedSeperatingAxis(-minNorm);
 
281
        gjkdet.getClosestPoints(input,res,debugDraw);
 
282
 
 
283
        btScalar correctedMinNorm = minProj - res.m_depth;
 
284
 
 
285
 
 
286
        //the penetration depth is over-estimated, relax it
 
287
        btScalar penetration_relaxation= btScalar(1.);
 
288
        minNorm*=penetration_relaxation;
 
289
        
 
290
 
 
291
        if (res.m_hasResult)
 
292
        {
 
293
 
 
294
                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 
295
                pb = res.m_pointInWorld;
 
296
                v = minNorm;
 
297
                
 
298
#ifdef DEBUG_DRAW
 
299
                if (debugDraw)
 
300
                {
 
301
                        btVector3 color(1,0,0);
 
302
                        debugDraw->drawLine(pa,pb,color);
 
303
                }
 
304
#endif//DEBUG_DRAW
 
305
 
 
306
 
 
307
        }
 
308
        return res.m_hasResult;
 
309
}
 
310
 
 
311
btVector3*      btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
 
312
{
 
313
        static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
 
314
        {
 
315
        btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
 
316
        btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
 
317
        btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
 
318
        btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
 
319
        btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
 
320
        btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
 
321
        btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
 
322
        btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
 
323
        btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
 
324
        btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
 
325
        btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
 
326
        btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
 
327
        btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
 
328
        btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
 
329
        btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
 
330
        btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
 
331
        btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
 
332
        btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
 
333
        btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
 
334
        btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
 
335
        btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
 
336
        btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
 
337
        btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
 
338
        btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
 
339
        btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
 
340
        btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
 
341
        btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
 
342
        btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
 
343
        btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
 
344
        btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
 
345
        btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
 
346
        btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
 
347
        btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
 
348
        btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
 
349
        btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
 
350
        btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
 
351
        btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
 
352
        btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
 
353
        btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
 
354
        btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
 
355
        btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
 
356
        btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
 
357
        };
 
358
 
 
359
        return sPenetrationDirections;
 
360
}
 
361
 
 
362