Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ai/src/external/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp @ 6847

Last change on this file since 6847 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: 10.5 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                btScalar halfHeight = capsuleShape->getHalfHeight();
158                int capsuleUpAxis = capsuleShape->getUpAxis();
159
160                btScalar radius = capsuleShape->getRadius();
161                btVector3 supVec(0,0,0);
162
163                btScalar maxDot(btScalar(-1e30));
164
165                btVector3 vec = vec0;
166                btScalar lenSqr = vec.length2();
167                if (lenSqr < btScalar(0.0001))
168                {
169                        vec.setValue(1,0,0);
170                } else
171                {
172                        btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
173                        vec *= rlen;
174                }
175                btVector3 vtx;
176                btScalar newDot;
177                {
178                        btVector3 pos(0,0,0);
179                        pos[capsuleUpAxis] = halfHeight;
180
181                        //vtx = pos +vec*(radius);
182                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
183                        newDot = vec.dot(vtx);
184                       
185
186                        if (newDot > maxDot)
187                        {
188                                maxDot = newDot;
189                                supVec = vtx;
190                        }
191                }
192                {
193                        btVector3 pos(0,0,0);
194                        pos[capsuleUpAxis] = -halfHeight;
195
196                        //vtx = pos +vec*(radius);
197                        vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
198                        newDot = vec.dot(vtx);
199                        if (newDot > maxDot)
200                        {
201                                maxDot = newDot;
202                                supVec = vtx;
203                        }
204                }
205                return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());   
206        }
207        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
208        {
209                btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
210                btVector3* points = convexPointCloudShape->getUnscaledPoints ();
211                int numPoints = convexPointCloudShape->getNumPoints ();
212                return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
213        }
214        case CONVEX_HULL_SHAPE_PROXYTYPE:
215        {
216                btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
217                btVector3* points = convexHullShape->getUnscaledPoints();
218                int numPoints = convexHullShape->getNumPoints ();
219                return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
220        }
221    default:
222#ifndef __SPU__
223                return this->localGetSupportingVertexWithoutMargin (localDir);
224#else
225                btAssert (0);
226#endif
227        }
228
229        // should never reach here
230        btAssert (0);
231        return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
232}
233
234btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
235{
236        btVector3 localDirNorm = localDir;
237        if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
238        {
239                localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
240        }
241        localDirNorm.normalize ();
242
243        return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
244}
245
246/* TODO: This should be bumped up to btCollisionShape () */
247btScalar btConvexShape::getMarginNonVirtual () const
248{
249        switch (m_shapeType)
250        {
251    case SPHERE_SHAPE_PROXYTYPE:
252        {
253                btSphereShape* sphereShape = (btSphereShape*)this;
254                return sphereShape->getRadius ();
255        }
256        case BOX_SHAPE_PROXYTYPE:
257        {
258                btBoxShape* convexShape = (btBoxShape*)this;
259                return convexShape->getMarginNV ();
260        }
261        case TRIANGLE_SHAPE_PROXYTYPE:
262        {
263                btTriangleShape* triangleShape = (btTriangleShape*)this;
264                return triangleShape->getMarginNV ();
265        }
266        case CYLINDER_SHAPE_PROXYTYPE:
267        {
268                btCylinderShape* cylShape = (btCylinderShape*)this;
269                return cylShape->getMarginNV();
270        }
271        case CAPSULE_SHAPE_PROXYTYPE:
272        {
273                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
274                return capsuleShape->getMarginNV();
275        }
276        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
277        /* fall through */
278        case CONVEX_HULL_SHAPE_PROXYTYPE:
279        {
280                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
281                return convexHullShape->getMarginNV();
282        }
283    default:
284#ifndef __SPU__
285                return this->getMargin ();
286#else
287                btAssert (0);
288#endif
289        }
290
291        // should never reach here
292        btAssert (0);
293        return btScalar(0.0f);
294}
295
296void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
297{
298        switch (m_shapeType)
299        {
300    case SPHERE_SHAPE_PROXYTYPE:
301        {
302                btSphereShape* sphereShape = (btSphereShape*)this;
303                btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
304                btScalar margin = radius + sphereShape->getMarginNonVirtual();
305                const btVector3& center = t.getOrigin();
306                btVector3 extent(margin,margin,margin);
307                aabbMin = center - extent;
308                aabbMax = center + extent;
309    }
310        break;
311        case CYLINDER_SHAPE_PROXYTYPE:
312        /* fall through */
313        case BOX_SHAPE_PROXYTYPE:
314        {
315                btBoxShape* convexShape = (btBoxShape*)this;
316                btScalar margin=convexShape->getMarginNonVirtual();
317                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
318                halfExtents += btVector3(margin,margin,margin);
319                btMatrix3x3 abs_b = t.getBasis().absolute(); 
320                btVector3 center = t.getOrigin();
321                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
322               
323                aabbMin = center - extent;
324                aabbMax = center + extent;
325                break;
326        }
327        case TRIANGLE_SHAPE_PROXYTYPE:
328        {
329                btTriangleShape* triangleShape = (btTriangleShape*)this;
330                btScalar margin = triangleShape->getMarginNonVirtual();
331                for (int i=0;i<3;i++)
332                {
333                        btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
334                        vec[i] = btScalar(1.);
335
336                        btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
337
338                        btVector3 tmp = t(sv);
339                        aabbMax[i] = tmp[i]+margin;
340                        vec[i] = btScalar(-1.);
341                        tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
342                        aabbMin[i] = tmp[i]-margin;
343                }       
344        }
345        break;
346        case CAPSULE_SHAPE_PROXYTYPE:
347        {
348                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
349                btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
350                int m_upAxis = capsuleShape->getUpAxis();
351                halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
352                halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
353                btMatrix3x3 abs_b = t.getBasis().absolute(); 
354                btVector3 center = t.getOrigin();
355                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));                   
356                aabbMin = center - extent;
357                aabbMax = center + extent;
358        }
359        break;
360        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
361        case CONVEX_HULL_SHAPE_PROXYTYPE:
362        {
363                btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
364                btScalar margin = convexHullShape->getMarginNonVirtual();
365                convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
366        }
367        break;
368    default:
369#ifndef __SPU__
370                this->getAabb (t, aabbMin, aabbMax);
371#else
372                btAssert (0);
373#endif
374        break;
375        }
376
377        // should never reach here
378        btAssert (0);
379}
Note: See TracBrowser for help on using the repository browser.