dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btCylinderShape.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
   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 "btCylinderShape.h"
  17
  18 btCylinderShape::btCylinderShape(const btVector3& halfExtents)
  19         : btConvexInternalShape(),
  20           m_upAxis(1)
  21 {
  22         btVector3 margin(getMargin(), getMargin(), getMargin());
  23         m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
  24
  25         setSafeMargin(halfExtents);
  26
  27         m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
  28 }
  29
  30 btCylinderShapeX::btCylinderShapeX(const btVector3& halfExtents)
  31         : btCylinderShape(halfExtents)
  32 {
  33         m_upAxis = 0;
  34 }
  35
  36 btCylinderShapeZ::btCylinderShapeZ(const btVector3& halfExtents)
  37         : btCylinderShape(halfExtents)
  38 {
  39         m_upAxis = 2;
  40 }
  41
  42 void btCylinderShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
  43 {
  44         btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
  45 }
  46
  47 void btCylinderShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
  48 {
  49 //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
  50 //#define USE_BOX_INERTIA_APPROXIMATION 1
  51 #ifndef USE_BOX_INERTIA_APPROXIMATION
  52
  53         /*
  54         cylinder is defined as following:
  55         *
  56         * - principle axis aligned along y by default, radius in x, z-value not used
  57         * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
  58         * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
  59         *
  60         */
  61
  62         btScalar radius2;                                    // square of cylinder radius
  63         btScalar height2;                                    // square of cylinder height
  64         btVector3 halfExtents = getHalfExtentsWithMargin();  // get cylinder dimension
  65         btScalar div12 = mass / 12.f;
  66         btScalar div4 = mass / 4.f;
  67         btScalar div2 = mass / 2.f;
  68         int idxRadius, idxHeight;
  69
  70         switch (m_upAxis)  // get indices of radius and height of cylinder
  71         {
  72                 case 0:  // cylinder is aligned along x
  73                         idxRadius = 1;
  74                         idxHeight = 0;
  75                         break;
  76                 case 2:  // cylinder is aligned along z
  77                         idxRadius = 0;
  78                         idxHeight = 2;
  79                         break;
  80                 default:  // cylinder is aligned along y
  81                         idxRadius = 0;
  82                         idxHeight = 1;
  83         }
  84
  85         // calculate squares
  86         radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
  87         height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight];
  88
  89         // calculate tensor terms
  90         btScalar t1 = div12 * height2 + div4 * radius2;
  91         btScalar t2 = div2 * radius2;
  92
  93         switch (m_upAxis)  // set diagonal elements of inertia tensor
  94         {
  95                 case 0:  // cylinder is aligned along x
  96                         inertia.setValue(t2, t1, t1);
  97                         break;
  98                 case 2:  // cylinder is aligned along z
  99                         inertia.setValue(t1, t1, t2);
 100                         break;
 101                 default:  // cylinder is aligned along y
 102                         inertia.setValue(t1, t2, t1);
 103         }
 104 #else   //USE_BOX_INERTIA_APPROXIMATION
 105         //approximation of box shape
 106         btVector3 halfExtents = getHalfExtentsWithMargin();
 107
 108         btScalar lx = btScalar(2.) * (halfExtents.x());
 109         btScalar ly = btScalar(2.) * (halfExtents.y());
 110         btScalar lz = btScalar(2.) * (halfExtents.z());
 111
 112         inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
 113                                          mass / (btScalar(12.0)) * (lx * lx + lz * lz),
 114                                          mass / (btScalar(12.0)) * (lx * lx + ly * ly));
 115 #endif  //USE_BOX_INERTIA_APPROXIMATION
 116 }
 117
 118 SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents, const btVector3& v)
 119 {
 120         const int cylinderUpAxis = 0;
 121         const int XX = 1;
 122         const int YY = 0;
 123         const int ZZ = 2;
 124
 125         //mapping depends on how cylinder local orientation is
 126         // extents of the cylinder is: X,Y is for radius, and Z for height
 127
 128         btScalar radius = halfExtents[XX];
 129         btScalar halfHeight = halfExtents[cylinderUpAxis];
 130
 131         btVector3 tmp;
 132         btScalar d;
 133
 134         btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
 135         if (s != btScalar(0.0))
 136         {
 137                 d = radius / s;
 138                 tmp[XX] = v[XX] * d;
 139                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 140                 tmp[ZZ] = v[ZZ] * d;
 141                 return tmp;
 142         }
 143         else
 144         {
 145                 tmp[XX] = radius;
 146                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 147                 tmp[ZZ] = btScalar(0.0);
 148                 return tmp;
 149         }
 150 }
 151
 152 inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents, const btVector3& v)
 153 {
 154         const int cylinderUpAxis = 1;
 155         const int XX = 0;
 156         const int YY = 1;
 157         const int ZZ = 2;
 158
 159         btScalar radius = halfExtents[XX];
 160         btScalar halfHeight = halfExtents[cylinderUpAxis];
 161
 162         btVector3 tmp;
 163         btScalar d;
 164
 165         btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
 166         if (s != btScalar(0.0))
 167         {
 168                 d = radius / s;
 169                 tmp[XX] = v[XX] * d;
 170                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 171                 tmp[ZZ] = v[ZZ] * d;
 172                 return tmp;
 173         }
 174         else
 175         {
 176                 tmp[XX] = radius;
 177                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 178                 tmp[ZZ] = btScalar(0.0);
 179                 return tmp;
 180         }
 181 }
 182
 183 inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents, const btVector3& v)
 184 {
 185         const int cylinderUpAxis = 2;
 186         const int XX = 0;
 187         const int YY = 2;
 188         const int ZZ = 1;
 189
 190         //mapping depends on how cylinder local orientation is
 191         // extents of the cylinder is: X,Y is for radius, and Z for height
 192
 193         btScalar radius = halfExtents[XX];
 194         btScalar halfHeight = halfExtents[cylinderUpAxis];
 195
 196         btVector3 tmp;
 197         btScalar d;
 198
 199         btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
 200         if (s != btScalar(0.0))
 201         {
 202                 d = radius / s;
 203                 tmp[XX] = v[XX] * d;
 204                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 205                 tmp[ZZ] = v[ZZ] * d;
 206                 return tmp;
 207         }
 208         else
 209         {
 210                 tmp[XX] = radius;
 211                 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 212                 tmp[ZZ] = btScalar(0.0);
 213                 return tmp;
 214         }
 215 }
 216
 217 btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 218 {
 219         return CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec);
 220 }
 221
 222 btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 223 {
 224         return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec);
 225 }
 226 btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 227 {
 228         return CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec);
 229 }
 230
 231 void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 232 {
 233         for (int i = 0; i < numVectors; i++)
 234         {
 235                 supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i]);
 236         }
 237 }
 238
 239 void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 240 {
 241         for (int i = 0; i < numVectors; i++)
 242         {
 243                 supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i]);
 244         }
 245 }
 246
 247 void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 248 {
 249         for (int i = 0; i < numVectors; i++)
 250         {
 251                 supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vectors[i]);
 252         }
 253 }