| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "BulletCollision/CollisionShapes/btConvexShape.h" | |
| #include "BulletCollision/CollisionShapes/btTriangleShape.h" | |
| #include "BulletCollision/CollisionShapes/btSphereShape.h" | |
| #include "BulletCollision/CollisionShapes/btCylinderShape.h" | |
| #include "BulletCollision/CollisionShapes/btCapsuleShape.h" | |
| #include "BulletCollision/CollisionShapes/btConvexHullShape.h" | |
| #include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h" | |
| ///not supported on IBM SDK, until we fix the alignment of btVector3 | |
| #if defined (__CELLOS_LV2__) && defined (__SPU__) | |
| #include <spu_intrinsics.h> | |
| static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) | |
| { | |
| vec_float4 result; | |
| result = spu_mul( vec0, vec1 ); | |
| result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); | |
| return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); | |
| } | |
| #endif //__SPU__ | |
| btConvexShape::btConvexShape () | |
| { | |
| } | |
| btConvexShape::~btConvexShape() | |
| { | |
| } | |
| static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) | |
| { | |
| btVector3 vec = localDirOrg * localScaling; | |
| #if defined (__CELLOS_LV2__) && defined (__SPU__) | |
| btVector3 localDir = vec; | |
| vec_float4 v_distMax = {-FLT_MAX,0,0,0}; | |
| vec_int4 v_idxMax = {-999,0,0,0}; | |
| int v=0; | |
| int numverts = numPoints; | |
| for(;v<(int)numverts-4;v+=4) { | |
| vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); | |
| vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); | |
| vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); | |
| vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); | |
| const vec_int4 i0 = {v ,0,0,0}; | |
| const vec_int4 i1 = {v+1,0,0,0}; | |
| const vec_int4 i2 = {v+2,0,0,0}; | |
| const vec_int4 i3 = {v+3,0,0,0}; | |
| vec_uint4 retGt01 = spu_cmpgt(p0,p1); | |
| vec_float4 pmax01 = spu_sel(p1,p0,retGt01); | |
| vec_int4 imax01 = spu_sel(i1,i0,retGt01); | |
| vec_uint4 retGt23 = spu_cmpgt(p2,p3); | |
| vec_float4 pmax23 = spu_sel(p3,p2,retGt23); | |
| vec_int4 imax23 = spu_sel(i3,i2,retGt23); | |
| vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); | |
| vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); | |
| vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); | |
| vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); | |
| v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); | |
| v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); | |
| } | |
| for(;v<(int)numverts;v++) { | |
| vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); | |
| const vec_int4 i = {v,0,0,0}; | |
| vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); | |
| v_distMax = spu_sel(p,v_distMax,retGtMax); | |
| v_idxMax = spu_sel(i,v_idxMax,retGtMax); | |
| } | |
| int ptIndex = spu_extract(v_idxMax,0); | |
| const btVector3& supVec= points[ptIndex] * localScaling; | |
| return supVec; | |
| #else | |
| btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); | |
| int ptIndex = -1; | |
| for (int i=0;i<numPoints;i++) | |
| { | |
| newDot = vec.dot(points[i]); | |
| if (newDot > maxDot) | |
| { | |
| maxDot = newDot; | |
| ptIndex = i; | |
| } | |
| } | |
| btAssert(ptIndex >= 0); | |
| btVector3 supVec = points[ptIndex] * localScaling; | |
| return supVec; | |
| #endif //__SPU__ | |
| } | |
| btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const | |
| { | |
| switch (m_shapeType) | |
| { | |
| case SPHERE_SHAPE_PROXYTYPE: | |
| { | |
| return btVector3(0,0,0); | |
| } | |
| case BOX_SHAPE_PROXYTYPE: | |
| { | |
| btBoxShape* convexShape = (btBoxShape*)this; | |
| const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); | |
| return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), | |
| btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), | |
| btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); | |
| } | |
| case TRIANGLE_SHAPE_PROXYTYPE: | |
| { | |
| btTriangleShape* triangleShape = (btTriangleShape*)this; | |
| btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); | |
| btVector3* vertices = &triangleShape->m_vertices1[0]; | |
| btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); | |
| btVector3 sup = vertices[dots.maxAxis()]; | |
| return btVector3(sup.getX(),sup.getY(),sup.getZ()); | |
| } | |
| case CYLINDER_SHAPE_PROXYTYPE: | |
| { | |
| btCylinderShape* cylShape = (btCylinderShape*)this; | |
| //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) | |
| btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); | |
| btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); | |
| int cylinderUpAxis = cylShape->getUpAxis(); | |
| int XX(1),YY(0),ZZ(2); | |
| switch (cylinderUpAxis) | |
| { | |
| case 0: | |
| { | |
| XX = 1; | |
| YY = 0; | |
| ZZ = 2; | |
| } | |
| break; | |
| case 1: | |
| { | |
| XX = 0; | |
| YY = 1; | |
| ZZ = 2; | |
| } | |
| break; | |
| case 2: | |
| { | |
| XX = 0; | |
| YY = 2; | |
| ZZ = 1; | |
| } | |
| break; | |
| default: | |
| btAssert(0); | |
| break; | |
| }; | |
| btScalar radius = halfExtents[XX]; | |
| btScalar halfHeight = halfExtents[cylinderUpAxis]; | |
| btVector3 tmp; | |
| btScalar d ; | |
| btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); | |
| if (s != btScalar(0.0)) | |
| { | |
| d = radius / s; | |
| tmp[XX] = v[XX] * d; | |
| tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; | |
| tmp[ZZ] = v[ZZ] * d; | |
| return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); | |
| } else { | |
| tmp[XX] = radius; | |
| tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; | |
| tmp[ZZ] = btScalar(0.0); | |
| return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); | |
| } | |
| } | |
| case CAPSULE_SHAPE_PROXYTYPE: | |
| { | |
| btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); | |
| btCapsuleShape* capsuleShape = (btCapsuleShape*)this; | |
| btScalar halfHeight = capsuleShape->getHalfHeight(); | |
| int capsuleUpAxis = capsuleShape->getUpAxis(); | |
| btScalar radius = capsuleShape->getRadius(); | |
| btVector3 supVec(0,0,0); | |
| btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); | |
| btVector3 vec = vec0; | |
| btScalar lenSqr = vec.length2(); | |
| if (lenSqr < btScalar(0.0001)) | |
| { | |
| vec.setValue(1,0,0); | |
| } else | |
| { | |
| btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); | |
| vec *= rlen; | |
| } | |
| btVector3 vtx; | |
| btScalar newDot; | |
| { | |
| btVector3 pos(0,0,0); | |
| pos[capsuleUpAxis] = halfHeight; | |
| //vtx = pos +vec*(radius); | |
| vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); | |
| newDot = vec.dot(vtx); | |
| if (newDot > maxDot) | |
| { | |
| maxDot = newDot; | |
| supVec = vtx; | |
| } | |
| } | |
| { | |
| btVector3 pos(0,0,0); | |
| pos[capsuleUpAxis] = -halfHeight; | |
| //vtx = pos +vec*(radius); | |
| vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); | |
| newDot = vec.dot(vtx); | |
| if (newDot > maxDot) | |
| { | |
| maxDot = newDot; | |
| supVec = vtx; | |
| } | |
| } | |
| return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); | |
| } | |
| case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: | |
| { | |
| btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; | |
| btVector3* points = convexPointCloudShape->getUnscaledPoints (); | |
| int numPoints = convexPointCloudShape->getNumPoints (); | |
| return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); | |
| } | |
| case CONVEX_HULL_SHAPE_PROXYTYPE: | |
| { | |
| btConvexHullShape* convexHullShape = (btConvexHullShape*)this; | |
| btVector3* points = convexHullShape->getUnscaledPoints(); | |
| int numPoints = convexHullShape->getNumPoints (); | |
| return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); | |
| } | |
| default: | |
| #ifndef __SPU__ | |
| return this->localGetSupportingVertexWithoutMargin (localDir); | |
| #else | |
| btAssert (0); | |
| #endif | |
| } | |
| // should never reach here | |
| btAssert (0); | |
| return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); | |
| } | |
| btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const | |
| { | |
| btVector3 localDirNorm = localDir; | |
| if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) | |
| { | |
| localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); | |
| } | |
| localDirNorm.normalize (); | |
| return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; | |
| } | |
| /* TODO: This should be bumped up to btCollisionShape () */ | |
| btScalar btConvexShape::getMarginNonVirtual () const | |
| { | |
| switch (m_shapeType) | |
| { | |
| case SPHERE_SHAPE_PROXYTYPE: | |
| { | |
| btSphereShape* sphereShape = (btSphereShape*)this; | |
| return sphereShape->getRadius (); | |
| } | |
| case BOX_SHAPE_PROXYTYPE: | |
| { | |
| btBoxShape* convexShape = (btBoxShape*)this; | |
| return convexShape->getMarginNV (); | |
| } | |
| case TRIANGLE_SHAPE_PROXYTYPE: | |
| { | |
| btTriangleShape* triangleShape = (btTriangleShape*)this; | |
| return triangleShape->getMarginNV (); | |
| } | |
| case CYLINDER_SHAPE_PROXYTYPE: | |
| { | |
| btCylinderShape* cylShape = (btCylinderShape*)this; | |
| return cylShape->getMarginNV(); | |
| } | |
| case CAPSULE_SHAPE_PROXYTYPE: | |
| { | |
| btCapsuleShape* capsuleShape = (btCapsuleShape*)this; | |
| return capsuleShape->getMarginNV(); | |
| } | |
| case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: | |
| /* fall through */ | |
| case CONVEX_HULL_SHAPE_PROXYTYPE: | |
| { | |
| btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; | |
| return convexHullShape->getMarginNV(); | |
| } | |
| default: | |
| #ifndef __SPU__ | |
| return this->getMargin (); | |
| #else | |
| btAssert (0); | |
| #endif | |
| } | |
| // should never reach here | |
| btAssert (0); | |
| return btScalar(0.0f); | |
| } | |
| #ifndef __SPU__ | |
| void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const | |
| { | |
| switch (m_shapeType) | |
| { | |
| case SPHERE_SHAPE_PROXYTYPE: | |
| { | |
| btSphereShape* sphereShape = (btSphereShape*)this; | |
| btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); | |
| btScalar margin = radius + sphereShape->getMarginNonVirtual(); | |
| const btVector3& center = t.getOrigin(); | |
| btVector3 extent(margin,margin,margin); | |
| aabbMin = center - extent; | |
| aabbMax = center + extent; | |
| } | |
| break; | |
| case CYLINDER_SHAPE_PROXYTYPE: | |
| /* fall through */ | |
| case BOX_SHAPE_PROXYTYPE: | |
| { | |
| btBoxShape* convexShape = (btBoxShape*)this; | |
| btScalar margin=convexShape->getMarginNonVirtual(); | |
| btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); | |
| halfExtents += btVector3(margin,margin,margin); | |
| btMatrix3x3 abs_b = t.getBasis().absolute(); | |
| btVector3 center = t.getOrigin(); | |
| btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); | |
| aabbMin = center - extent; | |
| aabbMax = center + extent; | |
| break; | |
| } | |
| case TRIANGLE_SHAPE_PROXYTYPE: | |
| { | |
| btTriangleShape* triangleShape = (btTriangleShape*)this; | |
| btScalar margin = triangleShape->getMarginNonVirtual(); | |
| for (int i=0;i<3;i++) | |
| { | |
| btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); | |
| vec[i] = btScalar(1.); | |
| btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); | |
| btVector3 tmp = t(sv); | |
| aabbMax[i] = tmp[i]+margin; | |
| vec[i] = btScalar(-1.); | |
| tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); | |
| aabbMin[i] = tmp[i]-margin; | |
| } | |
| } | |
| break; | |
| case CAPSULE_SHAPE_PROXYTYPE: | |
| { | |
| btCapsuleShape* capsuleShape = (btCapsuleShape*)this; | |
| btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); | |
| int m_upAxis = capsuleShape->getUpAxis(); | |
| halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); | |
| halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); | |
| btMatrix3x3 abs_b = t.getBasis().absolute(); | |
| btVector3 center = t.getOrigin(); | |
| btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); | |
| aabbMin = center - extent; | |
| aabbMax = center + extent; | |
| } | |
| break; | |
| case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: | |
| case CONVEX_HULL_SHAPE_PROXYTYPE: | |
| { | |
| btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; | |
| btScalar margin = convexHullShape->getMarginNonVirtual(); | |
| convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); | |
| } | |
| break; | |
| default: | |
| #ifndef __SPU__ | |
| this->getAabb (t, aabbMin, aabbMax); | |
| #else | |
| btAssert (0); | |
| #endif | |
| break; | |
| } | |
| // should never reach here | |
| btAssert (0); | |
| } | |
| #endif //__SPU__ |