Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @ 9499

Last change on this file since 9499 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: 7.6 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#include "btSphereBoxCollisionAlgorithm.h"
17#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
18#include "BulletCollision/CollisionShapes/btSphereShape.h"
19#include "BulletCollision/CollisionShapes/btBoxShape.h"
20#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
21//#include <stdio.h>
22
23btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
24: btActivatingCollisionAlgorithm(ci,col0,col1),
25m_ownManifold(false),
26m_manifoldPtr(mf),
27m_isSwapped(isSwapped)
28{
29        btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
30        btCollisionObject* boxObj = m_isSwapped? col0 : col1;
31       
32        if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
33        {
34                m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
35                m_ownManifold = true;
36        }
37}
38
39
40btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
41{
42        if (m_ownManifold)
43        {
44                if (m_manifoldPtr)
45                        m_dispatcher->releaseManifold(m_manifoldPtr);
46        }
47}
48
49
50
51void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
52{
53        (void)dispatchInfo;
54        (void)resultOut;
55        if (!m_manifoldPtr)
56                return;
57
58        btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
59        btCollisionObject* boxObj = m_isSwapped? body0 : body1;
60
61
62        btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
63
64        btVector3 normalOnSurfaceB;
65        btVector3 pOnBox,pOnSphere;
66        btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
67        btScalar radius = sphere0->getRadius();
68       
69        btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
70
71        resultOut->setPersistentManifold(m_manifoldPtr);
72
73        if (dist < SIMD_EPSILON)
74        {
75                btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
76
77                /// report a contact. internally this will be kept persistent, and contact reduction is done
78
79                resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
80               
81        }
82
83        if (m_ownManifold)
84        {
85                if (m_manifoldPtr->getNumContacts())
86                {
87                        resultOut->refreshContactPoints();
88                }
89        }
90
91}
92
93btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
94{
95        (void)resultOut;
96        (void)dispatchInfo;
97        (void)col0;
98        (void)col1;
99
100        //not yet
101        return btScalar(1.);
102}
103
104
105btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
106{
107
108        btScalar margins;
109        btVector3 bounds[2];
110        btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
111       
112        bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
113        bounds[1] = boxShape->getHalfExtentsWithoutMargin();
114
115        margins = boxShape->getMargin();//also add sphereShape margin?
116
117        const btTransform&      m44T = boxObj->getWorldTransform();
118
119        btVector3       boundsVec[2];
120        btScalar        fPenetration;
121
122        boundsVec[0] = bounds[0];
123        boundsVec[1] = bounds[1];
124
125        btVector3       marginsVec( margins, margins, margins );
126
127        // add margins
128        bounds[0] += marginsVec;
129        bounds[1] -= marginsVec;
130
131        /////////////////////////////////////////////////
132
133        btVector3       tmp, prel, n[6], normal, v3P;
134        btScalar   fSep = btScalar(10000000.0), fSepThis;
135
136        n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
137        n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
138        n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
139        n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
140        n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
141        n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
142
143        // convert  point in local space
144        prel = m44T.invXform( sphereCenter);
145       
146        bool    bFound = false;
147
148        v3P = prel;
149
150        for (int i=0;i<6;i++)
151        {
152                int j = i<3? 0:1;
153                if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
154                {
155                        v3P = v3P - n[i]*fSepThis;             
156                        bFound = true;
157                }
158        }
159       
160        //
161
162        if ( bFound )
163        {
164                bounds[0] = boundsVec[0];
165                bounds[1] = boundsVec[1];
166
167                normal = (prel - v3P).normalize();
168                pointOnBox = v3P + normal*margins;
169                v3PointOnSphere = prel - normal*fRadius;
170
171                if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
172                {
173                        return btScalar(1.0);
174                }
175
176                // transform back in world space
177                tmp = m44T( pointOnBox);
178                pointOnBox    = tmp;
179                tmp  = m44T( v3PointOnSphere);         
180                v3PointOnSphere = tmp;
181                btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
182               
183                //if this fails, fallback into deeper penetration case, below
184                if (fSeps2 > SIMD_EPSILON)
185                {
186                        fSep = - btSqrt(fSeps2);
187                        normal = (pointOnBox-v3PointOnSphere);
188                        normal *= btScalar(1.)/fSep;
189                }
190
191                return fSep;
192        }
193
194        //////////////////////////////////////////////////
195        // Deep penetration case
196
197        fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
198
199        bounds[0] = boundsVec[0];
200        bounds[1] = boundsVec[1];
201
202        if ( fPenetration <= btScalar(0.0) )
203                return (fPenetration-margins);
204        else
205                return btScalar(1.0);
206}
207
208btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
209{
210
211        btVector3 bounds[2];
212
213        bounds[0] = aabbMin;
214        bounds[1] = aabbMax;
215
216        btVector3       p0, tmp, prel, n[6], normal;
217        btScalar   fSep = btScalar(-10000000.0), fSepThis;
218
219        // set p0 and normal to a default value to shup up GCC
220        p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
221        normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
222
223        n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
224        n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
225        n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
226        n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
227        n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
228        n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
229
230        const btTransform&      m44T = boxObj->getWorldTransform();
231
232        // convert  point in local space
233        prel = m44T.invXform( sphereCenter);
234
235        ///////////
236
237        for (int i=0;i<6;i++)
238        {
239                int j = i<3 ? 0:1;
240                if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )      return btScalar(1.0);
241                if ( fSepThis > fSep )
242                {
243                        p0 = bounds[j]; normal = (btVector3&)n[i];
244                        fSep = fSepThis;
245                }
246        }
247
248        pointOnBox = prel - normal*(normal.dot((prel-p0)));
249        v3PointOnSphere = pointOnBox + normal*fSep;
250
251        // transform back in world space
252        tmp  = m44T( pointOnBox);               
253        pointOnBox    = tmp;
254        tmp  = m44T( v3PointOnSphere);          v3PointOnSphere = tmp;
255        normal = (pointOnBox-v3PointOnSphere).normalize();
256
257        return fSep;
258
259}
260
Note: See TracBrowser for help on using the repository browser.