Blender V2.61 - r43446

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