dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btConvexHullShape.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 #if defined(_WIN32) || defined(__i386__)
  17 #define BT_USE_SSE_IN_API
  18 #endif
  19
  20 #include "btConvexHullShape.h"
  21 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
  22
  23 #include "LinearMath/btQuaternion.h"
  24 #include "LinearMath/btSerializer.h"
  25 #include "btConvexPolyhedron.h"
  26 #include "LinearMath/btConvexHullComputer.h"
  27
  28 btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape()
  29 {
  30         m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
  31         m_unscaledPoints.resize(numPoints);
  32
  33         unsigned char* pointsAddress = (unsigned char*)points;
  34
  35         for (int i = 0; i < numPoints; i++)
  36         {
  37                 btScalar* point = (btScalar*)pointsAddress;
  38                 m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
  39                 pointsAddress += stride;
  40         }
  41
  42         recalcLocalAabb();
  43 }
  44
  45 void btConvexHullShape::setLocalScaling(const btVector3& scaling)
  46 {
  47         m_localScaling = scaling;
  48         recalcLocalAabb();
  49 }
  50
  51 void btConvexHullShape::addPoint(const btVector3& point, bool recalculateLocalAabb)
  52 {
  53         m_unscaledPoints.push_back(point);
  54         if (recalculateLocalAabb)
  55                 recalcLocalAabb();
  56 }
  57
  58 btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
  59 {
  60         btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
  61         btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
  62
  63         // Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically.
  64         if (0 < m_unscaledPoints.size())
  65         {
  66                 btVector3 scaled = vec * m_localScaling;
  67                 int index = (int)scaled.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), maxDot);  // FIXME: may violate encapsulation of m_unscaledPoints
  68                 return m_unscaledPoints[index] * m_localScaling;
  69         }
  70
  71         return supVec;
  72 }
  73
  74 void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
  75 {
  76         btScalar newDot;
  77         //use 'w' component of supportVerticesOut?
  78         {
  79                 for (int i = 0; i < numVectors; i++)
  80                 {
  81                         supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
  82                 }
  83         }
  84
  85         for (int j = 0; j < numVectors; j++)
  86         {
  87                 btVector3 vec = vectors[j] * m_localScaling;  // dot(a*b,c) = dot(a,b*c)
  88                 if (0 < m_unscaledPoints.size())
  89                 {
  90                         int i = (int)vec.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
  91                         supportVerticesOut[j] = getScaledPoint(i);
  92                         supportVerticesOut[j][3] = newDot;
  93                 }
  94                 else
  95                         supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
  96         }
  97 }
  98
  99 btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec) const
 100 {
 101         btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 102
 103         if (getMargin() != btScalar(0.))
 104         {
 105                 btVector3 vecnorm = vec;
 106                 if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 107                 {
 108                         vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
 109                 }
 110                 vecnorm.normalize();
 111                 supVertex += getMargin() * vecnorm;
 112         }
 113         return supVertex;
 114 }
 115
 116 void btConvexHullShape::optimizeConvexHull()
 117 {
 118         btConvexHullComputer conv;
 119         conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3), m_unscaledPoints.size(), 0.f, 0.f);
 120         int numVerts = conv.vertices.size();
 121         m_unscaledPoints.resize(0);
 122         for (int i = 0; i < numVerts; i++)
 123         {
 124                 m_unscaledPoints.push_back(conv.vertices[i]);
 125         }
 126 }
 127
 128 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 129 //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
 130 int btConvexHullShape::getNumVertices() const
 131 {
 132         return m_unscaledPoints.size();
 133 }
 134
 135 int btConvexHullShape::getNumEdges() const
 136 {
 137         return m_unscaledPoints.size();
 138 }
 139
 140 void btConvexHullShape::getEdge(int i, btVector3& pa, btVector3& pb) const
 141 {
 142         int index0 = i % m_unscaledPoints.size();
 143         int index1 = (i + 1) % m_unscaledPoints.size();
 144         pa = getScaledPoint(index0);
 145         pb = getScaledPoint(index1);
 146 }
 147
 148 void btConvexHullShape::getVertex(int i, btVector3& vtx) const
 149 {
 150         vtx = getScaledPoint(i);
 151 }
 152
 153 int btConvexHullShape::getNumPlanes() const
 154 {
 155         return 0;
 156 }
 157
 158 void btConvexHullShape::getPlane(btVector3&, btVector3&, int) const
 159 {
 160         btAssert(0);
 161 }
 162
 163 //not yet
 164 bool btConvexHullShape::isInside(const btVector3&, btScalar) const
 165 {
 166         btAssert(0);
 167         return false;
 168 }
 169
 170 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 171 const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
 172 {
 173         //int szc = sizeof(btConvexHullShapeData);
 174         btConvexHullShapeData* shapeData = (btConvexHullShapeData*)dataBuffer;
 175         btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
 176
 177         int numElem = m_unscaledPoints.size();
 178         shapeData->m_numUnscaledPoints = numElem;
 179 #ifdef BT_USE_DOUBLE_PRECISION
 180         shapeData->m_unscaledPointsFloatPtr = 0;
 181         shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
 182 #else
 183         shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
 184         shapeData->m_unscaledPointsDoublePtr = 0;
 185 #endif
 186
 187         if (numElem)
 188         {
 189                 int sz = sizeof(btVector3Data);
 190                 //      int sz2 = sizeof(btVector3DoubleData);
 191                 //      int sz3 = sizeof(btVector3FloatData);
 192                 btChunk* chunk = serializer->allocate(sz, numElem);
 193                 btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr;
 194                 for (int i = 0; i < numElem; i++, memPtr++)
 195                 {
 196                         m_unscaledPoints[i].serialize(*memPtr);
 197                 }
 198                 serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]);
 199         }
 200
 201         // Fill padding with zeros to appease msan.
 202         memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3));
 203
 204         return "btConvexHullShapeData";
 205 }
 206
 207 void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const
 208 {
 209 #if 1
 210         minProj = FLT_MAX;
 211         maxProj = -FLT_MAX;
 212
 213         int numVerts = m_unscaledPoints.size();
 214         for (int i = 0; i < numVerts; i++)
 215         {
 216                 btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
 217                 btVector3 pt = trans * vtx;
 218                 btScalar dp = pt.dot(dir);
 219                 if (dp < minProj)
 220                 {
 221                         minProj = dp;
 222                         witnesPtMin = pt;
 223                 }
 224                 if (dp > maxProj)
 225                 {
 226                         maxProj = dp;
 227                         witnesPtMax = pt;
 228                 }
 229         }
 230 #else
 231         btVector3 localAxis = dir * trans.getBasis();
 232         witnesPtMin = trans(localGetSupportingVertex(localAxis));
 233         witnesPtMax = trans(localGetSupportingVertex(-localAxis));
 234
 235         minProj = witnesPtMin.dot(dir);
 236         maxProj = witnesPtMax.dot(dir);
 237 #endif
 238
 239         if (minProj > maxProj)
 240         {
 241                 btSwap(minProj, maxProj);
 242                 btSwap(witnesPtMin, witnesPtMax);
 243         }
 244 }