Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_merge/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp @ 2442

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

Finally merged physics stuff. Target is physics_merge because I'll have to do some testing first.
  • Property svn:eol-style set to native
File size: 10.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 "btConvexShape.h"
17#include "btTriangleShape.h"
18#include "btSphereShape.h"
19#include "btCylinderShape.h"
20#include "btCapsuleShape.h"
21#include "btConvexHullShape.h"
22#include "btConvexPointCloudShape.h"
23
24btConvexShape::btConvexShape ()
25{
26}
27
28btConvexShape::~btConvexShape()
29{
30
31}
32
33
34
35static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling)
36{
37        btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
38        btScalar newDot,maxDot = btScalar(-1e30);
39
40        btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
41        btVector3 vec = vec0;
42        btScalar lenSqr = vec.length2();
43        if (lenSqr < btScalar(0.0001))
44        {
45                vec.setValue(1,0,0);
46        } else {
47                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
48                vec *= rlen;
49        }
50
51
52        for (int i=0;i<numPoints;i++)
53        {
54                btVector3 vtx = points[i] * localScaling;
55
56                newDot = vec.dot(vtx);
57                if (newDot > maxDot)
58                {
59                        maxDot = newDot;
60                        supVec = vtx;
61                }
62        }
63        return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
64}
65
66btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
67{
68        switch (m_shapeType)
69        {
70    case SPHERE_SHAPE_PROXYTYPE:
71        {
72                return btVector3(0,0,0);
73    }
74        case BOX_SHAPE_PROXYTYPE:
75        {
76                btBoxShape* convexShape = (btBoxShape*)this;
77                const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
78
79                return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
80                        btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
81                        btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
82        }
83        case TRIANGLE_SHAPE_PROXYTYPE:
84        {
85                btTriangleShape* triangleShape = (btTriangleShape*)this;
86                btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
87                btVector3* vertices = &triangleShape->m_vertices1[0];
88                btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
89                btVector3 sup = vertices[dots.maxAxis()];
90                return btVector3(sup.getX(),sup.getY(),sup.getZ());
91        }
92        case CYLINDER_SHAPE_PROXYTYPE:
93        {
94                btCylinderShape* cylShape = (btCylinderShape*)this;
95                //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
96
97                btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
98                btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
99                int cylinderUpAxis = cylShape->getUpAxis();
100                int XX(1),YY(0),ZZ(2);
101
102                switch (cylinderUpAxis)
103                {
104                case 0:
105                {
106                        XX = 1;
107                        YY = 0;
108                        ZZ = 2;
109                }
110                break;
111                case 1:
112                {
113                        XX = 0;
114                        YY = 1;
115                        ZZ = 2; 
116                }
117                break;
118                case 2:
119                {
120                        XX = 0;
121                        YY = 2;
122                        ZZ = 1;
123                       
124                }
125                break;
126                default:
127                        btAssert(0);
128                break;
129                };
130
131                btScalar radius = halfExtents[XX];
132                btScalar halfHeight = halfExtents[cylinderUpAxis];
133
134                btVector3 tmp;
135                btScalar d ;
136
137                btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
138                if (s != btScalar(0.0))
139                {
140                        d = radius / s; 
141                        tmp[XX] = v[XX] * d;
142                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
143                        tmp[ZZ] = v[ZZ] * d;
144                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
145                } else {
146                        tmp[XX] = radius;
147                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
148                        tmp[ZZ] = btScalar(0.0);
149                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
150                }
151        }
152        case CAPSULE_SHAPE_PROXYTYPE:
153        {
154                btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
155
156                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
157                btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
158                btScalar halfHeight = capsuleShape->getHalfHeight();
159                int capsuleUpAxis = capsuleShape->getUpAxis();
160
161                btScalar radius = capsuleShape->getRadius();
162                btVector3 supVec(0,0,0);
163
164                btScalar maxDot(btScalar(-1e30));
165
166                btVector3 vec = vec0;
167                btScalar lenSqr = vec.length2();
168                if (lenSqr < btScalar(0.0001))
169                {
170                        vec.setValue(1,0,0);
171                } else
172                {
173                        btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
174                        vec *= rlen;
175                }
176                btVector3 vtx;
177                btScalar newDot;
178                {
179                        btVector3 pos(0,0,0);
180                        pos[capsuleUpAxis] = halfHeight;
181
182                        //vtx = pos +vec*(radius);
183                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
184                        newDot = vec.dot(vtx);
185                       
186
187                        if (newDot > maxDot)
188                        {
189                                maxDot = newDot;
190                                supVec = vtx;
191                        }
192                }
193                {
194                        btVector3 pos(0,0,0);
195                        pos[capsuleUpAxis] = -halfHeight;
196
197                        //vtx = pos +vec*(radius);
198                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
199                        newDot = vec.dot(vtx);
200                        if (newDot > maxDot)
201                        {
202                                maxDot = newDot;
203                                supVec = vtx;
204                        }
205                }
206                return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());   
207        }
208        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
209        {
210                btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
211                btVector3* points = convexPointCloudShape->getUnscaledPoints ();
212                int numPoints = convexPointCloudShape->getNumPoints ();
213                return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
214        }
215        case CONVEX_HULL_SHAPE_PROXYTYPE:
216        {
217                btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
218                btVector3* points = convexHullShape->getUnscaledPoints();
219                int numPoints = convexHullShape->getNumPoints ();
220                return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
221        }
222    default:
223#ifndef __SPU__
224                return this->localGetSupportingVertexWithoutMargin (localDir);
225#else
226                btAssert (0);
227#endif
228        }
229
230        // should never reach here
231        btAssert (0);
232        return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
233}
234
235btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
236{
237        btVector3 localDirNorm = localDir;
238        if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
239        {
240                localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
241        }
242        localDirNorm.normalize ();
243
244        return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
245}
246
247/* TODO: This should be bumped up to btCollisionShape () */
248btScalar btConvexShape::getMarginNonVirtual () const
249{
250        switch (m_shapeType)
251        {
252    case SPHERE_SHAPE_PROXYTYPE:
253        {
254                btSphereShape* sphereShape = (btSphereShape*)this;
255                return sphereShape->getRadius ();
256        }
257        case BOX_SHAPE_PROXYTYPE:
258        {
259                btBoxShape* convexShape = (btBoxShape*)this;
260                return convexShape->getMarginNV ();
261        }
262        case TRIANGLE_SHAPE_PROXYTYPE:
263        {
264                btTriangleShape* triangleShape = (btTriangleShape*)this;
265                return triangleShape->getMarginNV ();
266        }
267        case CYLINDER_SHAPE_PROXYTYPE:
268        {
269                btCylinderShape* cylShape = (btCylinderShape*)this;
270                return cylShape->getMarginNV();
271        }
272        case CAPSULE_SHAPE_PROXYTYPE:
273        {
274                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
275                return capsuleShape->getMarginNV();
276        }
277        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
278        /* fall through */
279        case CONVEX_HULL_SHAPE_PROXYTYPE:
280        {
281                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
282                return convexHullShape->getMarginNV();
283        }
284    default:
285#ifndef __SPU__
286                return this->getMargin ();
287#else
288                btAssert (0);
289#endif
290        }
291
292        // should never reach here
293        btAssert (0);
294        return btScalar(0.0f);
295}
296
297void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
298{
299        switch (m_shapeType)
300        {
301    case SPHERE_SHAPE_PROXYTYPE:
302        {
303                btSphereShape* sphereShape = (btSphereShape*)this;
304                float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
305                float margin = radius + sphereShape->getMarginNonVirtual();
306                const btVector3& center = t.getOrigin();
307                btVector3 extent(margin,margin,margin);
308                aabbMin = center - extent;
309                aabbMax = center + extent;
310    }
311        break;
312        case CYLINDER_SHAPE_PROXYTYPE:
313        /* fall through */
314        case BOX_SHAPE_PROXYTYPE:
315        {
316                btBoxShape* convexShape = (btBoxShape*)this;
317                float margin=convexShape->getMarginNonVirtual();
318                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
319                halfExtents += btVector3(margin,margin,margin);
320                btMatrix3x3 abs_b = t.getBasis().absolute(); 
321                btVector3 center = t.getOrigin();
322                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
323               
324                aabbMin = center - extent;
325                aabbMax = center + extent;
326                break;
327        }
328        case TRIANGLE_SHAPE_PROXYTYPE:
329        {
330                btTriangleShape* triangleShape = (btTriangleShape*)this;
331                btScalar margin = triangleShape->getMarginNonVirtual();
332                for (int i=0;i<3;i++)
333                {
334                        btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
335                        vec[i] = btScalar(1.);
336
337                        btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
338
339                        btVector3 tmp = t(sv);
340                        aabbMax[i] = tmp[i]+margin;
341                        vec[i] = btScalar(-1.);
342                        tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
343                        aabbMin[i] = tmp[i]-margin;
344                }       
345        }
346        break;
347        case CAPSULE_SHAPE_PROXYTYPE:
348        {
349                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
350                btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
351                int m_upAxis = capsuleShape->getUpAxis();
352                halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
353                halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
354                btMatrix3x3 abs_b = t.getBasis().absolute(); 
355                btVector3 center = t.getOrigin();
356                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));                   
357                aabbMin = center - extent;
358                aabbMax = center + extent;
359        }
360        break;
361        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
362        case CONVEX_HULL_SHAPE_PROXYTYPE:
363        {
364                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
365                btScalar margin = convexHullShape->getMarginNonVirtual();
366                convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
367        }
368        break;
369    default:
370#ifndef __SPU__
371                this->getAabb (t, aabbMin, aabbMax);
372#else
373                btAssert (0);
374#endif
375        break;
376        }
377
378        // should never reach here
379        btAssert (0);
380}
Note: See TracBrowser for help on using the repository browser.