btCylinderShape.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 #include "btCylinderShape.h"
00017 
00018 btCylinderShape::btCylinderShape (const btVector3& halfExtents)
00019 :btConvexInternalShape(),
00020 m_upAxis(1)
00021 {
00022         setSafeMargin(halfExtents);
00023 
00024         btVector3 margin(getMargin(),getMargin(),getMargin());
00025         m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
00026         m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
00027 }
00028 
00029 
00030 btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
00031 :btCylinderShape(halfExtents)
00032 {
00033         m_upAxis = 0;
00034 
00035 }
00036 
00037 
00038 btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
00039 :btCylinderShape(halfExtents)
00040 {
00041         m_upAxis = 2;
00042 
00043 }
00044 
00045 void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
00046 {
00047         btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
00048 }
00049 
00050 void    btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00051 {
00052 
00053 //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
00054 //#define USE_BOX_INERTIA_APPROXIMATION 1
00055 #ifndef USE_BOX_INERTIA_APPROXIMATION
00056 
00057         /*
00058         cylinder is defined as following:
00059         *
00060         * - principle axis aligned along y by default, radius in x, z-value not used
00061         * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
00062         * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
00063         *
00064         */
00065 
00066         btScalar radius2;       // square of cylinder radius
00067         btScalar height2;       // square of cylinder height
00068         btVector3 halfExtents = getHalfExtentsWithMargin();     // get cylinder dimension
00069         btScalar div12 = mass / 12.f;
00070         btScalar div4 = mass / 4.f;
00071         btScalar div2 = mass / 2.f;
00072         int idxRadius, idxHeight;
00073 
00074         switch (m_upAxis)       // get indices of radius and height of cylinder
00075         {
00076                 case 0:         // cylinder is aligned along x
00077                         idxRadius = 1;
00078                         idxHeight = 0;
00079                         break;
00080                 case 2:         // cylinder is aligned along z
00081                         idxRadius = 0;
00082                         idxHeight = 2;
00083                         break;
00084                 default:        // cylinder is aligned along y
00085                         idxRadius = 0;
00086                         idxHeight = 1;
00087         }
00088 
00089         // calculate squares
00090         radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
00091         height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight];
00092 
00093         // calculate tensor terms
00094         btScalar t1 = div12 * height2 + div4 * radius2;
00095         btScalar t2 = div2 * radius2;
00096 
00097         switch (m_upAxis)       // set diagonal elements of inertia tensor
00098         {
00099                 case 0:         // cylinder is aligned along x
00100                         inertia.setValue(t2,t1,t1);
00101                         break;
00102                 case 2:         // cylinder is aligned along z
00103                         inertia.setValue(t1,t1,t2);
00104                         break;
00105                 default:        // cylinder is aligned along y
00106                         inertia.setValue(t1,t2,t1);
00107         }
00108 #else //USE_BOX_INERTIA_APPROXIMATION
00109         //approximation of box shape
00110         btVector3 halfExtents = getHalfExtentsWithMargin();
00111 
00112         btScalar lx=btScalar(2.)*(halfExtents.x());
00113         btScalar ly=btScalar(2.)*(halfExtents.y());
00114         btScalar lz=btScalar(2.)*(halfExtents.z());
00115 
00116         inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
00117                                         mass/(btScalar(12.0)) * (lx*lx + lz*lz),
00118                                         mass/(btScalar(12.0)) * (lx*lx + ly*ly));
00119 #endif //USE_BOX_INERTIA_APPROXIMATION
00120 }
00121 
00122 
00123 SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
00124 {
00125 const int cylinderUpAxis = 0;
00126 const int XX = 1;
00127 const int YY = 0;
00128 const int ZZ = 2;
00129 
00130         //mapping depends on how cylinder local orientation is
00131         // extents of the cylinder is: X,Y is for radius, and Z for height
00132 
00133 
00134         btScalar radius = halfExtents[XX];
00135         btScalar halfHeight = halfExtents[cylinderUpAxis];
00136 
00137 
00138     btVector3 tmp;
00139         btScalar d ;
00140 
00141     btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
00142     if (s != btScalar(0.0))
00143         {
00144         d = radius / s;  
00145                 tmp[XX] = v[XX] * d;
00146                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00147                 tmp[ZZ] = v[ZZ] * d;
00148                 return tmp;
00149         }
00150     else
00151         {
00152             tmp[XX] = radius;
00153                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00154                 tmp[ZZ] = btScalar(0.0);
00155                 return tmp;
00156     }
00157 
00158 
00159 }
00160 
00161 
00162 
00163 
00164 
00165 
00166 inline  btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) 
00167 {
00168 
00169 const int cylinderUpAxis = 1;
00170 const int XX = 0;
00171 const int YY = 1;
00172 const int ZZ = 2;
00173 
00174 
00175         btScalar radius = halfExtents[XX];
00176         btScalar halfHeight = halfExtents[cylinderUpAxis];
00177 
00178 
00179     btVector3 tmp;
00180         btScalar d ;
00181 
00182     btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
00183     if (s != btScalar(0.0))
00184         {
00185         d = radius / s;  
00186                 tmp[XX] = v[XX] * d;
00187                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00188                 tmp[ZZ] = v[ZZ] * d;
00189                 return tmp;
00190         }
00191     else
00192         {
00193             tmp[XX] = radius;
00194                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00195                 tmp[ZZ] = btScalar(0.0);
00196                 return tmp;
00197     }
00198 
00199 }
00200 
00201 inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) 
00202 {
00203 const int cylinderUpAxis = 2;
00204 const int XX = 0;
00205 const int YY = 2;
00206 const int ZZ = 1;
00207 
00208         //mapping depends on how cylinder local orientation is
00209         // extents of the cylinder is: X,Y is for radius, and Z for height
00210 
00211 
00212         btScalar radius = halfExtents[XX];
00213         btScalar halfHeight = halfExtents[cylinderUpAxis];
00214 
00215 
00216     btVector3 tmp;
00217         btScalar d ;
00218 
00219     btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
00220     if (s != btScalar(0.0))
00221         {
00222         d = radius / s;  
00223                 tmp[XX] = v[XX] * d;
00224                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00225                 tmp[ZZ] = v[ZZ] * d;
00226                 return tmp;
00227         }
00228     else
00229         {
00230             tmp[XX] = radius;
00231                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
00232                 tmp[ZZ] = btScalar(0.0);
00233                 return tmp;
00234     }
00235 
00236 
00237 }
00238 
00239 btVector3       btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
00240 {
00241         return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
00242 }
00243 
00244 
00245 btVector3       btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
00246 {
00247         return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
00248 }
00249 btVector3       btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
00250 {
00251         return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
00252 }
00253 
00254 void    btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00255 {
00256         for (int i=0;i<numVectors;i++)
00257         {
00258                 supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
00259         }
00260 }
00261 
00262 void    btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00263 {
00264         for (int i=0;i<numVectors;i++)
00265         {
00266                 supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
00267         }
00268 }
00269 
00270 
00271 
00272 
00273 void    btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00274 {
00275         for (int i=0;i<numVectors;i++)
00276         {
00277                 supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
00278         }
00279 }
00280 
00281