Bullet Collision Detection & Physics Library
btSoftBodyConcaveCollisionAlgorithm.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 
28 
32 
33 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06) //make this configurable
34 
37  m_isSwapped(isSwapped),
38  m_btSoftBodyTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped)
39 {
40 }
41 
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 
61 {
62  clearCache();
63  // m_dispatcher->releaseManifold( m_manifoldPtr );
64 }
65 
67 {
68  for (int i = 0; i < m_shapeCache.size(); i++)
69  {
71  btAssert(tmp);
72  btAssert(tmp->m_childShape);
74  delete tmp->m_childShape;
75  }
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 
86 
89  {
90  btVector3 color(1, 1, 0);
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
108 
110  //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
111  btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex);
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();
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();
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
150 
152  btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex); //btTransform::getIdentity());//??
153 
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();
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;
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 
187 {
189 }
190 
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 btCollisionObject* triOb = triBody->getCollisionObject();
199  const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triOb->getCollisionShape());
200 
201  // if (convexBody->getCollisionShape()->isConvex())
202  {
203  btScalar collisionMarginTriangle = concaveShape->getMargin();
204 
205  // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
206  m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut);
207 
209 
210  // resultOut->refreshContactPoints();
211  }
212  }
213 }
214 
216 {
217  (void)resultOut;
218  (void)dispatchInfo;
219  btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
220  btCollisionObject* triBody = m_isSwapped ? body0 : body1;
221 
222  //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
223 
224  //only perform CCD above a certain threshold, this prevents blocking on the long run
225  //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
226  btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
227  if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
228  {
229  return btScalar(1.);
230  }
231 
232  //const btVector3& from = convexbody->m_worldTransform.getOrigin();
233  //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
234  //todo: only do if the motion exceeds the 'radius'
235 
236  btTransform triInv = triBody->getWorldTransform().inverse();
237  btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
238  btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
239 
240  struct LocalTriangleSphereCastCallback : public btTriangleCallback
241  {
242  btTransform m_ccdSphereFromTrans;
243  btTransform m_ccdSphereToTrans;
244  btTransform m_meshTransform;
245 
246  btScalar m_ccdSphereRadius;
247  btScalar m_hitFraction;
248 
249  LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
250  : m_ccdSphereFromTrans(from),
251  m_ccdSphereToTrans(to),
252  m_ccdSphereRadius(ccdSphereRadius),
253  m_hitFraction(hitFraction)
254  {
255  }
256 
257  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
258  {
259  (void)partId;
260  (void)triangleIndex;
261  //do a swept sphere for now
262  btTransform ident;
263  ident.setIdentity();
264  btConvexCast::CastResult castResult;
265  castResult.m_fraction = m_hitFraction;
266  btSphereShape pointShape(m_ccdSphereRadius);
267  btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
268  btVoronoiSimplexSolver simplexSolver;
269  btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
270  //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
271  //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
272  //local space?
273 
274  if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
275  ident, ident, castResult))
276  {
277  if (m_hitFraction > castResult.m_fraction)
278  m_hitFraction = castResult.m_fraction;
279  }
280  }
281  };
282 
283  if (triBody->getCollisionShape()->isConcave())
284  {
285  btVector3 rayAabbMin = convexFromLocal.getOrigin();
286  rayAabbMin.setMin(convexToLocal.getOrigin());
287  btVector3 rayAabbMax = convexFromLocal.getOrigin();
288  rayAabbMax.setMax(convexToLocal.getOrigin());
289  btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
290  rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
291  rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
292 
293  btScalar curHitFraction = btScalar(1.); //is this available?
294  LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
295  convexbody->getCcdSweptSphereRadius(), curHitFraction);
296 
297  raycastCallback.m_hitFraction = convexbody->getHitFraction();
298 
299  btCollisionObject* concavebody = triBody;
300 
301  btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
302 
303  if (triangleMesh)
304  {
305  triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
306  }
307 
308  if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
309  {
310  convexbody->setHitFraction(raycastCallback.m_hitFraction);
311  return raycastCallback.m_hitFraction;
312  }
313  }
314 
315  return btScalar(1.);
316 }
void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
Definition: btAabbUtil2.h:172
ebtDispatcherQueryType
Definition: btDispatcher.h:69
@ BT_CLOSEST_POINT_ALGORITHMS
Definition: btDispatcher.h:71
@ BT_CONTACT_POINT_ALGORITHMS
Definition: btDispatcher.h:70
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:153
#define BT_SOFTBODY_TRIANGLE_EXTRUSION
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
btCollisionObject can be used to manage collision detection objects.
const btTransform & getInterpolationWorldTransform() const
btScalar getHitFraction() const
btTransform & getWorldTransform()
void setHitFraction(btScalar hitFraction)
const btCollisionShape * getCollisionShape() const
btScalar getCcdSweptSphereRadius() const
Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void * getUserPointer() const
void setUserPointer(void *userPtr)
optional user data pointer
bool isConcave() const
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
virtual btScalar getMargin() const
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const =0
The btConvexHullShape implements an implicit convex hull of an array of vertices.
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
void clear()
Definition: btHashMap.h:461
int size() const
Definition: btHashMap.h:373
const Value * getAtIndex(int index) const
Definition: btHashMap.h:378
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual int getDebugMode() const =0
btManifoldResult is a helper class to manage contact results.
btScalar m_closestPointDistanceThreshold
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
btSoftBodyTriangleCallback(btDispatcher *dispatcher, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
btHashMap< btHashKey< btTriIndex >, btTriIndex > m_shapeCache
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper *triObjWrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
btSoftBodyWorldInfo * getWorldInfo()
Definition: btSoftBody.h:876
virtual void getAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btSoftBody.h:1159
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btSubsimplexConvexCast implements Gino van den Bergens' paper "Ray Casting against bteral Convex Obje...
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:47
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:57
int RemoveReferences(btCollisionShape *pcs)
Definition: btSparseSDF.h:168
class btCollisionShape * m_childShape