1 /*
2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
3 
4  Bullet Continuous Collision Detection and Physics Library
5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
6  This software is provided 'as-is', without any express or implied warranty.
7  In no event will the authors be held liable for any damages arising from the use of this software.
8  Permission is granted to anyone to use this software for any purpose,
9  including commercial applications, and to alter it and redistribute it freely,
10  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.
14  */
15 
16 #ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
17 #define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
18 
19 #include "btSoftMultiBodyDynamicsWorld.h"
20 #include "btDeformableLagrangianForce.h"
21 #include "btDeformableMassSpringForce.h"
22 #include "btDeformableBodySolver.h"
23 #include "btDeformableMultiBodyConstraintSolver.h"
24 #include "btSoftBodyHelpers.h"
25 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
26 #include <functional>
27 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
28 
29 class btDeformableBodySolver;
30 class btDeformableLagrangianForce;
31 struct MultiBodyInplaceSolverIslandCallback;
32 struct DeformableBodyInplaceSolverIslandCallback;
33 class btDeformableMultiBodyConstraintSolver;
34 
35 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
36 
37 class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
38 {
39 	typedef btAlignedObjectArray<btVector3> TVStack;
40 	///Solver classes that encapsulate multiple deformable bodies for solving
41 	btDeformableBodySolver* m_deformableBodySolver;
42 	btSoftBodyArray m_softBodies;
43 	int m_drawFlags;
44 	bool m_drawNodeTree;
45 	bool m_drawFaceTree;
46 	bool m_drawClusterTree;
47 	btSoftBodyWorldInfo m_sbi;
48 	btScalar m_internalTime;
49 	int m_ccdIterations;
50 	bool m_implicit;
51 	bool m_lineSearch;
52 	bool m_useProjection;
53 	DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
54 
55 	typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
56 	btSolverCallback m_solverCallback;
57 
58 protected:
59 	virtual void internalSingleStepSimulation(btScalar timeStep);
60 
61 	virtual void integrateTransforms(btScalar timeStep);
62 
63 	void positionCorrection(btScalar timeStep);
64 
65 	void solveConstraints(btScalar timeStep);
66 
67 	void updateActivationState(btScalar timeStep);
68 
69 	void clearGravity();
70 
71 public:
72 	btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
73 
74 	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
75 
76 	virtual void debugDrawWorld();
77 
setSolverCallback(btSolverCallback cb)78 	void setSolverCallback(btSolverCallback cb)
79 	{
80 		m_solverCallback = cb;
81 	}
82 
83 	virtual ~btDeformableMultiBodyDynamicsWorld();
84 
getMultiBodyDynamicsWorld()85 	virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
86 	{
87 		return (btMultiBodyDynamicsWorld*)(this);
88 	}
89 
getMultiBodyDynamicsWorld()90 	virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
91 	{
92 		return (const btMultiBodyDynamicsWorld*)(this);
93 	}
94 
getWorldType()95 	virtual btDynamicsWorldType getWorldType() const
96 	{
97 		return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
98 	}
99 
100 	virtual void predictUnconstraintMotion(btScalar timeStep);
101 
102 	virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
103 
getSoftBodyArray()104 	btSoftBodyArray& getSoftBodyArray()
105 	{
106 		return m_softBodies;
107 	}
108 
getSoftBodyArray()109 	const btSoftBodyArray& getSoftBodyArray() const
110 	{
111 		return m_softBodies;
112 	}
113 
getWorldInfo()114 	btSoftBodyWorldInfo& getWorldInfo()
115 	{
116 		return m_sbi;
117 	}
118 
getWorldInfo()119 	const btSoftBodyWorldInfo& getWorldInfo() const
120 	{
121 		return m_sbi;
122 	}
123 
124 	void reinitialize(btScalar timeStep);
125 
126 	void applyRigidBodyGravity(btScalar timeStep);
127 
128 	void beforeSolverCallbacks(btScalar timeStep);
129 
130 	void afterSolverCallbacks(btScalar timeStep);
131 
132 	void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
133 
134 	void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
135 
136 	void removeSoftBodyForce(btSoftBody* psb);
137 
138 	void removeSoftBody(btSoftBody* body);
139 
140 	void removeCollisionObject(btCollisionObject* collisionObject);
141 
getDrawFlags()142 	int getDrawFlags() const { return (m_drawFlags); }
setDrawFlags(int f)143 	void setDrawFlags(int f) { m_drawFlags = f; }
144 
145 	void setupConstraints();
146 
147 	void performDeformableCollisionDetection();
148 
149 	void solveMultiBodyConstraints();
150 
151 	void solveContactConstraints();
152 
153 	void sortConstraints();
154 
155 	void softBodySelfCollision();
156 
setImplicit(bool implicit)157 	void setImplicit(bool implicit)
158 	{
159 		m_implicit = implicit;
160 	}
161 
setLineSearch(bool lineSearch)162 	void setLineSearch(bool lineSearch)
163 	{
164 		m_lineSearch = lineSearch;
165 	}
166 
setUseProjection(bool useProjection)167 	void setUseProjection(bool useProjection)
168 	{
169 		m_useProjection = useProjection;
170 	}
171 
172 	void applyRepulsionForce(btScalar timeStep);
173 
174 	void performGeometricCollisions(btScalar timeStep);
175 
176 	struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
177 	{
178 		btVector3 m_rayFromWorld;
179 		btVector3 m_rayToWorld;
180 		btTransform m_rayFromTrans;
181 		btTransform m_rayToTrans;
182 		btVector3 m_hitNormal;
183 
184 		const btDeformableMultiBodyDynamicsWorld* m_world;
185 		btCollisionWorld::RayResultCallback& m_resultCallback;
186 
btDeformableSingleRayCallbackbtDeformableSingleRayCallback187 		btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
188 			: m_rayFromWorld(rayFromWorld),
189 			  m_rayToWorld(rayToWorld),
190 			  m_world(world),
191 			  m_resultCallback(resultCallback)
192 		{
193 			m_rayFromTrans.setIdentity();
194 			m_rayFromTrans.setOrigin(m_rayFromWorld);
195 			m_rayToTrans.setIdentity();
196 			m_rayToTrans.setOrigin(m_rayToWorld);
197 
198 			btVector3 rayDir = (rayToWorld - rayFromWorld);
199 
200 			rayDir.normalize();
201 			///what about division by zero? --> just set rayDirection[i] to INF/1e30
202 			m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
203 			m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
204 			m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
205 			m_signs[0] = m_rayDirectionInverse[0] < 0.0;
206 			m_signs[1] = m_rayDirectionInverse[1] < 0.0;
207 			m_signs[2] = m_rayDirectionInverse[2] < 0.0;
208 
209 			m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
210 		}
211 
processbtDeformableSingleRayCallback212 		virtual bool process(const btBroadphaseProxy* proxy)
213 		{
214 			///terminate further ray tests, once the closestHitFraction reached zero
215 			if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
216 				return false;
217 
218 			btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
219 
220 			//only perform raycast if filterMask matches
221 			if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
222 			{
223 				//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
224 				//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
225 #if 0
226 #ifdef RECALCULATE_AABB
227                 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
228                 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
229 #else
230                 //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
231                 const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
232                 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
233 #endif
234 #endif
235 				//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
236 				//culling already done by broadphase
237 				//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
238 				{
239 					m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
240 										   collisionObject,
241 										   collisionObject->getCollisionShape(),
242 										   collisionObject->getWorldTransform(),
243 										   m_resultCallback);
244 				}
245 			}
246 			return true;
247 		}
248 	};
249 
rayTest(const btVector3 & rayFromWorld,const btVector3 & rayToWorld,RayResultCallback & resultCallback)250 	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
251 	{
252 		BT_PROFILE("rayTest");
253 		/// use the broadphase to accelerate the search for objects, based on their aabb
254 		/// and for each object with ray-aabb overlap, perform an exact ray test
255 		btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
256 
257 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
258 		m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
259 #else
260 		for (int i = 0; i < this->getNumCollisionObjects(); i++)
261 		{
262 			rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
263 		}
264 #endif  //USE_BRUTEFORCE_RAYBROADPHASE
265 	}
266 
rayTestSingle(const btTransform & rayFromTrans,const btTransform & rayToTrans,btCollisionObject * collisionObject,const btCollisionShape * collisionShape,const btTransform & colObjWorldTransform,RayResultCallback & resultCallback)267 	void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
268 					   btCollisionObject* collisionObject,
269 					   const btCollisionShape* collisionShape,
270 					   const btTransform& colObjWorldTransform,
271 					   RayResultCallback& resultCallback) const
272 	{
273 		if (collisionShape->isSoftBody())
274 		{
275 			btSoftBody* softBody = btSoftBody::upcast(collisionObject);
276 			if (softBody)
277 			{
278 				btSoftBody::sRayCast softResult;
279 				if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
280 				{
281 					if (softResult.fraction <= resultCallback.m_closestHitFraction)
282 					{
283 						btCollisionWorld::LocalShapeInfo shapeInfo;
284 						shapeInfo.m_shapePart = 0;
285 						shapeInfo.m_triangleIndex = softResult.index;
286 						// get the normal
287 						btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
288 						btVector3 normal = -rayDir;
289 						normal.normalize();
290 						{
291 							normal = softBody->m_faces[softResult.index].m_normal;
292 							if (normal.dot(rayDir) > 0)
293 							{
294 								// normal always point toward origin of the ray
295 								normal = -normal;
296 							}
297 						}
298 
299 						btCollisionWorld::LocalRayResult rayResult(collisionObject,
300 																   &shapeInfo,
301 																   normal,
302 																   softResult.fraction);
303 						bool normalInWorldSpace = true;
304 						resultCallback.addSingleResult(rayResult, normalInWorldSpace);
305 					}
306 				}
307 			}
308 		}
309 		else
310 		{
311 			btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
312 		}
313 	}
314 };
315 
316 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
317