8df876928c19993cb7ebf1ed4c0404b590831600
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btSphereBoxCollisionAlgorithm.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 #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
23 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
24 : btActivatingCollisionAlgorithm(ci,col0,col1),
25 m_ownManifold(false),
26 m_manifoldPtr(mf),
27 m_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
40 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
41 {
42         if (m_ownManifold)
43         {
44                 if (m_manifoldPtr)
45                         m_dispatcher->releaseManifold(m_manifoldPtr);
46         }
47 }
48
49
50
51 void 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
93 btScalar 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
105 btScalar 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
208 btScalar 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