dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btBox2dShape.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://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 #ifndef BT_OBB_BOX_2D_SHAPE_H
  17 #define BT_OBB_BOX_2D_SHAPE_H
  18
  19 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
  20 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
  21 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  22 #include "LinearMath/btVector3.h"
  23 #include "LinearMath/btMinMax.h"
  24
  25 ///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
  26 ATTRIBUTE_ALIGNED16(class)
  27 btBox2dShape : public btPolyhedralConvexShape
  28 {
  29         //btVector3     m_boxHalfExtents1; //use m_implicitShapeDimensions instead
  30
  31         btVector3 m_centroid;
  32         btVector3 m_vertices[4];
  33         btVector3 m_normals[4];
  34
  35 public:
  36         BT_DECLARE_ALIGNED_ALLOCATOR();
  37
  38         btVector3 getHalfExtentsWithMargin() const
  39         {
  40                 btVector3 halfExtents = getHalfExtentsWithoutMargin();
  41                 btVector3 margin(getMargin(), getMargin(), getMargin());
  42                 halfExtents += margin;
  43                 return halfExtents;
  44         }
  45
  46         const btVector3& getHalfExtentsWithoutMargin() const
  47         {
  48                 return m_implicitShapeDimensions;  //changed in Bullet 2.63: assume the scaling and margin are included
  49         }
  50
  51         virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
  52         {
  53                 btVector3 halfExtents = getHalfExtentsWithoutMargin();
  54                 btVector3 margin(getMargin(), getMargin(), getMargin());
  55                 halfExtents += margin;
  56
  57                 return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
  58                                                  btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
  59                                                  btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
  60         }
  61
  62         SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
  63         {
  64                 const btVector3& halfExtents = getHalfExtentsWithoutMargin();
  65
  66                 return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
  67                                                  btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
  68                                                  btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
  69         }
  70
  71         virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
  72         {
  73                 const btVector3& halfExtents = getHalfExtentsWithoutMargin();
  74
  75                 for (int i = 0; i < numVectors; i++)
  76                 {
  77                         const btVector3& vec = vectors[i];
  78                         supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
  79                                                                                    btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
  80                                                                                    btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
  81                 }
  82         }
  83
  84         ///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
  85         btBox2dShape(const btVector3& boxHalfExtents)
  86                 : btPolyhedralConvexShape(),
  87                   m_centroid(0, 0, 0)
  88         {
  89                 m_vertices[0].setValue(-boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
  90                 m_vertices[1].setValue(boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
  91                 m_vertices[2].setValue(boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
  92                 m_vertices[3].setValue(-boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
  93
  94                 m_normals[0].setValue(0, -1, 0);
  95                 m_normals[1].setValue(1, 0, 0);
  96                 m_normals[2].setValue(0, 1, 0);
  97                 m_normals[3].setValue(-1, 0, 0);
  98
  99                 btScalar minDimension = boxHalfExtents.getX();
 100                 if (minDimension > boxHalfExtents.getY())
 101                         minDimension = boxHalfExtents.getY();
 102
 103                 m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
 104                 btVector3 margin(getMargin(), getMargin(), getMargin());
 105                 m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
 106
 107                 setSafeMargin(minDimension);
 108         };
 109
 110         virtual void setMargin(btScalar collisionMargin)
 111         {
 112                 //correct the m_implicitShapeDimensions for the margin
 113                 btVector3 oldMargin(getMargin(), getMargin(), getMargin());
 114                 btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
 115
 116                 btConvexInternalShape::setMargin(collisionMargin);
 117                 btVector3 newMargin(getMargin(), getMargin(), getMargin());
 118                 m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
 119         }
 120         virtual void setLocalScaling(const btVector3& scaling)
 121         {
 122                 btVector3 oldMargin(getMargin(), getMargin(), getMargin());
 123                 btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
 124                 btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
 125
 126                 btConvexInternalShape::setLocalScaling(scaling);
 127
 128                 m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
 129         }
 130
 131         virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 132
 133         virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 134
 135         int getVertexCount() const
 136         {
 137                 return 4;
 138         }
 139
 140         virtual int getNumVertices() const
 141         {
 142                 return 4;
 143         }
 144
 145         const btVector3* getVertices() const
 146         {
 147                 return &m_vertices[0];
 148         }
 149
 150         const btVector3* getNormals() const
 151         {
 152                 return &m_normals[0];
 153         }
 154
 155         virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
 156         {
 157                 //this plane might not be aligned...
 158                 btVector4 plane;
 159                 getPlaneEquation(plane, i);
 160                 planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ());
 161                 planeSupport = localGetSupportingVertex(-planeNormal);
 162         }
 163
 164         const btVector3& getCentroid() const
 165         {
 166                 return m_centroid;
 167         }
 168
 169         virtual int getNumPlanes() const
 170         {
 171                 return 6;
 172         }
 173
 174         virtual int getNumEdges() const
 175         {
 176                 return 12;
 177         }
 178
 179         virtual void getVertex(int i, btVector3& vtx) const
 180         {
 181                 btVector3 halfExtents = getHalfExtentsWithoutMargin();
 182
 183                 vtx = btVector3(
 184                         halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1),
 185                         halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1),
 186                         halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2));
 187         }
 188
 189         virtual void getPlaneEquation(btVector4 & plane, int i) const
 190         {
 191                 btVector3 halfExtents = getHalfExtentsWithoutMargin();
 192
 193                 switch (i)
 194                 {
 195                         case 0:
 196                                 plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x());
 197                                 break;
 198                         case 1:
 199                                 plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x());
 200                                 break;
 201                         case 2:
 202                                 plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y());
 203                                 break;
 204                         case 3:
 205                                 plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y());
 206                                 break;
 207                         case 4:
 208                                 plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z());
 209                                 break;
 210                         case 5:
 211                                 plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z());
 212                                 break;
 213                         default:
 214                                 btAssert(0);
 215                 }
 216         }
 217
 218         virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
 219         //virtual void getEdge(int i,Edge& edge) const
 220         {
 221                 int edgeVert0 = 0;
 222                 int edgeVert1 = 0;
 223
 224                 switch (i)
 225                 {
 226                         case 0:
 227                                 edgeVert0 = 0;
 228                                 edgeVert1 = 1;
 229                                 break;
 230                         case 1:
 231                                 edgeVert0 = 0;
 232                                 edgeVert1 = 2;
 233                                 break;
 234                         case 2:
 235                                 edgeVert0 = 1;
 236                                 edgeVert1 = 3;
 237
 238                                 break;
 239                         case 3:
 240                                 edgeVert0 = 2;
 241                                 edgeVert1 = 3;
 242                                 break;
 243                         case 4:
 244                                 edgeVert0 = 0;
 245                                 edgeVert1 = 4;
 246                                 break;
 247                         case 5:
 248                                 edgeVert0 = 1;
 249                                 edgeVert1 = 5;
 250
 251                                 break;
 252                         case 6:
 253                                 edgeVert0 = 2;
 254                                 edgeVert1 = 6;
 255                                 break;
 256                         case 7:
 257                                 edgeVert0 = 3;
 258                                 edgeVert1 = 7;
 259                                 break;
 260                         case 8:
 261                                 edgeVert0 = 4;
 262                                 edgeVert1 = 5;
 263                                 break;
 264                         case 9:
 265                                 edgeVert0 = 4;
 266                                 edgeVert1 = 6;
 267                                 break;
 268                         case 10:
 269                                 edgeVert0 = 5;
 270                                 edgeVert1 = 7;
 271                                 break;
 272                         case 11:
 273                                 edgeVert0 = 6;
 274                                 edgeVert1 = 7;
 275                                 break;
 276                         default:
 277                                 btAssert(0);
 278                 }
 279
 280                 getVertex(edgeVert0, pa);
 281                 getVertex(edgeVert1, pb);
 282         }
 283
 284         virtual bool isInside(const btVector3& pt, btScalar tolerance) const
 285         {
 286                 btVector3 halfExtents = getHalfExtentsWithoutMargin();
 287
 288                 //btScalar minDist = 2*tolerance;
 289
 290                 bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
 291                                           (pt.x() >= (-halfExtents.x() - tolerance)) &&
 292                                           (pt.y() <= (halfExtents.y() + tolerance)) &&
 293                                           (pt.y() >= (-halfExtents.y() - tolerance)) &&
 294                                           (pt.z() <= (halfExtents.z() + tolerance)) &&
 295                                           (pt.z() >= (-halfExtents.z() - tolerance));
 296
 297                 return result;
 298         }
 299
 300         //debugging
 301         virtual const char* getName() const
 302         {
 303                 return "Box2d";
 304         }
 305
 306         virtual int getNumPreferredPenetrationDirections() const
 307         {
 308                 return 6;
 309         }
 310
 311         virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 312         {
 313                 switch (index)
 314                 {
 315                         case 0:
 316                                 penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
 317                                 break;
 318                         case 1:
 319                                 penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.));
 320                                 break;
 321                         case 2:
 322                                 penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.));
 323                                 break;
 324                         case 3:
 325                                 penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.));
 326                                 break;
 327                         case 4:
 328                                 penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.));
 329                                 break;
 330                         case 5:
 331                                 penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.));
 332                                 break;
 333                         default:
 334                                 btAssert(0);
 335                 }
 336         }
 337 };
 338
 339 #endif  //BT_OBB_BOX_2D_SHAPE_H