Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @ 7983

Last change on this file since 7983 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 6.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
26
27
28btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape*    convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
29:m_simplexSolver(simplexSolver),
30m_penetrationDepthSolver(penetrationDepthSolver),
31m_convexA(convexA),m_convexB(convexB)
32{
33}
34
35/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
36/// You don't want your game ever to lock-up.
37#define MAX_ITERATIONS 64
38
39bool    btContinuousConvexCollision::calcTimeOfImpact(
40                                const btTransform& fromA,
41                                const btTransform& toA,
42                                const btTransform& fromB,
43                                const btTransform& toB,
44                                CastResult& result)
45{
46
47        m_simplexSolver->reset();
48
49        /// compute linear and angular velocity for this interval, to interpolate
50        btVector3 linVelA,angVelA,linVelB,angVelB;
51        btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
52        btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
53
54
55        btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
56        btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
57
58        btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
59        btVector3 relLinVel = (linVelB-linVelA);
60
61        btScalar relLinVelocLength = (linVelB-linVelA).length();
62       
63        if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
64                return false;
65
66
67        btScalar radius = btScalar(0.001);
68
69        btScalar lambda = btScalar(0.);
70        btVector3 v(1,0,0);
71
72        int maxIter = MAX_ITERATIONS;
73
74        btVector3 n;
75        n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
76        bool hasResult = false;
77        btVector3 c;
78
79        btScalar lastLambda = lambda;
80        //btScalar epsilon = btScalar(0.001);
81
82        int numIter = 0;
83        //first solution, using GJK
84
85
86        btTransform identityTrans;
87        identityTrans.setIdentity();
88
89        btSphereShape   raySphere(btScalar(0.0));
90        raySphere.setMargin(btScalar(0.));
91
92
93//      result.drawCoordSystem(sphereTr);
94
95        btPointCollector        pointCollector1;
96
97        {
98               
99                btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver);         
100                btGjkPairDetector::ClosestPointInput input;
101       
102                //we don't use margins during CCD
103        //      gjk.setIgnoreMargin(true);
104
105                input.m_transformA = fromA;
106                input.m_transformB = fromB;
107                gjk.getClosestPoints(input,pointCollector1,0);
108
109                hasResult = pointCollector1.m_hasResult;
110                c = pointCollector1.m_pointInWorld;
111        }
112
113        if (hasResult)
114        {
115                btScalar dist;
116                dist = pointCollector1.m_distance;
117                n = pointCollector1.m_normalOnBInWorld;
118
119                btScalar projectedLinearVelocity = relLinVel.dot(n);
120               
121                //not close enough
122                while (dist > radius)
123                {
124                        if (result.m_debugDrawer)
125                        {
126                                result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
127                        }
128                        numIter++;
129                        if (numIter > maxIter)
130                        {
131                                return false; //todo: report a failure
132                        }
133                        btScalar dLambda = btScalar(0.);
134
135                        projectedLinearVelocity = relLinVel.dot(n);
136
137                        //calculate safe moving fraction from distance / (linear+rotational velocity)
138                       
139                        //btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
140                        //btScalar clippedDist  = dist;
141                       
142                        //don't report time of impact for motion away from the contact normal (or causes minor penetration)
143                        if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
144                                return false;
145                       
146                        dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
147
148                       
149                       
150                        lambda = lambda + dLambda;
151
152                        if (lambda > btScalar(1.))
153                                return false;
154
155                        if (lambda < btScalar(0.))
156                                return false;
157
158
159                        //todo: next check with relative epsilon
160                        if (lambda <= lastLambda)
161                        {
162                                return false;
163                                //n.setValue(0,0,0);
164                                break;
165                        }
166                        lastLambda = lambda;
167
168                       
169
170                        //interpolate to next lambda
171                        btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
172
173                        btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
174                        btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
175                        relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
176
177                        if (result.m_debugDrawer)
178                        {
179                                result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
180                        }
181
182                        result.DebugDraw( lambda );
183
184                        btPointCollector        pointCollector;
185                        btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
186                        btGjkPairDetector::ClosestPointInput input;
187                        input.m_transformA = interpolatedTransA;
188                        input.m_transformB = interpolatedTransB;
189                        gjk.getClosestPoints(input,pointCollector,0);
190                        if (pointCollector.m_hasResult)
191                        {
192                                if (pointCollector.m_distance < btScalar(0.))
193                                {
194                                        //degenerate ?!
195                                        result.m_fraction = lastLambda;
196                                        n = pointCollector.m_normalOnBInWorld;
197                                        result.m_normal=n;//.setValue(1,1,1);// = n;
198                                        result.m_hitPoint = pointCollector.m_pointInWorld;
199                                        return true;
200                                }
201                                c = pointCollector.m_pointInWorld;             
202                                n = pointCollector.m_normalOnBInWorld;
203                                dist = pointCollector.m_distance;
204                        } else
205                        {
206                                //??
207                                return false;
208                        }
209                       
210
211                }
212       
213                if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
214                        return false;
215                       
216                result.m_fraction = lambda;
217                result.m_normal = n;
218                result.m_hitPoint = c;
219                return true;
220        }
221
222        return false;
223
224/*
225//todo:
226        //if movement away from normal, discard result
227        btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
228        if (result.m_fraction < btScalar(1.))
229        {
230                if (move.dot(result.m_normal) <= btScalar(0.))
231                {
232                }
233        }
234*/
235
236}
Note: See TracBrowser for help on using the repository browser.