[1963] | 1 | /* |
---|
| 2 | Bullet Continuous Collision Detection and Physics Library |
---|
| 3 | Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
---|
| 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 | #include "btConvexShape.h" |
---|
| 17 | #include "btConvexInternalShape.h" |
---|
| 18 | #include "btTriangleShape.h" |
---|
| 19 | #include "btSphereShape.h" |
---|
| 20 | #include "btCylinderShape.h" |
---|
| 21 | #include "btCapsuleShape.h" |
---|
| 22 | #include "btConvexHullShape.h" |
---|
| 23 | #include "btConvexPointCloudShape.h" |
---|
| 24 | |
---|
| 25 | static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints) |
---|
| 26 | { |
---|
| 27 | btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); |
---|
| 28 | btScalar newDot,maxDot = btScalar(-1e30); |
---|
| 29 | |
---|
| 30 | btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 31 | btVector3 vec = vec0; |
---|
| 32 | btScalar lenSqr = vec.length2(); |
---|
| 33 | if (lenSqr < btScalar(0.0001)) |
---|
| 34 | { |
---|
| 35 | vec.setValue(1,0,0); |
---|
| 36 | } else { |
---|
| 37 | btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); |
---|
| 38 | vec *= rlen; |
---|
| 39 | } |
---|
| 40 | |
---|
| 41 | |
---|
| 42 | for (int i=0;i<numPoints;i++) |
---|
| 43 | { |
---|
| 44 | btPoint3 vtx = points[i];// * m_localScaling; |
---|
| 45 | |
---|
| 46 | newDot = vec.dot(vtx); |
---|
| 47 | if (newDot > maxDot) |
---|
| 48 | { |
---|
| 49 | maxDot = newDot; |
---|
| 50 | supVec = vtx; |
---|
| 51 | } |
---|
| 52 | } |
---|
| 53 | return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); |
---|
| 54 | } |
---|
| 55 | |
---|
| 56 | btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const |
---|
| 57 | { |
---|
| 58 | switch (m_shapeType) |
---|
| 59 | { |
---|
| 60 | case SPHERE_SHAPE_PROXYTYPE: |
---|
| 61 | { |
---|
| 62 | return btVector3(0,0,0); |
---|
| 63 | } |
---|
| 64 | break; |
---|
| 65 | case BOX_SHAPE_PROXYTYPE: |
---|
| 66 | { |
---|
| 67 | btConvexInternalShape* convexShape = (btConvexInternalShape*)this; |
---|
| 68 | const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); |
---|
| 69 | |
---|
| 70 | return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), |
---|
| 71 | btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), |
---|
| 72 | btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); |
---|
| 73 | } |
---|
| 74 | break; |
---|
| 75 | case TRIANGLE_SHAPE_PROXYTYPE: |
---|
| 76 | { |
---|
| 77 | btTriangleShape* triangleShape = (btTriangleShape*)this; |
---|
| 78 | btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 79 | btVector3* vertices = &triangleShape->m_vertices1[0]; |
---|
| 80 | btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); |
---|
| 81 | btVector3 sup = vertices[dots.maxAxis()]; |
---|
| 82 | return btVector3(sup.getX(),sup.getY(),sup.getZ()); |
---|
| 83 | } |
---|
| 84 | break; |
---|
| 85 | case CYLINDER_SHAPE_PROXYTYPE: |
---|
| 86 | { |
---|
| 87 | btCylinderShape* cylShape = (btCylinderShape*)this; |
---|
| 88 | //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) |
---|
| 89 | |
---|
| 90 | btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); |
---|
| 91 | btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 92 | int cylinderUpAxis = cylShape->getUpAxis(); |
---|
| 93 | int XX(1),YY(0),ZZ(2); |
---|
| 94 | |
---|
| 95 | switch (cylinderUpAxis) |
---|
| 96 | { |
---|
| 97 | case 0: |
---|
| 98 | { |
---|
| 99 | XX = 1; |
---|
| 100 | YY = 0; |
---|
| 101 | ZZ = 2; |
---|
| 102 | } |
---|
| 103 | break; |
---|
| 104 | case 1: |
---|
| 105 | { |
---|
| 106 | XX = 0; |
---|
| 107 | YY = 1; |
---|
| 108 | ZZ = 2; |
---|
| 109 | } |
---|
| 110 | break; |
---|
| 111 | case 2: |
---|
| 112 | { |
---|
| 113 | XX = 0; |
---|
| 114 | YY = 2; |
---|
| 115 | ZZ = 1; |
---|
| 116 | |
---|
| 117 | } |
---|
| 118 | break; |
---|
| 119 | default: |
---|
| 120 | btAssert(0); |
---|
| 121 | break; |
---|
| 122 | }; |
---|
| 123 | |
---|
| 124 | btScalar radius = halfExtents[XX]; |
---|
| 125 | btScalar halfHeight = halfExtents[cylinderUpAxis]; |
---|
| 126 | |
---|
| 127 | btVector3 tmp; |
---|
| 128 | btScalar d ; |
---|
| 129 | |
---|
| 130 | btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); |
---|
| 131 | if (s != btScalar(0.0)) |
---|
| 132 | { |
---|
| 133 | d = radius / s; |
---|
| 134 | tmp[XX] = v[XX] * d; |
---|
| 135 | tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; |
---|
| 136 | tmp[ZZ] = v[ZZ] * d; |
---|
| 137 | return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); |
---|
| 138 | } else { |
---|
| 139 | tmp[XX] = radius; |
---|
| 140 | tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; |
---|
| 141 | tmp[ZZ] = btScalar(0.0); |
---|
| 142 | return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); |
---|
| 143 | } |
---|
| 144 | } |
---|
| 145 | break; |
---|
| 146 | case CAPSULE_SHAPE_PROXYTYPE: |
---|
| 147 | { |
---|
| 148 | btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 149 | |
---|
| 150 | btCapsuleShape* capsuleShape = (btCapsuleShape*)this; |
---|
| 151 | btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions(); |
---|
| 152 | btScalar halfHeight = capsuleShape->getHalfHeight(); |
---|
| 153 | int capsuleUpAxis = capsuleShape->getUpAxis(); |
---|
| 154 | |
---|
| 155 | btScalar radius = capsuleShape->getRadius(); |
---|
| 156 | btVector3 supVec(0,0,0); |
---|
| 157 | |
---|
| 158 | btScalar maxDot(btScalar(-1e30)); |
---|
| 159 | |
---|
| 160 | btVector3 vec = vec0; |
---|
| 161 | btScalar lenSqr = vec.length2(); |
---|
| 162 | if (lenSqr < btScalar(0.0001)) |
---|
| 163 | { |
---|
| 164 | vec.setValue(1,0,0); |
---|
| 165 | } else |
---|
| 166 | { |
---|
| 167 | btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); |
---|
| 168 | vec *= rlen; |
---|
| 169 | } |
---|
| 170 | btVector3 vtx; |
---|
| 171 | btScalar newDot; |
---|
| 172 | { |
---|
| 173 | btVector3 pos(0,0,0); |
---|
| 174 | pos[capsuleUpAxis] = halfHeight; |
---|
| 175 | |
---|
| 176 | //vtx = pos +vec*(radius); |
---|
| 177 | vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); |
---|
| 178 | newDot = vec.dot(vtx); |
---|
| 179 | |
---|
| 180 | |
---|
| 181 | if (newDot > maxDot) |
---|
| 182 | { |
---|
| 183 | maxDot = newDot; |
---|
| 184 | supVec = vtx; |
---|
| 185 | } |
---|
| 186 | } |
---|
| 187 | { |
---|
| 188 | btVector3 pos(0,0,0); |
---|
| 189 | pos[capsuleUpAxis] = -halfHeight; |
---|
| 190 | |
---|
| 191 | //vtx = pos +vec*(radius); |
---|
| 192 | vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); |
---|
| 193 | newDot = vec.dot(vtx); |
---|
| 194 | if (newDot > maxDot) |
---|
| 195 | { |
---|
| 196 | maxDot = newDot; |
---|
| 197 | supVec = vtx; |
---|
| 198 | } |
---|
| 199 | } |
---|
| 200 | return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); |
---|
| 201 | } |
---|
| 202 | break; |
---|
| 203 | case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: |
---|
| 204 | { |
---|
| 205 | btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; |
---|
| 206 | btVector3* points = convexPointCloudShape->getPoints (); |
---|
| 207 | int numPoints = convexPointCloudShape->getNumPoints (); |
---|
| 208 | return convexHullSupport (localDir, points, numPoints); |
---|
| 209 | } |
---|
| 210 | case CONVEX_HULL_SHAPE_PROXYTYPE: |
---|
| 211 | { |
---|
| 212 | btConvexHullShape* convexHullShape = (btConvexHullShape*)this; |
---|
| 213 | btPoint3* points = convexHullShape->getPoints (); |
---|
| 214 | int numPoints = convexHullShape->getNumPoints (); |
---|
| 215 | return convexHullSupport (localDir, points, numPoints); |
---|
| 216 | } |
---|
| 217 | break; |
---|
| 218 | default: |
---|
| 219 | #ifndef __SPU__ |
---|
| 220 | return this->localGetSupportingVertexWithoutMargin (localDir); |
---|
| 221 | #else |
---|
| 222 | btAssert (0); |
---|
| 223 | #endif |
---|
| 224 | break; |
---|
| 225 | } |
---|
| 226 | |
---|
| 227 | // should never reach here |
---|
| 228 | btAssert (0); |
---|
| 229 | return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); |
---|
| 230 | } |
---|
| 231 | |
---|
| 232 | btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const |
---|
| 233 | { |
---|
| 234 | btVector3 localDirNorm = localDir; |
---|
| 235 | if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) |
---|
| 236 | { |
---|
| 237 | localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); |
---|
| 238 | } |
---|
| 239 | localDirNorm.normalize (); |
---|
| 240 | switch (m_shapeType) |
---|
| 241 | { |
---|
| 242 | case SPHERE_SHAPE_PROXYTYPE: |
---|
| 243 | { |
---|
| 244 | return btVector3(0,0,0) + getMarginNonVirtual() * localDirNorm; |
---|
| 245 | } |
---|
| 246 | break; |
---|
| 247 | case BOX_SHAPE_PROXYTYPE: |
---|
| 248 | { |
---|
| 249 | btConvexInternalShape* convexShape = (btConvexInternalShape*)this; |
---|
| 250 | const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); |
---|
| 251 | |
---|
| 252 | return btVector3(localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), |
---|
| 253 | localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), |
---|
| 254 | localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()) + getMarginNonVirtual() * localDirNorm; |
---|
| 255 | } |
---|
| 256 | break; |
---|
| 257 | case TRIANGLE_SHAPE_PROXYTYPE: |
---|
| 258 | { |
---|
| 259 | btTriangleShape* triangleShape = (btTriangleShape*)this; |
---|
| 260 | btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 261 | btVector3* vertices = &triangleShape->m_vertices1[0]; |
---|
| 262 | btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); |
---|
| 263 | btVector3 sup = vertices[dots.maxAxis()]; |
---|
| 264 | return btVector3(sup.getX(),sup.getY(),sup.getZ()) + getMarginNonVirtual() * localDirNorm; |
---|
| 265 | } |
---|
| 266 | break; |
---|
| 267 | case CYLINDER_SHAPE_PROXYTYPE: |
---|
| 268 | { |
---|
| 269 | btCylinderShape* cylShape = (btCylinderShape*)this; |
---|
| 270 | //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) |
---|
| 271 | |
---|
| 272 | btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); |
---|
| 273 | btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 274 | int cylinderUpAxis = cylShape->getUpAxis(); |
---|
| 275 | int XX(1),YY(0),ZZ(2); |
---|
| 276 | |
---|
| 277 | switch (cylinderUpAxis) |
---|
| 278 | { |
---|
| 279 | case 0: |
---|
| 280 | { |
---|
| 281 | XX = 1; |
---|
| 282 | YY = 0; |
---|
| 283 | ZZ = 2; |
---|
| 284 | } |
---|
| 285 | break; |
---|
| 286 | case 1: |
---|
| 287 | { |
---|
| 288 | XX = 0; |
---|
| 289 | YY = 1; |
---|
| 290 | ZZ = 2; |
---|
| 291 | } |
---|
| 292 | break; |
---|
| 293 | case 2: |
---|
| 294 | { |
---|
| 295 | XX = 0; |
---|
| 296 | YY = 2; |
---|
| 297 | ZZ = 1; |
---|
| 298 | |
---|
| 299 | } |
---|
| 300 | break; |
---|
| 301 | default: |
---|
| 302 | btAssert(0); |
---|
| 303 | break; |
---|
| 304 | }; |
---|
| 305 | |
---|
| 306 | btScalar radius = halfExtents[XX]; |
---|
| 307 | btScalar halfHeight = halfExtents[cylinderUpAxis]; |
---|
| 308 | |
---|
| 309 | btVector3 tmp; |
---|
| 310 | btScalar d ; |
---|
| 311 | |
---|
| 312 | btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); |
---|
| 313 | if (s != btScalar(0.0)) |
---|
| 314 | { |
---|
| 315 | d = radius / s; |
---|
| 316 | tmp[XX] = v[XX] * d; |
---|
| 317 | tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; |
---|
| 318 | tmp[ZZ] = v[ZZ] * d; |
---|
| 319 | return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm; |
---|
| 320 | } else { |
---|
| 321 | tmp[XX] = radius; |
---|
| 322 | tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; |
---|
| 323 | tmp[ZZ] = btScalar(0.0); |
---|
| 324 | return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm; |
---|
| 325 | } |
---|
| 326 | } |
---|
| 327 | break; |
---|
| 328 | case CAPSULE_SHAPE_PROXYTYPE: |
---|
| 329 | { |
---|
| 330 | btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); |
---|
| 331 | |
---|
| 332 | btCapsuleShape* capsuleShape = (btCapsuleShape*)this; |
---|
| 333 | btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions(); |
---|
| 334 | btScalar halfHeight = capsuleShape->getHalfHeight(); |
---|
| 335 | int capsuleUpAxis = capsuleShape->getUpAxis(); |
---|
| 336 | |
---|
| 337 | btScalar radius = capsuleShape->getRadius(); |
---|
| 338 | btVector3 supVec(0,0,0); |
---|
| 339 | |
---|
| 340 | btScalar maxDot(btScalar(-1e30)); |
---|
| 341 | |
---|
| 342 | btVector3 vec = vec0; |
---|
| 343 | btScalar lenSqr = vec.length2(); |
---|
| 344 | if (lenSqr < btScalar(0.0001)) |
---|
| 345 | { |
---|
| 346 | vec.setValue(1,0,0); |
---|
| 347 | } else |
---|
| 348 | { |
---|
| 349 | btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); |
---|
| 350 | vec *= rlen; |
---|
| 351 | } |
---|
| 352 | btVector3 vtx; |
---|
| 353 | btScalar newDot; |
---|
| 354 | { |
---|
| 355 | btVector3 pos(0,0,0); |
---|
| 356 | pos[capsuleUpAxis] = halfHeight; |
---|
| 357 | |
---|
| 358 | vtx = pos +vec*(radius); |
---|
| 359 | newDot = vec.dot(vtx); |
---|
| 360 | if (newDot > maxDot) |
---|
| 361 | { |
---|
| 362 | maxDot = newDot; |
---|
| 363 | supVec = vtx; |
---|
| 364 | } |
---|
| 365 | } |
---|
| 366 | { |
---|
| 367 | btVector3 pos(0,0,0); |
---|
| 368 | pos[capsuleUpAxis] = -halfHeight; |
---|
| 369 | |
---|
| 370 | vtx = pos +vec*(radius); |
---|
| 371 | newDot = vec.dot(vtx); |
---|
| 372 | if (newDot > maxDot) |
---|
| 373 | { |
---|
| 374 | maxDot = newDot; |
---|
| 375 | supVec = vtx; |
---|
| 376 | } |
---|
| 377 | } |
---|
| 378 | return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()) + getMarginNonVirtual() * localDirNorm; |
---|
| 379 | } |
---|
| 380 | break; |
---|
| 381 | case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: |
---|
| 382 | { |
---|
| 383 | btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; |
---|
| 384 | btVector3* points = convexPointCloudShape->getPoints (); |
---|
| 385 | int numPoints = convexPointCloudShape->getNumPoints (); |
---|
| 386 | return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm; |
---|
| 387 | } |
---|
| 388 | case CONVEX_HULL_SHAPE_PROXYTYPE: |
---|
| 389 | { |
---|
| 390 | btConvexHullShape* convexHullShape = (btConvexHullShape*)this; |
---|
| 391 | btPoint3* points = convexHullShape->getPoints (); |
---|
| 392 | int numPoints = convexHullShape->getNumPoints (); |
---|
| 393 | return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm; |
---|
| 394 | } |
---|
| 395 | break; |
---|
| 396 | default: |
---|
| 397 | #ifndef __SPU__ |
---|
| 398 | return this->localGetSupportingVertex (localDir); |
---|
| 399 | #else |
---|
| 400 | btAssert (0); |
---|
| 401 | #endif |
---|
| 402 | break; |
---|
| 403 | } |
---|
| 404 | |
---|
| 405 | // should never reach here |
---|
| 406 | btAssert (0); |
---|
| 407 | return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); |
---|
| 408 | } |
---|
| 409 | |
---|
| 410 | /* TODO: This should be bumped up to btCollisionShape () */ |
---|
| 411 | btScalar btConvexShape::getMarginNonVirtual () const |
---|
| 412 | { |
---|
| 413 | switch (m_shapeType) |
---|
| 414 | { |
---|
| 415 | case SPHERE_SHAPE_PROXYTYPE: |
---|
| 416 | { |
---|
| 417 | btSphereShape* sphereShape = (btSphereShape*)this; |
---|
| 418 | return sphereShape->getRadius (); |
---|
| 419 | } |
---|
| 420 | break; |
---|
| 421 | case BOX_SHAPE_PROXYTYPE: |
---|
| 422 | { |
---|
| 423 | btConvexInternalShape* convexShape = (btConvexInternalShape*)this; |
---|
| 424 | return convexShape->getMarginNV (); |
---|
| 425 | } |
---|
| 426 | break; |
---|
| 427 | case TRIANGLE_SHAPE_PROXYTYPE: |
---|
| 428 | { |
---|
| 429 | btTriangleShape* triangleShape = (btTriangleShape*)this; |
---|
| 430 | return triangleShape->getMarginNV (); |
---|
| 431 | } |
---|
| 432 | break; |
---|
| 433 | case CYLINDER_SHAPE_PROXYTYPE: |
---|
| 434 | { |
---|
| 435 | btCylinderShape* cylShape = (btCylinderShape*)this; |
---|
| 436 | return cylShape->getMarginNV(); |
---|
| 437 | } |
---|
| 438 | break; |
---|
| 439 | case CAPSULE_SHAPE_PROXYTYPE: |
---|
| 440 | { |
---|
| 441 | btCapsuleShape* capsuleShape = (btCapsuleShape*)this; |
---|
| 442 | return capsuleShape->getMarginNV(); |
---|
| 443 | } |
---|
| 444 | break; |
---|
| 445 | case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: |
---|
| 446 | /* fall through */ |
---|
| 447 | case CONVEX_HULL_SHAPE_PROXYTYPE: |
---|
| 448 | { |
---|
| 449 | btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; |
---|
| 450 | return convexHullShape->getMarginNV(); |
---|
| 451 | } |
---|
| 452 | break; |
---|
| 453 | default: |
---|
| 454 | #ifndef __SPU__ |
---|
| 455 | return this->getMargin (); |
---|
| 456 | #else |
---|
| 457 | btAssert (0); |
---|
| 458 | #endif |
---|
| 459 | break; |
---|
| 460 | } |
---|
| 461 | |
---|
| 462 | // should never reach here |
---|
| 463 | btAssert (0); |
---|
| 464 | return btScalar(0.0f); |
---|
| 465 | } |
---|
| 466 | |
---|
| 467 | void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const |
---|
| 468 | { |
---|
| 469 | switch (m_shapeType) |
---|
| 470 | { |
---|
| 471 | case SPHERE_SHAPE_PROXYTYPE: |
---|
| 472 | { |
---|
| 473 | btSphereShape* sphereShape = (btSphereShape*)this; |
---|
| 474 | float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); |
---|
| 475 | float margin = radius + sphereShape->getMarginNonVirtual(); |
---|
| 476 | const btVector3& center = t.getOrigin(); |
---|
| 477 | btVector3 extent(margin,margin,margin); |
---|
| 478 | aabbMin = center - extent; |
---|
| 479 | aabbMax = center + extent; |
---|
| 480 | } |
---|
| 481 | break; |
---|
| 482 | case CYLINDER_SHAPE_PROXYTYPE: |
---|
| 483 | /* fall through */ |
---|
| 484 | case BOX_SHAPE_PROXYTYPE: |
---|
| 485 | { |
---|
| 486 | btConvexInternalShape* convexShape = (btConvexInternalShape*)this; |
---|
| 487 | float margin=convexShape->getMarginNonVirtual(); |
---|
| 488 | btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); |
---|
| 489 | halfExtents += btVector3(margin,margin,margin); |
---|
| 490 | btMatrix3x3 abs_b = t.getBasis().absolute(); |
---|
| 491 | btPoint3 center = t.getOrigin(); |
---|
| 492 | btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); |
---|
| 493 | |
---|
| 494 | aabbMin = center - extent; |
---|
| 495 | aabbMax = center + extent; |
---|
| 496 | break; |
---|
| 497 | } |
---|
| 498 | break; |
---|
| 499 | case TRIANGLE_SHAPE_PROXYTYPE: |
---|
| 500 | { |
---|
| 501 | btTriangleShape* triangleShape = (btTriangleShape*)this; |
---|
| 502 | btScalar margin = triangleShape->getMarginNonVirtual(); |
---|
| 503 | for (int i=0;i<3;i++) |
---|
| 504 | { |
---|
| 505 | btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); |
---|
| 506 | vec[i] = btScalar(1.); |
---|
| 507 | |
---|
| 508 | btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); |
---|
| 509 | |
---|
| 510 | btVector3 tmp = t(sv); |
---|
| 511 | aabbMax[i] = tmp[i]+margin; |
---|
| 512 | vec[i] = btScalar(-1.); |
---|
| 513 | tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); |
---|
| 514 | aabbMin[i] = tmp[i]-margin; |
---|
| 515 | } |
---|
| 516 | } |
---|
| 517 | break; |
---|
| 518 | case CAPSULE_SHAPE_PROXYTYPE: |
---|
| 519 | { |
---|
| 520 | btCapsuleShape* capsuleShape = (btCapsuleShape*)this; |
---|
| 521 | btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); |
---|
| 522 | int m_upAxis = capsuleShape->getUpAxis(); |
---|
| 523 | halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); |
---|
| 524 | halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); |
---|
| 525 | btMatrix3x3 abs_b = t.getBasis().absolute(); |
---|
| 526 | btPoint3 center = t.getOrigin(); |
---|
| 527 | btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); |
---|
| 528 | aabbMin = center - extent; |
---|
| 529 | aabbMax = center + extent; |
---|
| 530 | } |
---|
| 531 | break; |
---|
| 532 | case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: |
---|
| 533 | case CONVEX_HULL_SHAPE_PROXYTYPE: |
---|
| 534 | { |
---|
| 535 | btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; |
---|
| 536 | btScalar margin = convexHullShape->getMarginNonVirtual(); |
---|
| 537 | convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); |
---|
| 538 | } |
---|
| 539 | break; |
---|
| 540 | default: |
---|
| 541 | #ifndef __SPU__ |
---|
| 542 | this->getAabb (t, aabbMin, aabbMax); |
---|
| 543 | #else |
---|
| 544 | btAssert (0); |
---|
| 545 | #endif |
---|
| 546 | break; |
---|
| 547 | } |
---|
| 548 | |
---|
| 549 | // should never reach here |
---|
| 550 | btAssert (0); |
---|
| 551 | } |
---|