dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

   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 #include "btSoftBodyConcaveCollisionAlgorithm.h"
  17 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  18 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
  19 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  20 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
  21 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  22 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
  23 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  24 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  25 #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
  26 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
  27 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  28
  29 #include "LinearMath/btIDebugDraw.h"
  30 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  31 #include "BulletSoftBody/btSoftBody.h"
  32
  33 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)  //make this configurable
  34
  35 btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
  36         : btCollisionAlgorithm(ci),
  37           m_isSwapped(isSwapped),
  38           m_btSoftBodyTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped)
  39 {
  40 }
  41
  42 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
  43 {
  44 }
  45
  46 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
  47                                                                                                                                                                                                                                                                                                                                                                                  m_dispatchInfoPtr(0)
  48 {
  49         m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject());
  50         m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject();
  51
  52         //
  53         // create the manifold from the dispatcher 'manifold pool'
  54         //
  55         //        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
  56
  57         clearCache();
  58 }
  59
  60 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
  61 {
  62         clearCache();
  63         //      m_dispatcher->releaseManifold( m_manifoldPtr );
  64 }
  65
  66 void btSoftBodyTriangleCallback::clearCache()
  67 {
  68         for (int i = 0; i < m_shapeCache.size(); i++)
  69         {
  70                 btTriIndex* tmp = m_shapeCache.getAtIndex(i);
  71                 btAssert(tmp);
  72                 btAssert(tmp->m_childShape);
  73                 m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);  //necessary?
  74                 delete tmp->m_childShape;
  75         }
  76         m_shapeCache.clear();
  77 }
  78
  79 void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
  80 {
  81         //just for debugging purposes
  82         //printf("triangle %d",m_triangleCount++);
  83
  84         btCollisionAlgorithmConstructionInfo ci;
  85         ci.m_dispatcher1 = m_dispatcher;
  86
  87         ///debug drawing of the overlapping triangles
  88         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
  89         {
  90                 btVector3 color(1, 1, 0);
  91                 const btTransform& tr = m_triBody->getWorldTransform();
  92                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color);
  93                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color);
  94                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);
  95         }
  96
  97         btTriIndex triIndex(partId, triangleIndex, 0);
  98         btHashKey<btTriIndex> triKey(triIndex.getUid());
  99
 100         btTriIndex* shapeIndex = m_shapeCache[triKey];
 101         if (shapeIndex)
 102         {
 103                 btCollisionShape* tm = shapeIndex->m_childShape;
 104                 btAssert(tm);
 105
 106                 //copy over user pointers to temporary shape
 107                 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
 108
 109                 btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1);
 110                 //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
 111                 btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex);
 112                 ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
 113                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType);  //m_manifoldPtr);
 114
 115                 colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
 116                 colAlgo->~btCollisionAlgorithm();
 117                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 118
 119                 return;
 120         }
 121
 122         //aabb filter is already applied!
 123
 124         //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
 125
 126         //      if (m_softBody->getCollisionShape()->getShapeType()==
 127         {
 128                 //              btVector3 other;
 129                 btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]);
 130                 normal.normalize();
 131                 normal *= BT_SOFTBODY_TRIANGLE_EXTRUSION;
 132                 //              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
 133                 //              other+=normal*22.f;
 134                 btVector3 pts[6] = {triangle[0] + normal,
 135                                                         triangle[1] + normal,
 136                                                         triangle[2] + normal,
 137                                                         triangle[0] - normal,
 138                                                         triangle[1] - normal,
 139                                                         triangle[2] - normal};
 140
 141                 btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(), 6);
 142
 143                 //              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
 144
 145                 //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
 146                 //      tm.setMargin(m_collisionMarginTriangle);
 147
 148                 //copy over user pointers to temporary shape
 149                 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
 150
 151                 btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1);
 152                 btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex);  //btTransform::getIdentity());//??
 153
 154                 ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
 155                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType);  //m_manifoldPtr);
 156
 157                 colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
 158                 colAlgo->~btCollisionAlgorithm();
 159                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 160
 161                 triIndex.m_childShape = tm;
 162                 m_shapeCache.insert(triKey, triIndex);
 163         }
 164 }
 165
 166 void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 167 {
 168         m_dispatchInfoPtr = &dispatchInfo;
 169         m_collisionMarginTriangle = collisionMarginTriangle + btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
 170         m_resultOut = resultOut;
 171
 172         btVector3 aabbWorldSpaceMin, aabbWorldSpaceMax;
 173         m_softBody->getAabb(aabbWorldSpaceMin, aabbWorldSpaceMax);
 174         btVector3 halfExtents = (aabbWorldSpaceMax - aabbWorldSpaceMin) * btScalar(0.5);
 175         btVector3 softBodyCenter = (aabbWorldSpaceMax + aabbWorldSpaceMin) * btScalar(0.5);
 176
 177         btTransform softTransform;
 178         softTransform.setIdentity();
 179         softTransform.setOrigin(softBodyCenter);
 180
 181         btTransform convexInTriangleSpace;
 182         convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
 183         btTransformAabb(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax);
 184 }
 185
 186 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
 187 {
 188         m_btSoftBodyTriangleCallback.clearCache();
 189 }
 190
 191 void btSoftBodyConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 192 {
 193         //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
 194         const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
 195
 196         if (triBody->getCollisionShape()->isConcave())
 197         {
 198                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBody->getCollisionShape());
 199
 200                 //      if (convexBody->getCollisionShape()->isConvex())
 201                 {
 202                         btScalar collisionMarginTriangle = concaveShape->getMargin();
 203
 204                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
 205                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut);
 206
 207                         concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax());
 208
 209                         //      resultOut->refreshContactPoints();
 210                 }
 211         }
 212 }
 213
 214 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 215 {
 216         (void)resultOut;
 217         (void)dispatchInfo;
 218         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 219         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 220
 221         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 222
 223         //only perform CCD above a certain threshold, this prevents blocking on the long run
 224         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
 225         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
 226         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
 227         {
 228                 return btScalar(1.);
 229         }
 230
 231         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
 232         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 233         //todo: only do if the motion exceeds the 'radius'
 234
 235         btTransform triInv = triBody->getWorldTransform().inverse();
 236         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 237         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 238
 239         struct LocalTriangleSphereCastCallback : public btTriangleCallback
 240         {
 241                 btTransform m_ccdSphereFromTrans;
 242                 btTransform m_ccdSphereToTrans;
 243                 btTransform m_meshTransform;
 244
 245                 btScalar m_ccdSphereRadius;
 246                 btScalar m_hitFraction;
 247
 248                 LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
 249                         : m_ccdSphereFromTrans(from),
 250                           m_ccdSphereToTrans(to),
 251                           m_ccdSphereRadius(ccdSphereRadius),
 252                           m_hitFraction(hitFraction)
 253                 {
 254                 }
 255
 256                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 257                 {
 258                         (void)partId;
 259                         (void)triangleIndex;
 260                         //do a swept sphere for now
 261                         btTransform ident;
 262                         ident.setIdentity();
 263                         btConvexCast::CastResult castResult;
 264                         castResult.m_fraction = m_hitFraction;
 265                         btSphereShape pointShape(m_ccdSphereRadius);
 266                         btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
 267                         btVoronoiSimplexSolver simplexSolver;
 268                         btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
 269                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 270                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 271                         //local space?
 272
 273                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
 274                                                                                           ident, ident, castResult))
 275                         {
 276                                 if (m_hitFraction > castResult.m_fraction)
 277                                         m_hitFraction = castResult.m_fraction;
 278                         }
 279                 }
 280         };
 281
 282         if (triBody->getCollisionShape()->isConcave())
 283         {
 284                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
 285                 rayAabbMin.setMin(convexToLocal.getOrigin());
 286                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
 287                 rayAabbMax.setMax(convexToLocal.getOrigin());
 288                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
 289                 rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 290                 rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 291
 292                 btScalar curHitFraction = btScalar(1.);  //is this available?
 293                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
 294                                                                                                                 convexbody->getCcdSweptSphereRadius(), curHitFraction);
 295
 296                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
 297
 298                 btCollisionObject* concavebody = triBody;
 299
 300                 btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
 301
 302                 if (triangleMesh)
 303                 {
 304                         triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
 305                 }
 306
 307                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 308                 {
 309                         convexbody->setHitFraction(raycastCallback.m_hitFraction);
 310                         return raycastCallback.m_hitFraction;
 311                 }
 312         }
 313
 314         return btScalar(1.);
 315 }