Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.

  • Property svn:eol-style set to native
File size: 6.3 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_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                        numIter++;
125                        if (numIter > maxIter)
126                        {
127                                return false; //todo: report a failure
128                        }
129                        btScalar dLambda = btScalar(0.);
130
131                        projectedLinearVelocity = relLinVel.dot(n);
132
133                        //calculate safe moving fraction from distance / (linear+rotational velocity)
134                       
135                        //btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
136                        //btScalar clippedDist  = dist;
137                       
138                       
139                        dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
140
141                       
142                       
143                        lambda = lambda + dLambda;
144
145                        if (lambda > btScalar(1.))
146                                return false;
147
148                        if (lambda < btScalar(0.))
149                                return false;
150
151
152                        //todo: next check with relative epsilon
153                        if (lambda <= lastLambda)
154                        {
155                                return false;
156                                //n.setValue(0,0,0);
157                                break;
158                        }
159                        lastLambda = lambda;
160
161                       
162
163                        //interpolate to next lambda
164                        btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
165
166                        btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
167                        btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
168                        relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
169
170                        result.DebugDraw( lambda );
171
172                        btPointCollector        pointCollector;
173                        btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
174                        btGjkPairDetector::ClosestPointInput input;
175                        input.m_transformA = interpolatedTransA;
176                        input.m_transformB = interpolatedTransB;
177                        gjk.getClosestPoints(input,pointCollector,0);
178                        if (pointCollector.m_hasResult)
179                        {
180                                if (pointCollector.m_distance < btScalar(0.))
181                                {
182                                        //degenerate ?!
183                                        result.m_fraction = lastLambda;
184                                        n = pointCollector.m_normalOnBInWorld;
185                                        result.m_normal=n;//.setValue(1,1,1);// = n;
186                                        result.m_hitPoint = pointCollector.m_pointInWorld;
187                                        return true;
188                                }
189                                c = pointCollector.m_pointInWorld;             
190                                n = pointCollector.m_normalOnBInWorld;
191                                dist = pointCollector.m_distance;
192                        } else
193                        {
194                                //??
195                                return false;
196                        }
197
198                }
199
200                //don't report time of impact for motion away from the contact normal (or causes minor penetration)
201                if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
202                        return false;
203
204                result.m_fraction = lambda;
205                result.m_normal = n;
206                result.m_hitPoint = c;
207                return true;
208        }
209
210        return false;
211
212/*
213//todo:
214        //if movement away from normal, discard result
215        btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
216        if (result.m_fraction < btScalar(1.))
217        {
218                if (move.dot(result.m_normal) <= btScalar(0.))
219                {
220                }
221        }
222*/
223
224}
225
Note: See TracBrowser for help on using the repository browser.