← 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/btContinuousConvexCollision.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/btContinuousConvexCollision.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
#include "btContinuousConvexCollision.h"
 
18
#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
19
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 
20
#include "LinearMath/btTransformUtil.h"
 
21
#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
22
 
 
23
#include "btGjkPairDetector.h"
 
24
#include "btPointCollector.h"
 
25
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 
26
 
 
27
 
 
28
 
 
29
btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape*    convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
 
30
:m_simplexSolver(simplexSolver),
 
31
m_penetrationDepthSolver(penetrationDepthSolver),
 
32
m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
 
33
{
 
34
}
 
35
 
 
36
 
 
37
btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape*  convexA,const btStaticPlaneShape*       plane)
 
38
:m_simplexSolver(0),
 
39
m_penetrationDepthSolver(0),
 
40
m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
 
41
{
 
42
}
 
43
 
 
44
 
 
45
/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
 
46
/// You don't want your game ever to lock-up.
 
47
#define MAX_ITERATIONS 64
 
48
 
 
49
void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
 
50
{
 
51
        if (m_convexB1)
 
52
        {
 
53
                m_simplexSolver->reset();
 
54
                btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);               
 
55
                btGjkPairDetector::ClosestPointInput input;
 
56
                input.m_transformA = transA;
 
57
                input.m_transformB = transB;
 
58
                gjk.getClosestPoints(input,pointCollector,0);
 
59
        } else
 
60
        {
 
61
                //convex versus plane
 
62
                const btConvexShape* convexShape = m_convexA;
 
63
                const btStaticPlaneShape* planeShape = m_planeShape;
 
64
                
 
65
                bool hasCollision = false;
 
66
                const btVector3& planeNormal = planeShape->getPlaneNormal();
 
67
                const btScalar& planeConstant = planeShape->getPlaneConstant();
 
68
                
 
69
                btTransform convexWorldTransform = transA;
 
70
                btTransform convexInPlaneTrans;
 
71
                convexInPlaneTrans= transB.inverse() * convexWorldTransform;
 
72
                btTransform planeInConvex;
 
73
                planeInConvex= convexWorldTransform.inverse() * transB;
 
74
                
 
75
                btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
 
76
 
 
77
                btVector3 vtxInPlane = convexInPlaneTrans(vtx);
 
78
                btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
 
79
 
 
80
                btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
 
81
                btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
 
82
                btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
 
83
 
 
84
                pointCollector.addContactPoint(
 
85
                        normalOnSurfaceB,
 
86
                        vtxInPlaneWorld,
 
87
                        distance);
 
88
        }
 
89
}
 
90
 
 
91
bool    btContinuousConvexCollision::calcTimeOfImpact(
 
92
                                const btTransform& fromA,
 
93
                                const btTransform& toA,
 
94
                                const btTransform& fromB,
 
95
                                const btTransform& toB,
 
96
                                CastResult& result)
 
97
{
 
98
 
 
99
 
 
100
        /// compute linear and angular velocity for this interval, to interpolate
 
101
        btVector3 linVelA,angVelA,linVelB,angVelB;
 
102
        btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
 
103
        btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
 
104
 
 
105
 
 
106
        btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
 
107
        btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
 
108
 
 
109
        btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
 
110
        btVector3 relLinVel = (linVelB-linVelA);
 
111
 
 
112
        btScalar relLinVelocLength = (linVelB-linVelA).length();
 
113
        
 
114
        if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
 
115
                return false;
 
116
 
 
117
 
 
118
 
 
119
        btScalar lambda = btScalar(0.);
 
120
        btVector3 v(1,0,0);
 
121
 
 
122
        int maxIter = MAX_ITERATIONS;
 
123
 
 
124
        btVector3 n;
 
125
        n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 
126
        bool hasResult = false;
 
127
        btVector3 c;
 
128
 
 
129
        btScalar lastLambda = lambda;
 
130
        //btScalar epsilon = btScalar(0.001);
 
131
 
 
132
        int numIter = 0;
 
133
        //first solution, using GJK
 
134
 
 
135
 
 
136
        btScalar radius = 0.001f;
 
137
//      result.drawCoordSystem(sphereTr);
 
138
 
 
139
        btPointCollector        pointCollector1;
 
140
 
 
141
        {
 
142
        
 
143
                computeClosestPoints(fromA,fromB,pointCollector1);
 
144
 
 
145
                hasResult = pointCollector1.m_hasResult;
 
146
                c = pointCollector1.m_pointInWorld;
 
147
        }
 
148
 
 
149
        if (hasResult)
 
150
        {
 
151
                btScalar dist;
 
152
                dist = pointCollector1.m_distance + result.m_allowedPenetration;
 
153
                n = pointCollector1.m_normalOnBInWorld;
 
154
                btScalar projectedLinearVelocity = relLinVel.dot(n);
 
155
                if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
 
156
                        return false;
 
157
 
 
158
                //not close enough
 
159
                while (dist > radius)
 
160
                {
 
161
                        if (result.m_debugDrawer)
 
162
                        {
 
163
                                result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
 
164
                        }
 
165
                        btScalar dLambda = btScalar(0.);
 
166
 
 
167
                        projectedLinearVelocity = relLinVel.dot(n);
 
168
 
 
169
                        
 
170
                        //don't report time of impact for motion away from the contact normal (or causes minor penetration)
 
171
                        if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
 
172
                                return false;
 
173
                        
 
174
                        dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
 
175
 
 
176
                        
 
177
                        
 
178
                        lambda = lambda + dLambda;
 
179
 
 
180
                        if (lambda > btScalar(1.))
 
181
                                return false;
 
182
 
 
183
                        if (lambda < btScalar(0.))
 
184
                                return false;
 
185
 
 
186
 
 
187
                        //todo: next check with relative epsilon
 
188
                        if (lambda <= lastLambda)
 
189
                        {
 
190
                                return false;
 
191
                                //n.setValue(0,0,0);
 
192
                                break;
 
193
                        }
 
194
                        lastLambda = lambda;
 
195
 
 
196
                        
 
197
 
 
198
                        //interpolate to next lambda
 
199
                        btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
 
200
 
 
201
                        btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
 
202
                        btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
 
203
                        relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
 
204
 
 
205
                        if (result.m_debugDrawer)
 
206
                        {
 
207
                                result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
 
208
                        }
 
209
 
 
210
                        result.DebugDraw( lambda );
 
211
 
 
212
                        btPointCollector        pointCollector;
 
213
                        computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
 
214
 
 
215
                        if (pointCollector.m_hasResult)
 
216
                        {
 
217
                                dist = pointCollector.m_distance+result.m_allowedPenetration;
 
218
                                c = pointCollector.m_pointInWorld;              
 
219
                                n = pointCollector.m_normalOnBInWorld;
 
220
                        } else
 
221
                        {
 
222
                                result.reportFailure(-1, numIter);
 
223
                                return false;
 
224
                        }
 
225
 
 
226
                        numIter++;
 
227
                        if (numIter > maxIter)
 
228
                        {
 
229
                                result.reportFailure(-2, numIter);
 
230
                                return false;
 
231
                        }
 
232
                }
 
233
        
 
234
                result.m_fraction = lambda;
 
235
                result.m_normal = n;
 
236
                result.m_hitPoint = c;
 
237
                return true;
 
238
        }
 
239
 
 
240
        return false;
 
241
 
 
242
}
 
243