← Back to branch summary

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

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.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/CollisionDispatch/btConvex2dConvex2dAlgorithm.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 "btConvex2dConvex2dAlgorithm.h"
 
17
 
 
18
//#include <stdio.h>
 
19
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 
20
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 
21
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
22
#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
23
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 
24
 
 
25
 
 
26
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 
27
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
28
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
29
#include "BulletCollision/CollisionShapes/btBoxShape.h"
 
30
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 
31
 
 
32
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
 
33
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
 
34
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 
35
#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 
36
 
 
37
 
 
38
 
 
39
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 
40
#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
41
 
 
42
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
 
43
 
 
44
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
45
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 
46
 
 
47
 
 
48
btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                   simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 
49
{
 
50
        m_numPerturbationIterations = 0;
 
51
        m_minimumPointsPerturbationThreshold = 3;
 
52
        m_simplexSolver = simplexSolver;
 
53
        m_pdSolver = pdSolver;
 
54
}
 
55
 
 
56
btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
 
57
{ 
 
58
}
 
59
 
 
60
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 
61
: btActivatingCollisionAlgorithm(ci,body0,body1),
 
62
m_simplexSolver(simplexSolver),
 
63
m_pdSolver(pdSolver),
 
64
m_ownManifold (false),
 
65
m_manifoldPtr(mf),
 
66
m_lowLevelOfDetail(false),
 
67
 m_numPerturbationIterations(numPerturbationIterations),
 
68
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 
69
{
 
70
        (void)body0;
 
71
        (void)body1;
 
72
}
 
73
 
 
74
 
 
75
 
 
76
 
 
77
btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
 
78
{
 
79
        if (m_ownManifold)
 
80
        {
 
81
                if (m_manifoldPtr)
 
82
                        m_dispatcher->releaseManifold(m_manifoldPtr);
 
83
        }
 
84
}
 
85
 
 
86
void    btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 
87
{
 
88
        m_lowLevelOfDetail = useLowLevel;
 
89
}
 
90
 
 
91
 
 
92
 
 
93
extern btScalar gContactBreakingThreshold;
 
94
 
 
95
 
 
96
//
 
97
// Convex-Convex collision algorithm
 
98
//
 
99
void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
100
{
 
101
 
 
102
        if (!m_manifoldPtr)
 
103
        {
 
104
                //swapped?
 
105
                m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
 
106
                m_ownManifold = true;
 
107
        }
 
108
        resultOut->setPersistentManifold(m_manifoldPtr);
 
109
 
 
110
        //comment-out next line to test multi-contact generation
 
111
        //resultOut->getPersistentManifold()->clearManifold();
 
112
 
 
113
 
 
114
        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
 
115
        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
 
116
 
 
117
        btVector3  normalOnB;
 
118
        btVector3  pointOnBWorld;
 
119
 
 
120
        {
 
121
 
 
122
 
 
123
                btGjkPairDetector::ClosestPointInput input;
 
124
 
 
125
                btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
 
126
                //TODO: if (dispatchInfo.m_useContinuous)
 
127
                gjkPairDetector.setMinkowskiA(min0);
 
128
                gjkPairDetector.setMinkowskiB(min1);
 
129
 
 
130
                {
 
131
                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
 
132
                        input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 
133
                }
 
134
 
 
135
                input.m_stackAlloc = dispatchInfo.m_stackAllocator;
 
136
                input.m_transformA = body0->getWorldTransform();
 
137
                input.m_transformB = body1->getWorldTransform();
 
138
 
 
139
                gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
140
 
 
141
                btVector3 v0,v1;
 
142
                btVector3 sepNormalWorldSpace;
 
143
 
 
144
        }
 
145
 
 
146
        if (m_ownManifold)
 
147
        {
 
148
                resultOut->refreshContactPoints();
 
149
        }
 
150
 
 
151
}
 
152
 
 
153
 
 
154
 
 
155
 
 
156
btScalar        btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
157
{
 
158
        (void)resultOut;
 
159
        (void)dispatchInfo;
 
160
        ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
 
161
 
 
162
        ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
 
163
        ///col0->m_worldTransform,
 
164
        btScalar resultFraction = btScalar(1.);
 
165
 
 
166
 
 
167
        btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
 
168
        btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
 
169
 
 
170
        if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
 
171
                squareMot1 < col1->getCcdSquareMotionThreshold())
 
172
                return resultFraction;
 
173
 
 
174
 
 
175
        //An adhoc way of testing the Continuous Collision Detection algorithms
 
176
        //One object is approximated as a sphere, to simplify things
 
177
        //Starting in penetration should report no time of impact
 
178
        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 
179
        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
180
 
 
181
 
 
182
        /// Convex0 against sphere for Convex1
 
183
        {
 
184
                btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 
185
 
 
186
                btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
 
187
                btConvexCast::CastResult result;
 
188
                btVoronoiSimplexSolver voronoiSimplex;
 
189
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 
190
                ///Simplification, one object is simplified as a sphere
 
191
                btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
 
192
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
193
                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
 
194
                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
 
195
                {
 
196
 
 
197
                        //store result.m_fraction in both bodies
 
198
 
 
199
                        if (col0->getHitFraction()> result.m_fraction)
 
200
                                col0->setHitFraction( result.m_fraction );
 
201
 
 
202
                        if (col1->getHitFraction() > result.m_fraction)
 
203
                                col1->setHitFraction( result.m_fraction);
 
204
 
 
205
                        if (resultFraction > result.m_fraction)
 
206
                                resultFraction = result.m_fraction;
 
207
 
 
208
                }
 
209
 
 
210
 
 
211
 
 
212
 
 
213
        }
 
214
 
 
215
        /// Sphere (for convex0) against Convex1
 
216
        {
 
217
                btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 
218
 
 
219
                btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
 
220
                btConvexCast::CastResult result;
 
221
                btVoronoiSimplexSolver voronoiSimplex;
 
222
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 
223
                ///Simplification, one object is simplified as a sphere
 
224
                btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
 
225
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
226
                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
 
227
                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
 
228
                {
 
229
 
 
230
                        //store result.m_fraction in both bodies
 
231
 
 
232
                        if (col0->getHitFraction()      > result.m_fraction)
 
233
                                col0->setHitFraction( result.m_fraction);
 
234
 
 
235
                        if (col1->getHitFraction() > result.m_fraction)
 
236
                                col1->setHitFraction( result.m_fraction);
 
237
 
 
238
                        if (resultFraction > result.m_fraction)
 
239
                                resultFraction = result.m_fraction;
 
240
 
 
241
                }
 
242
        }
 
243
 
 
244
        return resultFraction;
 
245
 
 
246
}
 
247