2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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:
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.
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"
29 #include "LinearMath/btIDebugDraw.h"
30 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
31 #include "BulletSoftBody/btSoftBody.h"
33 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06) //make this configurable
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)
42 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
46 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
49 m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject());
50 m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject();
53 // create the manifold from the dispatcher 'manifold pool'
55 // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
60 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
63 // m_dispatcher->releaseManifold( m_manifoldPtr );
66 void btSoftBodyTriangleCallback::clearCache()
68 for (int i = 0; i < m_shapeCache.size(); i++)
70 btTriIndex* tmp = m_shapeCache.getAtIndex(i);
72 btAssert(tmp->m_childShape);
73 m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape); //necessary?
74 delete tmp->m_childShape;
79 void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
81 //just for debugging purposes
82 //printf("triangle %d",m_triangleCount++);
84 btCollisionAlgorithmConstructionInfo ci;
85 ci.m_dispatcher1 = m_dispatcher;
87 ///debug drawing of the overlapping triangles
88 if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
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);
97 btTriIndex triIndex(partId, triangleIndex, 0);
98 btHashKey<btTriIndex> triKey(triIndex.getUid());
100 btTriIndex* shapeIndex = m_shapeCache[triKey];
103 btCollisionShape* tm = shapeIndex->m_childShape;
106 //copy over user pointers to temporary shape
107 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
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);
115 colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
116 colAlgo->~btCollisionAlgorithm();
117 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
122 //aabb filter is already applied!
124 //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
126 // if (m_softBody->getCollisionShape()->getShapeType()==
129 btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]);
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};
141 btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(), 6);
143 // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
145 //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
146 // tm.setMargin(m_collisionMarginTriangle);
148 //copy over user pointers to temporary shape
149 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
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());//??
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);
157 colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
158 colAlgo->~btCollisionAlgorithm();
159 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
161 triIndex.m_childShape = tm;
162 m_shapeCache.insert(triKey, triIndex);
166 void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
168 m_dispatchInfoPtr = &dispatchInfo;
169 m_collisionMarginTriangle = collisionMarginTriangle + btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
170 m_resultOut = resultOut;
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);
177 btTransform softTransform;
178 softTransform.setIdentity();
179 softTransform.setOrigin(softBodyCenter);
181 btTransform convexInTriangleSpace;
182 convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
183 btTransformAabb(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax);
186 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
188 m_btSoftBodyTriangleCallback.clearCache();
191 void btSoftBodyConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
193 //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
194 const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
196 if (triBody->getCollisionShape()->isConcave())
198 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBody->getCollisionShape());
200 // if (convexBody->getCollisionShape()->isConvex())
202 btScalar collisionMarginTriangle = concaveShape->getMargin();
204 // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
205 m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut);
207 concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax());
209 // resultOut->refreshContactPoints();
214 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
218 btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
219 btCollisionObject* triBody = m_isSwapped ? body0 : body1;
221 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
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())
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'
235 btTransform triInv = triBody->getWorldTransform().inverse();
236 btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
237 btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
239 struct LocalTriangleSphereCastCallback : public btTriangleCallback
241 btTransform m_ccdSphereFromTrans;
242 btTransform m_ccdSphereToTrans;
243 btTransform m_meshTransform;
245 btScalar m_ccdSphereRadius;
246 btScalar m_hitFraction;
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)
256 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
260 //do a swept sphere for now
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);
273 if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
274 ident, ident, castResult))
276 if (m_hitFraction > castResult.m_fraction)
277 m_hitFraction = castResult.m_fraction;
282 if (triBody->getCollisionShape()->isConcave())
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);
292 btScalar curHitFraction = btScalar(1.); //is this available?
293 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
294 convexbody->getCcdSweptSphereRadius(), curHitFraction);
296 raycastCallback.m_hitFraction = convexbody->getHitFraction();
298 btCollisionObject* concavebody = triBody;
300 btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
304 triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
307 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
309 convexbody->setHitFraction(raycastCallback.m_hitFraction);
310 return raycastCallback.m_hitFraction;