Blender V2.61 - r43446
|
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