1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 #if defined (_WIN32) || defined (__i386__) 17 #define BT_USE_SSE_IN_API 18 #endif 19 20 #include "btConvexShape.h" 21 #include "btTriangleShape.h" 22 #include "btSphereShape.h" 23 #include "btCylinderShape.h" 24 #include "btConeShape.h" 25 #include "btCapsuleShape.h" 26 #include "btConvexHullShape.h" 27 #include "btConvexPointCloudShape.h" 28 29 ///not supported on IBM SDK, until we fix the alignment of btVector3 30 #if defined (__CELLOS_LV2__) && defined (__SPU__) 31 #include <spu_intrinsics.h> 32 static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) 33 { 34 vec_float4 result; 35 result = spu_mul( vec0, vec1 ); 36 result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); 37 return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); 38 } 39 #endif //__SPU__ 40 41 btConvexShape::btConvexShape () 42 { 43 } 44 45 btConvexShape::~btConvexShape() 46 { 47 48 } 49 50 51 void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const 52 { 53 btVector3 localAxis = dir*trans.getBasis(); 54 btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); 55 btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); 56 57 min = vtx1.dot(dir); 58 max = vtx2.dot(dir); 59 witnesPtMax = vtx2; 60 witnesPtMin = vtx1; 61 62 if(min>max) 63 { 64 btScalar tmp = min; 65 min = max; 66 max = tmp; 67 witnesPtMax = vtx1; 68 witnesPtMin = vtx2; 69 } 70 } 71 72 73 static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) 74 { 75 76 btVector3 vec = localDirOrg * localScaling; 77 78 #if defined (__CELLOS_LV2__) && defined (__SPU__) 79 80 btVector3 localDir = vec; 81 82 vec_float4 v_distMax = {-FLT_MAX,0,0,0}; 83 vec_int4 v_idxMax = {-999,0,0,0}; 84 int v=0; 85 int numverts = numPoints; 86 87 for(;v<(int)numverts-4;v+=4) { 88 vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); 89 vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); 90 vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); 91 vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); 92 const vec_int4 i0 = {v ,0,0,0}; 93 const vec_int4 i1 = {v+1,0,0,0}; 94 const vec_int4 i2 = {v+2,0,0,0}; 95 const vec_int4 i3 = {v+3,0,0,0}; 96 vec_uint4 retGt01 = spu_cmpgt(p0,p1); 97 vec_float4 pmax01 = spu_sel(p1,p0,retGt01); 98 vec_int4 imax01 = spu_sel(i1,i0,retGt01); 99 vec_uint4 retGt23 = spu_cmpgt(p2,p3); 100 vec_float4 pmax23 = spu_sel(p3,p2,retGt23); 101 vec_int4 imax23 = spu_sel(i3,i2,retGt23); 102 vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); 103 vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); 104 vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); 105 vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); 106 v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); 107 v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); 108 } 109 for(;v<(int)numverts;v++) { 110 vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); 111 const vec_int4 i = {v,0,0,0}; 112 vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); 113 v_distMax = spu_sel(p,v_distMax,retGtMax); 114 v_idxMax = spu_sel(i,v_idxMax,retGtMax); 115 } 116 int ptIndex = spu_extract(v_idxMax,0); 117 const btVector3& supVec= points[ptIndex] * localScaling; 118 return supVec; 119 #else 120 121 btScalar maxDot; 122 long ptIndex = vec.maxDot( points, numPoints, maxDot); 123 btAssert(ptIndex >= 0); 124 btVector3 supVec = points[ptIndex] * localScaling; 125 return supVec; 126 #endif //__SPU__ 127 } 128 129 btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const 130 { 131 switch (m_shapeType) 132 { 133 case SPHERE_SHAPE_PROXYTYPE: 134 { 135 return btVector3(0,0,0); 136 } 137 case BOX_SHAPE_PROXYTYPE: 138 { 139 btBoxShape* convexShape = (btBoxShape*)this; 140 const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); 141 142 #if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON )) 143 #if defined( BT_USE_SSE ) 144 return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 )); 145 #elif defined( BT_USE_NEON ) 146 return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 )); 147 #else 148 #error unknown vector arch 149 #endif 150 #else 151 return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), 152 btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), 153 btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); 154 #endif 155 } 156 case TRIANGLE_SHAPE_PROXYTYPE: 157 { 158 btTriangleShape* triangleShape = (btTriangleShape*)this; 159 btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); 160 btVector3* vertices = &triangleShape->m_vertices1[0]; 161 btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); 162 btVector3 sup = vertices[dots.maxAxis()]; 163 return btVector3(sup.getX(),sup.getY(),sup.getZ()); 164 } 165 case CYLINDER_SHAPE_PROXYTYPE: 166 { 167 btCylinderShape* cylShape = (btCylinderShape*)this; 168 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) 169 170 btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); 171 btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); 172 int cylinderUpAxis = cylShape->getUpAxis(); 173 int XX(1),YY(0),ZZ(2); 174 175 switch (cylinderUpAxis) 176 { 177 case 0: 178 { 179 XX = 1; 180 YY = 0; 181 ZZ = 2; 182 } 183 break; 184 case 1: 185 { 186 XX = 0; 187 YY = 1; 188 ZZ = 2; 189 } 190 break; 191 case 2: 192 { 193 XX = 0; 194 YY = 2; 195 ZZ = 1; 196 197 } 198 break; 199 default: 200 btAssert(0); 201 break; 202 }; 203 204 btScalar radius = halfExtents[XX]; 205 btScalar halfHeight = halfExtents[cylinderUpAxis]; 206 207 btVector3 tmp; 208 btScalar d ; 209 210 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); 211 if (s != btScalar(0.0)) 212 { 213 d = radius / s; 214 tmp[XX] = v[XX] * d; 215 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; 216 tmp[ZZ] = v[ZZ] * d; 217 return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); 218 } else { 219 tmp[XX] = radius; 220 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; 221 tmp[ZZ] = btScalar(0.0); 222 return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); 223 } 224 } 225 case CAPSULE_SHAPE_PROXYTYPE: 226 { 227 btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); 228 229 btCapsuleShape* capsuleShape = (btCapsuleShape*)this; 230 btScalar halfHeight = capsuleShape->getHalfHeight(); 231 int capsuleUpAxis = capsuleShape->getUpAxis(); 232 233 btScalar radius = capsuleShape->getRadius(); 234 btVector3 supVec(0,0,0); 235 236 btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); 237 238 btVector3 vec = vec0; 239 btScalar lenSqr = vec.length2(); 240 if (lenSqr < btScalar(0.0001)) 241 { 242 vec.setValue(1,0,0); 243 } else 244 { 245 btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); 246 vec *= rlen; 247 } 248 btVector3 vtx; 249 btScalar newDot; 250 { 251 btVector3 pos(0,0,0); 252 pos[capsuleUpAxis] = halfHeight; 253 254 //vtx = pos +vec*(radius); 255 vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); 256 newDot = vec.dot(vtx); 257 258 259 if (newDot > maxDot) 260 { 261 maxDot = newDot; 262 supVec = vtx; 263 } 264 } 265 { 266 btVector3 pos(0,0,0); 267 pos[capsuleUpAxis] = -halfHeight; 268 269 //vtx = pos +vec*(radius); 270 vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); 271 newDot = vec.dot(vtx); 272 if (newDot > maxDot) 273 { 274 maxDot = newDot; 275 supVec = vtx; 276 } 277 } 278 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); 279 } 280 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: 281 { 282 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; 283 btVector3* points = convexPointCloudShape->getUnscaledPoints (); 284 int numPoints = convexPointCloudShape->getNumPoints (); 285 return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); 286 } 287 case CONVEX_HULL_SHAPE_PROXYTYPE: 288 { 289 btConvexHullShape* convexHullShape = (btConvexHullShape*)this; 290 btVector3* points = convexHullShape->getUnscaledPoints(); 291 int numPoints = convexHullShape->getNumPoints (); 292 return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); 293 } 294 default: 295 #ifndef __SPU__ 296 return this->localGetSupportingVertexWithoutMargin (localDir); 297 #else 298 btAssert (0); 299 #endif 300 } 301 302 // should never reach here 303 btAssert (0); 304 return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); 305 } 306 307 btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const 308 { 309 btVector3 localDirNorm = localDir; 310 if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) 311 { 312 localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); 313 } 314 localDirNorm.normalize (); 315 316 return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; 317 } 318 319 /* TODO: This should be bumped up to btCollisionShape () */ 320 btScalar btConvexShape::getMarginNonVirtual () const 321 { 322 switch (m_shapeType) 323 { 324 case SPHERE_SHAPE_PROXYTYPE: 325 { 326 btSphereShape* sphereShape = (btSphereShape*)this; 327 return sphereShape->getRadius (); 328 } 329 case BOX_SHAPE_PROXYTYPE: 330 { 331 btBoxShape* convexShape = (btBoxShape*)this; 332 return convexShape->getMarginNV (); 333 } 334 case TRIANGLE_SHAPE_PROXYTYPE: 335 { 336 btTriangleShape* triangleShape = (btTriangleShape*)this; 337 return triangleShape->getMarginNV (); 338 } 339 case CYLINDER_SHAPE_PROXYTYPE: 340 { 341 btCylinderShape* cylShape = (btCylinderShape*)this; 342 return cylShape->getMarginNV(); 343 } 344 case CONE_SHAPE_PROXYTYPE: 345 { 346 btConeShape* conShape = (btConeShape*)this; 347 return conShape->getMarginNV(); 348 } 349 case CAPSULE_SHAPE_PROXYTYPE: 350 { 351 btCapsuleShape* capsuleShape = (btCapsuleShape*)this; 352 return capsuleShape->getMarginNV(); 353 } 354 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: 355 /* fall through */ 356 case CONVEX_HULL_SHAPE_PROXYTYPE: 357 { 358 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; 359 return convexHullShape->getMarginNV(); 360 } 361 default: 362 #ifndef __SPU__ 363 return this->getMargin (); 364 #else 365 btAssert (0); 366 #endif 367 } 368 369 // should never reach here 370 btAssert (0); 371 return btScalar(0.0f); 372 } 373 #ifndef __SPU__ 374 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const 375 { 376 switch (m_shapeType) 377 { 378 case SPHERE_SHAPE_PROXYTYPE: 379 { 380 btSphereShape* sphereShape = (btSphereShape*)this; 381 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); 382 btScalar margin = radius + sphereShape->getMarginNonVirtual(); 383 const btVector3& center = t.getOrigin(); 384 btVector3 extent(margin,margin,margin); 385 aabbMin = center - extent; 386 aabbMax = center + extent; 387 } 388 break; 389 case CYLINDER_SHAPE_PROXYTYPE: 390 /* fall through */ 391 case BOX_SHAPE_PROXYTYPE: 392 { 393 btBoxShape* convexShape = (btBoxShape*)this; 394 btScalar margin=convexShape->getMarginNonVirtual(); 395 btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); 396 halfExtents += btVector3(margin,margin,margin); 397 btMatrix3x3 abs_b = t.getBasis().absolute(); 398 btVector3 center = t.getOrigin(); 399 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); 400 401 aabbMin = center - extent; 402 aabbMax = center + extent; 403 break; 404 } 405 case TRIANGLE_SHAPE_PROXYTYPE: 406 { 407 btTriangleShape* triangleShape = (btTriangleShape*)this; 408 btScalar margin = triangleShape->getMarginNonVirtual(); 409 for (int i=0;i<3;i++) 410 { 411 btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); 412 vec[i] = btScalar(1.); 413 414 btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); 415 416 btVector3 tmp = t(sv); 417 aabbMax[i] = tmp[i]+margin; 418 vec[i] = btScalar(-1.); 419 tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); 420 aabbMin[i] = tmp[i]-margin; 421 } 422 } 423 break; 424 case CAPSULE_SHAPE_PROXYTYPE: 425 { 426 btCapsuleShape* capsuleShape = (btCapsuleShape*)this; 427 btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); 428 int m_upAxis = capsuleShape->getUpAxis(); 429 halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); 430 halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); 431 btMatrix3x3 abs_b = t.getBasis().absolute(); 432 btVector3 center = t.getOrigin(); 433 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); 434 aabbMin = center - extent; 435 aabbMax = center + extent; 436 } 437 break; 438 case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: 439 case CONVEX_HULL_SHAPE_PROXYTYPE: 440 { 441 btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; 442 btScalar margin = convexHullShape->getMarginNonVirtual(); 443 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); 444 } 445 break; 446 default: 447 #ifndef __SPU__ 448 this->getAabb (t, aabbMin, aabbMax); 449 #else 450 btAssert (0); 451 #endif 452 break; 453 } 454 455 // should never reach here 456 btAssert (0); 457 } 458 459 #endif //__SPU__ 460
