Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 9.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
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///This file was written by Erwin Coumans
18///Separating axis rest based on work from Pierre Terdiman, see
19///And contact clipping based on work from Simon Hobbs
20
21
22#include "btPolyhedralContactClipping.h"
23#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
24
25#include <float.h> //for FLT_MAX
26
27
28// Clips a face to the back of a plane
29void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
30{
31       
32        int ve;
33        btScalar ds, de;
34        int numVerts = pVtxIn.size();
35        if (numVerts < 2)
36                return;
37
38        btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
39        btVector3 endVertex = pVtxIn[0];
40       
41        ds = planeNormalWS.dot(firstVertex)+planeEqWS;
42
43        for (ve = 0; ve < numVerts; ve++)
44        {
45                endVertex=pVtxIn[ve];
46
47                de = planeNormalWS.dot(endVertex)+planeEqWS;
48
49                if (ds<0)
50                {
51                        if (de<0)
52                        {
53                                // Start < 0, end < 0, so output endVertex
54                                ppVtxOut.push_back(endVertex);
55                        }
56                        else
57                        {
58                                // Start < 0, end >= 0, so output intersection
59                                ppVtxOut.push_back(     firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
60                        }
61                }
62                else
63                {
64                        if (de<0)
65                        {
66                                // Start >= 0, end < 0 so output intersection and end
67                                ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
68                                ppVtxOut.push_back(endVertex);
69                        }
70                }
71                firstVertex = endVertex;
72                ds = de;
73        }
74}
75#include <stdio.h>
76
77
78static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, float& depth)
79{
80        float Min0,Max0;
81        float Min1,Max1;
82        hullA.project(transA,sep_axis, Min0, Max0);
83        hullB.project(transB, sep_axis, Min1, Max1);
84
85        if(Max0<Min1 || Max1<Min0)
86                return false;
87
88        float d0 = Max0 - Min1;
89        assert(d0>=0.0f);
90        float d1 = Max1 - Min0;
91        assert(d1>=0.0f);
92        depth = d0<d1 ? d0:d1;
93        return true;
94}
95
96
97
98static int gActualSATPairTests=0;
99
100inline bool IsAlmostZero(const btVector3& v)
101{
102        if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
103        return true;
104}
105
106
107bool btPolyhedralContactClipping::findSeparatingAxis(   const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
108{
109        gActualSATPairTests++;
110
111#ifdef TEST_INTERNAL_OBJECTS
112        const btVector3 c0 = transA * hullA.mLocalCenter;
113        const btVector3 c1 = transB * hullB.mLocalCenter;
114        const btVector3 DeltaC2 = c0 - c1;
115#endif
116
117        float dmin = FLT_MAX;
118        int curPlaneTests=0;
119
120        int numFacesA = hullA.m_faces.size();
121        // Test normals from hullA
122        for(int i=0;i<numFacesA;i++)
123        {
124                const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
125                const btVector3 faceANormalWS = transA.getBasis() * Normal;
126
127                curPlaneTests++;
128#ifdef TEST_INTERNAL_OBJECTS
129                gExpectedNbTests++;
130                if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, faceANormalWS, hullA, hullB, dmin))
131                        continue;
132                gActualNbTests++;
133#endif
134
135                float d;
136                if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
137                        return false;
138
139                if(d<dmin)
140                {
141                        dmin = d;
142                        sep = faceANormalWS;
143                }
144        }
145
146        int numFacesB = hullB.m_faces.size();
147        // Test normals from hullB
148        for(int i=0;i<numFacesB;i++)
149        {
150                const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
151                const btVector3 WorldNormal = transB.getBasis() * Normal;
152
153                curPlaneTests++;
154#ifdef TEST_INTERNAL_OBJECTS
155                gExpectedNbTests++;
156                if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
157                        continue;
158                gActualNbTests++;
159#endif
160
161                float d;
162                if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
163                        return false;
164
165                if(d<dmin)
166                {
167                        dmin = d;
168                        sep = WorldNormal;
169                }
170        }
171
172        btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
173
174        int curEdgeEdge = 0;
175        // Test edges
176        for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
177        {
178                const btVector3 edge0 = hullA.m_uniqueEdges[e0];
179                const btVector3 WorldEdge0 = transA.getBasis() * edge0;
180                for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
181                {
182                        const btVector3 edge1 = hullB.m_uniqueEdges[e1];
183                        const btVector3 WorldEdge1 = transB.getBasis() * edge1;
184
185                        btVector3 Cross = WorldEdge0.cross(WorldEdge1);
186                        curEdgeEdge++;
187                        if(!IsAlmostZero(Cross))
188                        {
189                                Cross = Cross.normalize();
190
191#ifdef TEST_INTERNAL_OBJECTS
192                                gExpectedNbTests++;
193                                if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
194                                        continue;
195                                gActualNbTests++;
196#endif
197
198                                float dist;
199                                if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
200                                        return false;
201
202                                if(dist<dmin)
203                                {
204                                        dmin = dist;
205                                        sep = Cross;
206                                }
207                        }
208                }
209
210        }
211
212        const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
213        if((deltaC.dot(sep))>0.0f)
214                sep = -sep;
215
216        return true;
217}
218
219void    btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
220{
221        btVertexArray worldVertsB2;
222        btVertexArray* pVtxIn = &worldVertsB1;
223        btVertexArray* pVtxOut = &worldVertsB2;
224        pVtxOut->reserve(pVtxIn->size());
225
226        int closestFaceA=-1;
227        {
228                btScalar dmin = FLT_MAX;
229                for(int face=0;face<hullA.m_faces.size();face++)
230                {
231                        const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
232                        const btVector3 faceANormalWS = transA.getBasis() * Normal;
233               
234                        btScalar d = faceANormalWS.dot(separatingNormal);
235                        if (d < dmin)
236                        {
237                                dmin = d;
238                                closestFaceA = face;
239                        }
240                }
241        }
242        if (closestFaceA<0)
243                return;
244
245        const btFace& polyA = hullA.m_faces[closestFaceA];
246
247                // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
248        int numContacts = pVtxIn->size();
249        int numVerticesA = polyA.m_indices.size();
250        for(int e0=0;e0<numVerticesA;e0++)
251        {
252                /*const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
253                const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
254                const btVector3 edge0 = a - b;
255                const btVector3 WorldEdge0 = transA.getBasis() * edge0;
256                */
257
258                int otherFace = polyA.m_connectedFaces[e0];
259                btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
260                btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
261
262                btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
263                btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
264                //clip face
265
266                clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
267                btSwap(pVtxIn,pVtxOut);
268                pVtxOut->resize(0);
269        }
270
271
272
273//#define ONLY_REPORT_DEEPEST_POINT
274
275        btVector3 point;
276       
277
278        // only keep points that are behind the witness face
279        {
280                btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
281                btScalar localPlaneEq = polyA.m_plane[3];
282                btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
283                btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
284                for (int i=0;i<pVtxIn->size();i++)
285                {
286                       
287                        btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
288                        if (depth <=maxDist && depth >=minDist)
289                        {
290                                btVector3 point = pVtxIn->at(i);
291#ifdef ONLY_REPORT_DEEPEST_POINT
292                                curMaxDist = depth;
293#else
294#if 0
295                                if (depth<-3)
296                                {
297                                        printf("error in btPolyhedralContactClipping depth = %f\n", depth);
298                                        printf("likely wrong separatingNormal passed in\n");
299                                }
300#endif                         
301                                resultOut.addContactPoint(separatingNormal,point,depth);
302#endif
303                        }
304                }
305        }
306#ifdef ONLY_REPORT_DEEPEST_POINT
307        if (curMaxDist<maxDist)
308        {
309                resultOut.addContactPoint(separatingNormal,point,curMaxDist);
310        }
311#endif //ONLY_REPORT_DEEPEST_POINT
312
313}
314
315void    btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
316{
317
318        btScalar curMaxDist=maxDist;
319        int closestFaceB=-1;
320
321        {
322                btScalar dmax = -FLT_MAX;
323                for(int face=0;face<hullB.m_faces.size();face++)
324                {
325                        const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
326                        const btVector3 WorldNormal = transB.getBasis() * Normal;
327
328                        btScalar d = WorldNormal.dot(separatingNormal);
329                        if (d > dmax)
330                        {
331                                dmax = d;
332                                closestFaceB = face;
333                        }
334                }
335        }
336
337
338
339        if (closestFaceB<0)
340        {
341                return;
342        }
343
344
345
346        // setup initial clip face (minimizing face from hull B)
347        btVertexArray worldVertsB1;
348        {
349                const btFace& polyB = hullB.m_faces[closestFaceB];
350                const int numVertices = polyB.m_indices.size();
351                for(int e0=0;e0<numVertices;e0++)
352                {
353                        const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
354                        worldVertsB1.push_back(transB*b);
355                }
356        }
357
358        clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
359
360}
Note: See TracBrowser for help on using the repository browser.