Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/sound3/src/external/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @ 5950

Last change on this file since 5950 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

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