btConvexShape.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btConvexShape.h"
00017 #include "btTriangleShape.h"
00018 #include "btSphereShape.h"
00019 #include "btCylinderShape.h"
00020 #include "btCapsuleShape.h"
00021 #include "btConvexHullShape.h"
00022 #include "btConvexPointCloudShape.h"
00023 
00025 #if defined (__CELLOS_LV2__) && defined (__SPU__)
00026 #include <spu_intrinsics.h>
00027 static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
00028 {
00029     vec_float4 result;
00030     result = spu_mul( vec0, vec1 );
00031     result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
00032     return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
00033 }
00034 #endif //__SPU__
00035 
00036 btConvexShape::btConvexShape ()
00037 {
00038 }
00039 
00040 btConvexShape::~btConvexShape()
00041 {
00042 
00043 }
00044 
00045 
00046 void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
00047 {
00048         btVector3 localAxis = dir*trans.getBasis();
00049         btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
00050         btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
00051 
00052         min = vtx1.dot(dir);
00053         max = vtx2.dot(dir);
00054 
00055         if(min>max)
00056         {
00057                 btScalar tmp = min;
00058                 min = max;
00059                 max = tmp;
00060         }
00061 }
00062 
00063 
00064 static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
00065 {       
00066 
00067         btVector3 vec = localDirOrg * localScaling;
00068 
00069 #if defined (__CELLOS_LV2__) && defined (__SPU__)
00070 
00071         btVector3 localDir = vec;
00072 
00073         vec_float4 v_distMax = {-FLT_MAX,0,0,0};
00074         vec_int4 v_idxMax = {-999,0,0,0};
00075         int v=0;
00076         int numverts = numPoints;
00077 
00078         for(;v<(int)numverts-4;v+=4) {
00079                 vec_float4 p0 = vec_dot3(points[v  ].get128(),localDir.get128());
00080                 vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
00081                 vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
00082                 vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
00083                 const vec_int4 i0 = {v  ,0,0,0};
00084                 const vec_int4 i1 = {v+1,0,0,0};
00085                 const vec_int4 i2 = {v+2,0,0,0};
00086                 const vec_int4 i3 = {v+3,0,0,0};
00087                 vec_uint4  retGt01 = spu_cmpgt(p0,p1);
00088                 vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
00089                 vec_int4   imax01 = spu_sel(i1,i0,retGt01);
00090                 vec_uint4  retGt23 = spu_cmpgt(p2,p3);
00091                 vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
00092                 vec_int4   imax23 = spu_sel(i3,i2,retGt23);
00093                 vec_uint4  retGt0123 = spu_cmpgt(pmax01,pmax23);
00094                 vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
00095                 vec_int4   imax0123 = spu_sel(imax23,imax01,retGt0123);
00096                 vec_uint4  retGtMax = spu_cmpgt(v_distMax,pmax0123);
00097                 v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
00098                 v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
00099         }
00100         for(;v<(int)numverts;v++) {
00101                 vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
00102                 const vec_int4 i = {v,0,0,0};
00103                 vec_uint4  retGtMax = spu_cmpgt(v_distMax,p);
00104                 v_distMax = spu_sel(p,v_distMax,retGtMax);
00105                 v_idxMax = spu_sel(i,v_idxMax,retGtMax);
00106         }
00107         int ptIndex = spu_extract(v_idxMax,0);
00108         const btVector3& supVec= points[ptIndex] * localScaling;
00109         return supVec;
00110 #else
00111 
00112     btScalar maxDot;
00113     long ptIndex = vec.maxDot( points, numPoints, maxDot);
00114         btAssert(ptIndex >= 0);
00115         btVector3 supVec = points[ptIndex] * localScaling;
00116         return supVec;
00117 #endif //__SPU__
00118 }
00119 
00120 btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
00121 {
00122         switch (m_shapeType)
00123         {
00124     case SPHERE_SHAPE_PROXYTYPE:
00125         {
00126                 return btVector3(0,0,0);
00127     }
00128         case BOX_SHAPE_PROXYTYPE:
00129         {
00130                 btBoxShape* convexShape = (btBoxShape*)this;
00131                 const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
00132 
00133 #if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON ))
00134     #if defined( BT_USE_SSE )
00135             return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 ));
00136     #elif defined( BT_USE_NEON )
00137             return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 ));
00138     #else
00139         #error unknown vector arch
00140     #endif
00141 #else
00142                 return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
00143                         btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
00144                         btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
00145 #endif
00146         }
00147         case TRIANGLE_SHAPE_PROXYTYPE:
00148         {
00149                 btTriangleShape* triangleShape = (btTriangleShape*)this;
00150                 btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
00151                 btVector3* vertices = &triangleShape->m_vertices1[0];
00152         btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
00153                 btVector3 sup = vertices[dots.maxAxis()];
00154                 return btVector3(sup.getX(),sup.getY(),sup.getZ());
00155         }
00156         case CYLINDER_SHAPE_PROXYTYPE:
00157         {
00158                 btCylinderShape* cylShape = (btCylinderShape*)this;
00159                 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
00160 
00161                 btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
00162                 btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
00163                 int cylinderUpAxis = cylShape->getUpAxis();
00164                 int XX(1),YY(0),ZZ(2);
00165 
00166                 switch (cylinderUpAxis)
00167                 {
00168                 case 0:
00169                 {
00170                         XX = 1;
00171                         YY = 0;
00172                         ZZ = 2;
00173                 }
00174                 break;
00175                 case 1:
00176                 {
00177                         XX = 0;
00178                         YY = 1;
00179                         ZZ = 2; 
00180                 }
00181                 break;
00182                 case 2:
00183                 {
00184                         XX = 0;
00185                         YY = 2;
00186                         ZZ = 1;
00187                         
00188                 }
00189                 break;
00190                 default:
00191                         btAssert(0);
00192                 break;
00193                 };
00194 
00195                 btScalar radius = halfExtents[XX];
00196                 btScalar halfHeight = halfExtents[cylinderUpAxis];
00197 
00198                 btVector3 tmp;
00199                 btScalar d ;
00200 
00201                 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
00202                 if (s != btScalar(0.0))
00203                 {
00204                         d = radius / s;  
00205                         tmp[XX] = v[XX] * d;
00206                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00207                         tmp[ZZ] = v[ZZ] * d;
00208                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
00209                 } else {
00210                         tmp[XX] = radius;
00211                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00212                         tmp[ZZ] = btScalar(0.0);
00213                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
00214                 }
00215         }
00216         case CAPSULE_SHAPE_PROXYTYPE:
00217         {
00218                 btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
00219 
00220                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
00221                 btScalar halfHeight = capsuleShape->getHalfHeight();
00222                 int capsuleUpAxis = capsuleShape->getUpAxis();
00223 
00224                 btScalar radius = capsuleShape->getRadius();
00225                 btVector3 supVec(0,0,0);
00226 
00227                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00228 
00229                 btVector3 vec = vec0;
00230                 btScalar lenSqr = vec.length2();
00231                 if (lenSqr < btScalar(0.0001))
00232                 {
00233                         vec.setValue(1,0,0);
00234                 } else
00235                 {
00236                         btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00237                         vec *= rlen;
00238                 }
00239                 btVector3 vtx;
00240                 btScalar newDot;
00241                 {
00242                         btVector3 pos(0,0,0);
00243                         pos[capsuleUpAxis] = halfHeight;
00244 
00245                         //vtx = pos +vec*(radius);
00246                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
00247                         newDot = vec.dot(vtx);
00248                         
00249 
00250                         if (newDot > maxDot)
00251                         {
00252                                 maxDot = newDot;
00253                                 supVec = vtx;
00254                         }
00255                 }
00256                 {
00257                         btVector3 pos(0,0,0);
00258                         pos[capsuleUpAxis] = -halfHeight;
00259 
00260                         //vtx = pos +vec*(radius);
00261                         vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
00262                         newDot = vec.dot(vtx);
00263                         if (newDot > maxDot)
00264                         {
00265                                 maxDot = newDot;
00266                                 supVec = vtx;
00267                         }
00268                 }
00269                 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());    
00270         }
00271         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
00272         {
00273                 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
00274                 btVector3* points = convexPointCloudShape->getUnscaledPoints ();
00275                 int numPoints = convexPointCloudShape->getNumPoints ();
00276                 return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
00277         }
00278         case CONVEX_HULL_SHAPE_PROXYTYPE:
00279         {
00280                 btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
00281                 btVector3* points = convexHullShape->getUnscaledPoints();
00282                 int numPoints = convexHullShape->getNumPoints ();
00283                 return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
00284         }
00285     default:
00286 #ifndef __SPU__
00287                 return this->localGetSupportingVertexWithoutMargin (localDir);
00288 #else
00289                 btAssert (0);
00290 #endif
00291         }
00292 
00293         // should never reach here
00294         btAssert (0);
00295         return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
00296 }
00297 
00298 btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
00299 {
00300         btVector3 localDirNorm = localDir;
00301         if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
00302         {
00303                 localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
00304         }
00305         localDirNorm.normalize ();
00306 
00307         return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
00308 }
00309 
00310 /* TODO: This should be bumped up to btCollisionShape () */
00311 btScalar btConvexShape::getMarginNonVirtual () const
00312 {
00313         switch (m_shapeType)
00314         {
00315     case SPHERE_SHAPE_PROXYTYPE:
00316         {
00317                 btSphereShape* sphereShape = (btSphereShape*)this;
00318                 return sphereShape->getRadius ();
00319         }
00320         case BOX_SHAPE_PROXYTYPE:
00321         {
00322                 btBoxShape* convexShape = (btBoxShape*)this;
00323                 return convexShape->getMarginNV ();
00324         }
00325         case TRIANGLE_SHAPE_PROXYTYPE:
00326         {
00327                 btTriangleShape* triangleShape = (btTriangleShape*)this;
00328                 return triangleShape->getMarginNV ();
00329         }
00330         case CYLINDER_SHAPE_PROXYTYPE:
00331         {
00332                 btCylinderShape* cylShape = (btCylinderShape*)this;
00333                 return cylShape->getMarginNV();
00334         }
00335         case CAPSULE_SHAPE_PROXYTYPE:
00336         {
00337                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
00338                 return capsuleShape->getMarginNV();
00339         }
00340         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
00341         /* fall through */
00342         case CONVEX_HULL_SHAPE_PROXYTYPE:
00343         {
00344                 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
00345                 return convexHullShape->getMarginNV();
00346         }
00347     default:
00348 #ifndef __SPU__
00349                 return this->getMargin ();
00350 #else
00351                 btAssert (0);
00352 #endif
00353         }
00354 
00355         // should never reach here
00356         btAssert (0);
00357         return btScalar(0.0f);
00358 }
00359 #ifndef __SPU__
00360 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
00361 {
00362         switch (m_shapeType)
00363         {
00364     case SPHERE_SHAPE_PROXYTYPE:
00365         {
00366                 btSphereShape* sphereShape = (btSphereShape*)this;
00367                 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
00368                 btScalar margin = radius + sphereShape->getMarginNonVirtual();
00369                 const btVector3& center = t.getOrigin();
00370                 btVector3 extent(margin,margin,margin);
00371                 aabbMin = center - extent;
00372                 aabbMax = center + extent;
00373     }
00374         break;
00375         case CYLINDER_SHAPE_PROXYTYPE:
00376         /* fall through */
00377         case BOX_SHAPE_PROXYTYPE:
00378         {
00379                 btBoxShape* convexShape = (btBoxShape*)this;
00380                 btScalar margin=convexShape->getMarginNonVirtual();
00381                 btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
00382                 halfExtents += btVector3(margin,margin,margin);
00383                 btMatrix3x3 abs_b = t.getBasis().absolute();  
00384                 btVector3 center = t.getOrigin();
00385         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
00386         
00387                 aabbMin = center - extent;
00388                 aabbMax = center + extent;
00389                 break;
00390         }
00391         case TRIANGLE_SHAPE_PROXYTYPE:
00392         {
00393                 btTriangleShape* triangleShape = (btTriangleShape*)this;
00394                 btScalar margin = triangleShape->getMarginNonVirtual();
00395                 for (int i=0;i<3;i++)
00396                 {
00397                         btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
00398                         vec[i] = btScalar(1.);
00399 
00400                         btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
00401 
00402                         btVector3 tmp = t(sv);
00403                         aabbMax[i] = tmp[i]+margin;
00404                         vec[i] = btScalar(-1.);
00405                         tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
00406                         aabbMin[i] = tmp[i]-margin;
00407                 }       
00408         }
00409         break;
00410         case CAPSULE_SHAPE_PROXYTYPE:
00411         {
00412                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
00413                 btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
00414                 int m_upAxis = capsuleShape->getUpAxis();
00415                 halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
00416                 halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
00417                 btMatrix3x3 abs_b = t.getBasis().absolute();  
00418                 btVector3 center = t.getOrigin();
00419         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
00420                 aabbMin = center - extent;
00421                 aabbMax = center + extent;
00422         }
00423         break;
00424         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
00425         case CONVEX_HULL_SHAPE_PROXYTYPE:
00426         {
00427                 btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
00428                 btScalar margin = convexHullShape->getMarginNonVirtual();
00429                 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
00430         }
00431         break;
00432     default:
00433 #ifndef __SPU__
00434                 this->getAabb (t, aabbMin, aabbMax);
00435 #else
00436                 btAssert (0);
00437 #endif
00438         break;
00439         }
00440 
00441         // should never reach here
00442         btAssert (0);
00443 }
00444 
00445 #endif //__SPU__