1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2019 Google Inc. http://bulletphysics.org
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 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.
10 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
11 3. This notice may not be removed or altered from any source distribution.
12 */
13
14 #include "DeformableContact.h"
15 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
16 #include "btBulletDynamicsCommon.h"
17 #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
18 #include "BulletSoftBody/btSoftBody.h"
19 #include "BulletSoftBody/btSoftBodyHelpers.h"
20 #include "BulletSoftBody/btDeformableBodySolver.h"
21 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
22 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
23 #include <stdio.h> //printf debugging
24
25 #include "../CommonInterfaces/CommonDeformableBodyBase.h"
26 #include "../Utils/b3ResourcePath.h"
27
28 ///The DeformableContact shows the contact between deformable objects
29
30 class DeformableContact : public CommonDeformableBodyBase
31 {
32 public:
DeformableContact(struct GUIHelperInterface * helper)33 DeformableContact(struct GUIHelperInterface* helper)
34 : CommonDeformableBodyBase(helper)
35 {
36 }
~DeformableContact()37 virtual ~DeformableContact()
38 {
39 }
40 void initPhysics();
41
42 void exitPhysics();
43
resetCamera()44 void resetCamera()
45 {
46 float dist = 12;
47 float pitch = -50;
48 float yaw = 120;
49 float targetPos[3] = {0, -3, 0};
50 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
51 }
52
stepSimulation(float deltaTime)53 void stepSimulation(float deltaTime)
54 {
55 float internalTimeStep = 1. / 240.f;
56 m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
57 }
58
renderScene()59 virtual void renderScene()
60 {
61 CommonDeformableBodyBase::renderScene();
62
63
64 btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
65
66 for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
67 {
68 btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
69 {
70 //btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
71 btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// StddeformableWorld->getDrawFlags());
72 }
73 }
74
75 }
76 };
77
initPhysics()78 void DeformableContact::initPhysics()
79 {
80 m_guiHelper->setUpAxis(1);
81
82 ///collision configuration contains default setup for memory, collision setup
83 m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
84
85 ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
86 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
87
88 m_broadphase = new btDbvtBroadphase();
89 btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
90
91 ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
92 btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
93 sol->setDeformableSolver(deformableBodySolver);
94 m_solver = sol;
95
96 m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
97 btVector3 gravity = btVector3(0, -10, 0);
98 m_dynamicsWorld->setGravity(gravity);
99 getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
100 getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
101 getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
102 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
103
104 {
105 ///create a ground
106 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150), btScalar(25.), btScalar(150)));
107
108 m_collisionShapes.push_back(groundShape);
109
110 btTransform groundTransform;
111 groundTransform.setIdentity();
112 groundTransform.setOrigin(btVector3(0, -32, 0));
113 groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
114 //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
115 btScalar mass(0.);
116
117 //rigidbody is dynamic if and only if mass is non zero, otherwise static
118 bool isDynamic = (mass != 0.f);
119
120 btVector3 localInertia(0, 0, 0);
121 if (isDynamic)
122 groundShape->calculateLocalInertia(mass, localInertia);
123
124 //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
125 btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
126 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
127 btRigidBody* body = new btRigidBody(rbInfo);
128 body->setFriction(2);
129
130 //add the ground to the dynamics world
131 m_dynamicsWorld->addRigidBody(body);
132 }
133
134 // create a piece of cloth
135 {
136 btScalar s = 4;
137 btScalar h = 0;
138
139 btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
140 btVector3(+s, h, -s),
141 btVector3(-s, h, +s),
142 btVector3(+s, h, +s),
143 20,20,
144 1 + 2 + 4 + 8, true);
145
146 psb->getCollisionShape()->setMargin(0.05);
147 psb->generateBendingConstraints(2);
148 psb->setSpringStiffness(10);
149 psb->setTotalMass(1);
150 psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
151 psb->m_cfg.kCHR = 1; // collision hardness with rigid body
152 psb->m_cfg.kDF = 0;
153 psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
154 psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
155 psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
156 getDeformableDynamicsWorld()->addSoftBody(psb);
157
158 btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true);
159 getDeformableDynamicsWorld()->addForce(psb, mass_spring);
160 m_forces.push_back(mass_spring);
161
162 btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
163 getDeformableDynamicsWorld()->addForce(psb, gravity_force);
164 m_forces.push_back(gravity_force);
165
166
167 h = 2;
168 s = 2;
169 btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
170 btVector3(+s, h, -s),
171 btVector3(-s, h, +s),
172 btVector3(+s, h, +s),
173 10,10,
174 0, true);
175 psb2->getCollisionShape()->setMargin(0.05);
176 psb2->generateBendingConstraints(2);
177 psb2->setSpringStiffness(10);
178 psb2->setTotalMass(1);
179 psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
180 psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
181 psb2->m_cfg.kDF = 0.5;
182 psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
183 psb2->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
184 psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
185 psb->translate(btVector3(3.5,0,0));
186 getDeformableDynamicsWorld()->addSoftBody(psb2);
187
188 btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true);
189 getDeformableDynamicsWorld()->addForce(psb2, mass_spring2);
190 m_forces.push_back(mass_spring2);
191
192 btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity);
193 getDeformableDynamicsWorld()->addForce(psb2, gravity_force2);
194 m_forces.push_back(gravity_force2);
195 }
196 getDeformableDynamicsWorld()->setImplicit(false);
197 getDeformableDynamicsWorld()->setLineSearch(false);
198 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
199 int numInstances = m_guiHelper->getRenderInterface()->getTotalNumInstances();
200 double rgbaColors[3][4] = { { 1, 0, 0, 1 } , { 0, 1, 0, 1 } ,{ 0, 0, 1, 1 } };
201
202 for (int i = 0; i < numInstances; i++)
203 {
204 m_guiHelper->changeInstanceFlags(i, B3_INSTANCE_DOUBLE_SIDED);
205 }
206
207 }
208
exitPhysics()209 void DeformableContact::exitPhysics()
210 {
211 //cleanup in the reverse order of creation/initialization
212 removePickingConstraint();
213 //remove the rigidbodies from the dynamics world and delete them
214 int i;
215 for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
216 {
217 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
218 btRigidBody* body = btRigidBody::upcast(obj);
219 if (body && body->getMotionState())
220 {
221 delete body->getMotionState();
222 }
223 m_dynamicsWorld->removeCollisionObject(obj);
224 delete obj;
225 }
226 // delete forces
227 for (int j = 0; j < m_forces.size(); j++)
228 {
229 btDeformableLagrangianForce* force = m_forces[j];
230 delete force;
231 }
232 m_forces.clear();
233 //delete collision shapes
234 for (int j = 0; j < m_collisionShapes.size(); j++)
235 {
236 btCollisionShape* shape = m_collisionShapes[j];
237 delete shape;
238 }
239 m_collisionShapes.clear();
240
241 delete m_dynamicsWorld;
242
243 delete m_solver;
244
245 delete m_broadphase;
246
247 delete m_dispatcher;
248
249 delete m_collisionConfiguration;
250 }
251
DeformableContactCreateFunc(struct CommonExampleOptions & options)252 class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options)
253 {
254 return new DeformableContact(options.m_guiHelper);
255 }
256
257
258