btCapsuleShape.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 
00017 #include "btCapsuleShape.h"
00018 
00019 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
00020 #include "LinearMath/btQuaternion.h"
00021 
00022 btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
00023 {
00024         m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
00025         m_upAxis = 1;
00026         m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
00027 }
00028 
00029  
00030  btVector3      btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
00031 {
00032 
00033         btVector3 supVec(0,0,0);
00034 
00035         btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00036 
00037         btVector3 vec = vec0;
00038         btScalar lenSqr = vec.length2();
00039         if (lenSqr < btScalar(0.0001))
00040         {
00041                 vec.setValue(1,0,0);
00042         } else
00043         {
00044                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00045                 vec *= rlen;
00046         }
00047 
00048         btVector3 vtx;
00049         btScalar newDot;
00050         
00051         btScalar radius = getRadius();
00052 
00053 
00054         {
00055                 btVector3 pos(0,0,0);
00056                 pos[getUpAxis()] = getHalfHeight();
00057 
00058                 vtx = pos +vec*(radius) - vec * getMargin();
00059                 newDot = vec.dot(vtx);
00060                 if (newDot > maxDot)
00061                 {
00062                         maxDot = newDot;
00063                         supVec = vtx;
00064                 }
00065         }
00066         {
00067                 btVector3 pos(0,0,0);
00068                 pos[getUpAxis()] = -getHalfHeight();
00069 
00070                 vtx = pos +vec*(radius) - vec * getMargin();
00071                 newDot = vec.dot(vtx);
00072                 if (newDot > maxDot)
00073                 {
00074                         maxDot = newDot;
00075                         supVec = vtx;
00076                 }
00077         }
00078 
00079         return supVec;
00080 
00081 }
00082 
00083  void   btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00084 {
00085 
00086         
00087         btScalar radius = getRadius();
00088 
00089         for (int j=0;j<numVectors;j++)
00090         {
00091                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00092                 const btVector3& vec = vectors[j];
00093 
00094                 btVector3 vtx;
00095                 btScalar newDot;
00096                 {
00097                         btVector3 pos(0,0,0);
00098                         pos[getUpAxis()] = getHalfHeight();
00099                         vtx = pos +vec*(radius) - vec * getMargin();
00100                         newDot = vec.dot(vtx);
00101                         if (newDot > maxDot)
00102                         {
00103                                 maxDot = newDot;
00104                                 supportVerticesOut[j] = vtx;
00105                         }
00106                 }
00107                 {
00108                         btVector3 pos(0,0,0);
00109                         pos[getUpAxis()] = -getHalfHeight();
00110                         vtx = pos +vec*(radius) - vec * getMargin();
00111                         newDot = vec.dot(vtx);
00112                         if (newDot > maxDot)
00113                         {
00114                                 maxDot = newDot;
00115                                 supportVerticesOut[j] = vtx;
00116                         }
00117                 }
00118                 
00119         }
00120 }
00121 
00122 
00123 void    btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00124 {
00125         //as an approximation, take the inertia of the box that bounds the spheres
00126 
00127         btTransform ident;
00128         ident.setIdentity();
00129 
00130         
00131         btScalar radius = getRadius();
00132 
00133         btVector3 halfExtents(radius,radius,radius);
00134         halfExtents[getUpAxis()]+=getHalfHeight();
00135 
00136         btScalar margin = CONVEX_DISTANCE_MARGIN;
00137 
00138         btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
00139         btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
00140         btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
00141         const btScalar x2 = lx*lx;
00142         const btScalar y2 = ly*ly;
00143         const btScalar z2 = lz*lz;
00144         const btScalar scaledmass = mass * btScalar(.08333333);
00145 
00146         inertia[0] = scaledmass * (y2+z2);
00147         inertia[1] = scaledmass * (x2+z2);
00148         inertia[2] = scaledmass * (x2+y2);
00149 
00150 }
00151 
00152 btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
00153 {
00154         m_upAxis = 0;
00155         m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
00156 }
00157 
00158 
00159 
00160 
00161 
00162 
00163 btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
00164 {
00165         m_upAxis = 2;
00166         m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
00167 }
00168 
00169 
00170 
00171