00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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
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
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
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
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
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();
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
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
00442 btAssert (0);
00443 }
00444
00445 #endif //__SPU__