← Back to branch summary

~ubuntu-branches/ubuntu/precise/supertuxkart/precise

« back to all changes in this revision

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

  • Committer: Bazaar Package Importer
  • Author(s): Christoph Egger
  • Date: 2011-02-24 22:36:25 UTC
  • mfrom: (1.1.9 upstream) (6.1.4 sid)
  • Revision ID: james.westby@ubuntu.com-20110224223625-ygrjfpg92obovuch
Tags: 0.7+dfsg1-1
New upstream release

Show diffs side-by-side

added added

removed removed

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