Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp @ 1972

Last change on this file since 1972 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 14.8 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 "btConvexInternalShape.h"
18#include "btTriangleShape.h"
19#include "btSphereShape.h"
20#include "btCylinderShape.h"
21#include "btCapsuleShape.h"
22#include "btConvexHullShape.h"
23#include "btConvexPointCloudShape.h"
24
25static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints)
26{
27        btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
28        btScalar newDot,maxDot = btScalar(-1e30);
29
30        btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
31        btVector3 vec = vec0;
32        btScalar lenSqr = vec.length2();
33        if (lenSqr < btScalar(0.0001))
34        {
35                vec.setValue(1,0,0);
36        } else {
37                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
38                vec *= rlen;
39        }
40
41
42        for (int i=0;i<numPoints;i++)
43        {
44                btPoint3 vtx = points[i];// * m_localScaling;
45
46                newDot = vec.dot(vtx);
47                if (newDot > maxDot)
48                {
49                        maxDot = newDot;
50                        supVec = vtx;
51                }
52        }
53        return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
54}
55
56btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
57{
58        switch (m_shapeType)
59        {
60    case SPHERE_SHAPE_PROXYTYPE:
61        {
62                return btVector3(0,0,0);
63    }
64        break;
65        case BOX_SHAPE_PROXYTYPE:
66        {
67                btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
68                const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
69
70                return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
71                        btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
72                        btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
73        }
74        break;
75        case TRIANGLE_SHAPE_PROXYTYPE:
76        {
77                btTriangleShape* triangleShape = (btTriangleShape*)this;
78                btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
79                btVector3* vertices = &triangleShape->m_vertices1[0];
80                btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
81                btVector3 sup = vertices[dots.maxAxis()];
82                return btVector3(sup.getX(),sup.getY(),sup.getZ());
83        }
84        break;
85        case CYLINDER_SHAPE_PROXYTYPE:
86        {
87                btCylinderShape* cylShape = (btCylinderShape*)this;
88                //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
89
90                btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
91                btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
92                int cylinderUpAxis = cylShape->getUpAxis();
93                int XX(1),YY(0),ZZ(2);
94
95                switch (cylinderUpAxis)
96                {
97                case 0:
98                {
99                        XX = 1;
100                        YY = 0;
101                        ZZ = 2;
102                }
103                break;
104                case 1:
105                {
106                        XX = 0;
107                        YY = 1;
108                        ZZ = 2; 
109                }
110                break;
111                case 2:
112                {
113                        XX = 0;
114                        YY = 2;
115                        ZZ = 1;
116                       
117                }
118                break;
119                default:
120                        btAssert(0);
121                break;
122                };
123
124                btScalar radius = halfExtents[XX];
125                btScalar halfHeight = halfExtents[cylinderUpAxis];
126
127                btVector3 tmp;
128                btScalar d ;
129
130                btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
131                if (s != btScalar(0.0))
132                {
133                        d = radius / s; 
134                        tmp[XX] = v[XX] * d;
135                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
136                        tmp[ZZ] = v[ZZ] * d;
137                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
138                } else {
139                        tmp[XX] = radius;
140                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
141                        tmp[ZZ] = btScalar(0.0);
142                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
143                }
144        }
145        break;
146        case CAPSULE_SHAPE_PROXYTYPE:
147        {
148                btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
149
150                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
151                btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
152                btScalar halfHeight = capsuleShape->getHalfHeight();
153                int capsuleUpAxis = capsuleShape->getUpAxis();
154
155                btScalar radius = capsuleShape->getRadius();
156                btVector3 supVec(0,0,0);
157
158                btScalar maxDot(btScalar(-1e30));
159
160                btVector3 vec = vec0;
161                btScalar lenSqr = vec.length2();
162                if (lenSqr < btScalar(0.0001))
163                {
164                        vec.setValue(1,0,0);
165                } else
166                {
167                        btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
168                        vec *= rlen;
169                }
170                btVector3 vtx;
171                btScalar newDot;
172                {
173                        btVector3 pos(0,0,0);
174                        pos[capsuleUpAxis] = halfHeight;
175
176                        //vtx = pos +vec*(radius);
177                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
178                        newDot = vec.dot(vtx);
179                       
180
181                        if (newDot > maxDot)
182                        {
183                                maxDot = newDot;
184                                supVec = vtx;
185                        }
186                }
187                {
188                        btVector3 pos(0,0,0);
189                        pos[capsuleUpAxis] = -halfHeight;
190
191                        //vtx = pos +vec*(radius);
192                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
193                        newDot = vec.dot(vtx);
194                        if (newDot > maxDot)
195                        {
196                                maxDot = newDot;
197                                supVec = vtx;
198                        }
199                }
200                return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());   
201        }
202        break;
203        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
204        {
205                btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
206                btVector3* points = convexPointCloudShape->getPoints ();
207                int numPoints = convexPointCloudShape->getNumPoints ();
208                return convexHullSupport (localDir, points, numPoints);
209        }
210        case CONVEX_HULL_SHAPE_PROXYTYPE:
211        {
212                btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
213                btPoint3* points = convexHullShape->getPoints ();
214                int numPoints = convexHullShape->getNumPoints ();
215                return convexHullSupport (localDir, points, numPoints);
216        }
217        break;
218    default:
219#ifndef __SPU__
220                return this->localGetSupportingVertexWithoutMargin (localDir);
221#else
222                btAssert (0);
223#endif
224        break;
225        }
226
227        // should never reach here
228        btAssert (0);
229        return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
230}
231
232btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
233{
234        btVector3 localDirNorm = localDir;
235        if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
236        {
237                localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
238        }
239        localDirNorm.normalize ();
240        switch (m_shapeType)
241        {
242    case SPHERE_SHAPE_PROXYTYPE:
243        {
244                return btVector3(0,0,0) + getMarginNonVirtual() * localDirNorm;
245    }
246        break;
247        case BOX_SHAPE_PROXYTYPE:
248        {
249                btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
250                const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
251
252                return btVector3(localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
253                                                localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
254                                                localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()) + getMarginNonVirtual() * localDirNorm;
255        }
256        break;
257        case TRIANGLE_SHAPE_PROXYTYPE:
258        {
259                btTriangleShape* triangleShape = (btTriangleShape*)this;
260                btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
261                btVector3* vertices = &triangleShape->m_vertices1[0];
262                btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
263                btVector3 sup = vertices[dots.maxAxis()];
264                return btVector3(sup.getX(),sup.getY(),sup.getZ()) + getMarginNonVirtual() * localDirNorm;
265        }
266        break;
267        case CYLINDER_SHAPE_PROXYTYPE:
268        {
269                btCylinderShape* cylShape = (btCylinderShape*)this;
270                //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
271
272                btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
273                btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
274                int cylinderUpAxis = cylShape->getUpAxis();
275                int XX(1),YY(0),ZZ(2);
276
277                switch (cylinderUpAxis)
278                {
279                case 0:
280                {
281                        XX = 1;
282                        YY = 0;
283                        ZZ = 2;
284                }
285                break;
286                case 1:
287                {
288                        XX = 0;
289                        YY = 1;
290                        ZZ = 2; 
291                }
292                break;
293                case 2:
294                {
295                        XX = 0;
296                        YY = 2;
297                        ZZ = 1;
298                       
299                }
300                break;
301                default:
302                        btAssert(0);
303                break;
304                };
305
306                btScalar radius = halfExtents[XX];
307                btScalar halfHeight = halfExtents[cylinderUpAxis];
308
309                btVector3 tmp;
310                btScalar d ;
311
312                btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
313                if (s != btScalar(0.0))
314                {
315                        d = radius / s; 
316                        tmp[XX] = v[XX] * d;
317                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
318                        tmp[ZZ] = v[ZZ] * d;
319                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
320                } else {
321                        tmp[XX] = radius;
322                        tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
323                        tmp[ZZ] = btScalar(0.0);
324                        return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
325                }
326        }
327        break;
328        case CAPSULE_SHAPE_PROXYTYPE:
329        {
330                btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
331
332                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
333                btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
334                btScalar halfHeight = capsuleShape->getHalfHeight();
335                int capsuleUpAxis = capsuleShape->getUpAxis();
336
337                btScalar radius = capsuleShape->getRadius();
338                btVector3 supVec(0,0,0);
339
340                btScalar maxDot(btScalar(-1e30));
341
342                btVector3 vec = vec0;
343                btScalar lenSqr = vec.length2();
344                if (lenSqr < btScalar(0.0001))
345                {
346                        vec.setValue(1,0,0);
347                } else
348                {
349                        btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
350                        vec *= rlen;
351                }
352                btVector3 vtx;
353                btScalar newDot;
354                {
355                        btVector3 pos(0,0,0);
356                        pos[capsuleUpAxis] = halfHeight;
357
358                        vtx = pos +vec*(radius);
359                        newDot = vec.dot(vtx);
360                        if (newDot > maxDot)
361                        {
362                                maxDot = newDot;
363                                supVec = vtx;
364                        }
365                }
366                {
367                        btVector3 pos(0,0,0);
368                        pos[capsuleUpAxis] = -halfHeight;
369
370                        vtx = pos +vec*(radius);
371                        newDot = vec.dot(vtx);
372                        if (newDot > maxDot)
373                        {
374                                maxDot = newDot;
375                                supVec = vtx;
376                        }
377                }
378                return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()) + getMarginNonVirtual() * localDirNorm;
379        }
380        break;
381        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
382        {
383                btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
384                btVector3* points = convexPointCloudShape->getPoints ();
385                int numPoints = convexPointCloudShape->getNumPoints ();
386                return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
387        }
388        case CONVEX_HULL_SHAPE_PROXYTYPE:
389        {
390                btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
391                btPoint3* points = convexHullShape->getPoints ();
392                int numPoints = convexHullShape->getNumPoints ();
393                return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
394        }
395        break;
396    default:
397#ifndef __SPU__
398                return this->localGetSupportingVertex (localDir);
399#else
400                btAssert (0);
401#endif
402        break;
403        }
404
405        // should never reach here
406        btAssert (0);
407        return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
408}
409
410/* TODO: This should be bumped up to btCollisionShape () */
411btScalar btConvexShape::getMarginNonVirtual () const
412{
413        switch (m_shapeType)
414        {
415    case SPHERE_SHAPE_PROXYTYPE:
416        {
417                btSphereShape* sphereShape = (btSphereShape*)this;
418                return sphereShape->getRadius ();
419        }
420        break;
421        case BOX_SHAPE_PROXYTYPE:
422        {
423                btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
424                return convexShape->getMarginNV ();
425        }
426        break;
427        case TRIANGLE_SHAPE_PROXYTYPE:
428        {
429                btTriangleShape* triangleShape = (btTriangleShape*)this;
430                return triangleShape->getMarginNV ();
431        }
432        break;
433        case CYLINDER_SHAPE_PROXYTYPE:
434        {
435                btCylinderShape* cylShape = (btCylinderShape*)this;
436                return cylShape->getMarginNV();
437        }
438        break;
439        case CAPSULE_SHAPE_PROXYTYPE:
440        {
441                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
442                return capsuleShape->getMarginNV();
443        }
444        break;
445        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
446        /* fall through */
447        case CONVEX_HULL_SHAPE_PROXYTYPE:
448        {
449                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
450                return convexHullShape->getMarginNV();
451        }
452        break;
453    default:
454#ifndef __SPU__
455                return this->getMargin ();
456#else
457                btAssert (0);
458#endif
459        break;
460        }
461
462        // should never reach here
463        btAssert (0);
464        return btScalar(0.0f);
465}
466
467void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
468{
469        switch (m_shapeType)
470        {
471    case SPHERE_SHAPE_PROXYTYPE:
472        {
473                btSphereShape* sphereShape = (btSphereShape*)this;
474                float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
475                float margin = radius + sphereShape->getMarginNonVirtual();
476                const btVector3& center = t.getOrigin();
477                btVector3 extent(margin,margin,margin);
478                aabbMin = center - extent;
479                aabbMax = center + extent;
480    }
481        break;
482        case CYLINDER_SHAPE_PROXYTYPE:
483        /* fall through */
484        case BOX_SHAPE_PROXYTYPE:
485        {
486                btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
487                float margin=convexShape->getMarginNonVirtual();
488                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
489                halfExtents += btVector3(margin,margin,margin);
490                btMatrix3x3 abs_b = t.getBasis().absolute(); 
491                btPoint3 center = t.getOrigin();
492                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
493               
494                aabbMin = center - extent;
495                aabbMax = center + extent;
496                break;
497        }
498        break;
499        case TRIANGLE_SHAPE_PROXYTYPE:
500        {
501                btTriangleShape* triangleShape = (btTriangleShape*)this;
502                btScalar margin = triangleShape->getMarginNonVirtual();
503                for (int i=0;i<3;i++)
504                {
505                        btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
506                        vec[i] = btScalar(1.);
507
508                        btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
509
510                        btVector3 tmp = t(sv);
511                        aabbMax[i] = tmp[i]+margin;
512                        vec[i] = btScalar(-1.);
513                        tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
514                        aabbMin[i] = tmp[i]-margin;
515                }       
516        }
517        break;
518        case CAPSULE_SHAPE_PROXYTYPE:
519        {
520                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
521                btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
522                int m_upAxis = capsuleShape->getUpAxis();
523                halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
524                halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
525                btMatrix3x3 abs_b = t.getBasis().absolute(); 
526                btPoint3 center = t.getOrigin();
527                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));                   
528                aabbMin = center - extent;
529                aabbMax = center + extent;
530        }
531        break;
532        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
533        case CONVEX_HULL_SHAPE_PROXYTYPE:
534        {
535                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
536                btScalar margin = convexHullShape->getMarginNonVirtual();
537                convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
538        }
539        break;
540    default:
541#ifndef __SPU__
542                this->getAabb (t, aabbMin, aabbMax);
543#else
544                btAssert (0);
545#endif
546        break;
547        }
548
549        // should never reach here
550        btAssert (0);
551}
Note: See TracBrowser for help on using the repository browser.