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 "Pinch.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 Pinch shows the frictional contact between kinematic rigid objects with deformable objects
29
30 struct TetraCube
31 {
32 #include "../SoftDemo/cube.inl"
33 };
34
35 class Pinch : public CommonDeformableBodyBase
36 {
37 public:
Pinch(struct GUIHelperInterface * helper)38 Pinch(struct GUIHelperInterface* helper)
39 : CommonDeformableBodyBase(helper)
40 {
41 }
~Pinch()42 virtual ~Pinch()
43 {
44 }
45 void initPhysics();
46
47 void exitPhysics();
48
resetCamera()49 void resetCamera()
50 {
51 float dist = 25;
52 float pitch = -30;
53 float yaw = 100;
54 float targetPos[3] = {0, -0, 0};
55 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
56 }
57
stepSimulation(float deltaTime)58 void stepSimulation(float deltaTime)
59 {
60 //use a smaller internal timestep, there are stability issues
61 float internalTimeStep = 1. / 240.f;
62 m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
63 }
64
createGrip()65 void createGrip()
66 {
67 int count = 2;
68 float mass = 1e6;
69 btCollisionShape* shape[] = {
70 new btBoxShape(btVector3(3, 3, 0.5)),
71 };
72 static const int nshapes = sizeof(shape) / sizeof(shape[0]);
73 for (int i = 0; i < count; ++i)
74 {
75 btTransform startTransform;
76 startTransform.setIdentity();
77 startTransform.setOrigin(btVector3(10, 0, 0));
78 startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
79 createRigidBody(mass, startTransform, shape[i % nshapes]);
80 }
81 }
82
renderScene()83 virtual void renderScene()
84 {
85 CommonDeformableBodyBase::renderScene();
86 btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
87
88 for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
89 {
90 btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
91 {
92 btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
93 btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
94 }
95 }
96 }
97 };
98
dynamics(btScalar time,btDeformableMultiBodyDynamicsWorld * world)99 void dynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
100 {
101 btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
102 if (rbs.size()<2)
103 return;
104 btRigidBody* rb0 = rbs[0];
105 btScalar pressTime = 0.9;
106 btScalar liftTime = 2.5;
107 btScalar shiftTime = 3.5;
108 btScalar holdTime = 4.5*1000;
109 btScalar dropTime = 5.3*1000;
110 btTransform rbTransform;
111 rbTransform.setIdentity();
112 btVector3 translation;
113 btVector3 velocity;
114
115 btVector3 initialTranslationLeft = btVector3(0.5,3,4);
116 btVector3 initialTranslationRight = btVector3(0.5,3,-4);
117 btVector3 pinchVelocityLeft = btVector3(0,0,-2);
118 btVector3 pinchVelocityRight = btVector3(0,0,2);
119 btVector3 liftVelocity = btVector3(0,5,0);
120 btVector3 shiftVelocity = btVector3(0,0,5);
121 btVector3 holdVelocity = btVector3(0,0,0);
122 btVector3 openVelocityLeft = btVector3(0,0,4);
123 btVector3 openVelocityRight = btVector3(0,0,-4);
124
125 if (time < pressTime)
126 {
127 velocity = pinchVelocityLeft;
128 translation = initialTranslationLeft + pinchVelocityLeft * time;
129 }
130 else if (time < liftTime)
131 {
132 velocity = liftVelocity;
133 translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
134
135 }
136 else if (time < shiftTime)
137 {
138 velocity = shiftVelocity;
139 translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
140 }
141 else if (time < holdTime)
142 {
143 velocity = btVector3(0,0,0);
144 translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
145 }
146 else if (time < dropTime)
147 {
148 velocity = openVelocityLeft;
149 translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
150 }
151 else
152 {
153 velocity = holdVelocity;
154 translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
155 }
156 rbTransform.setOrigin(translation);
157 rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
158 rb0->setCenterOfMassTransform(rbTransform);
159 rb0->setAngularVelocity(btVector3(0,0,0));
160 rb0->setLinearVelocity(velocity);
161
162 btRigidBody* rb1 = rbs[1];
163 if (time < pressTime)
164 {
165 velocity = pinchVelocityRight;
166 translation = initialTranslationRight + pinchVelocityRight * time;
167 }
168 else if (time < liftTime)
169 {
170 velocity = liftVelocity;
171 translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
172
173 }
174 else if (time < shiftTime)
175 {
176 velocity = shiftVelocity;
177 translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
178 }
179 else if (time < holdTime)
180 {
181 velocity = btVector3(0,0,0);
182 translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
183 }
184 else if (time < dropTime)
185 {
186 velocity = openVelocityRight;
187 translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
188 }
189 else
190 {
191 velocity = holdVelocity;
192 translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
193 }
194 rbTransform.setOrigin(translation);
195 rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
196 rb1->setCenterOfMassTransform(rbTransform);
197 rb1->setAngularVelocity(btVector3(0,0,0));
198 rb1->setLinearVelocity(velocity);
199
200 rb0->setFriction(20);
201 rb1->setFriction(20);
202 }
203
initPhysics()204 void Pinch::initPhysics()
205 {
206 m_guiHelper->setUpAxis(1);
207
208 m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
209
210 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
211
212 m_broadphase = new btDbvtBroadphase();
213 btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
214
215 btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
216 sol->setDeformableSolver(deformableBodySolver);
217 m_solver = sol;
218
219 m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
220 btVector3 gravity = btVector3(0, -10, 0);
221 m_dynamicsWorld->setGravity(gravity);
222 getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
223 getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
224 getDeformableDynamicsWorld()->setSolverCallback(dynamics);
225 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
226
227 //create a ground
228 {
229 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
230
231 m_collisionShapes.push_back(groundShape);
232
233 btTransform groundTransform;
234 groundTransform.setIdentity();
235 groundTransform.setOrigin(btVector3(0, -25, 0));
236 groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
237 //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
238 btScalar mass(0.);
239
240 //rigidbody is dynamic if and only if mass is non zero, otherwise static
241 bool isDynamic = (mass != 0.f);
242
243 btVector3 localInertia(0, 0, 0);
244 if (isDynamic)
245 groundShape->calculateLocalInertia(mass, localInertia);
246
247 //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
248 btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
249 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
250 btRigidBody* body = new btRigidBody(rbInfo);
251 body->setFriction(0.5);
252
253 //add the ground to the dynamics world
254 m_dynamicsWorld->addRigidBody(body);
255 }
256
257 // create a soft block
258 {
259 btScalar verts[24] = {0.f, 0.f, 0.f,
260 1.f, 0.f, 0.f,
261 0.f, 1.f, 0.f,
262 0.f, 0.f, 1.f,
263 1.f, 1.f, 0.f,
264 0.f, 1.f, 1.f,
265 1.f, 0.f, 1.f,
266 1.f, 1.f, 1.f
267 };
268 int triangles[60] = {0, 6, 3,
269 0,1,6,
270 7,5,3,
271 7,3,6,
272 4,7,6,
273 4,6,1,
274 7,2,5,
275 7,4,2,
276 0,3,2,
277 2,3,5,
278 0,2,4,
279 0,4,1,
280 0,6,5,
281 0,6,4,
282 3,4,2,
283 3,4,7,
284 2,7,3,
285 2,7,1,
286 4,5,0,
287 4,5,6,
288 };
289 // btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
290 ////
291 btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
292 TetraCube::getElements(),
293 0,
294 TetraCube::getNodes(),
295 false, true, true);
296
297 psb->scale(btVector3(2, 2, 2));
298 psb->translate(btVector3(0, 4, 0));
299 psb->getCollisionShape()->setMargin(0.01);
300 psb->setTotalMass(1);
301 psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
302 psb->m_cfg.kCHR = 1; // collision hardness with rigid body
303 psb->m_cfg.kDF = .5;
304 psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
305 psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
306 getDeformableDynamicsWorld()->addSoftBody(psb);
307 btSoftBodyHelpers::generateBoundaryFaces(psb);
308
309 btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
310 getDeformableDynamicsWorld()->addForce(psb, gravity_force);
311 m_forces.push_back(gravity_force);
312
313 btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,3, 0.02);
314 neohookean->setPoissonRatio(0.3);
315 neohookean->setYoungsModulus(25);
316 neohookean->setDamping(0.01);
317 psb->m_cfg.drag = 0.001;
318 getDeformableDynamicsWorld()->addForce(psb, neohookean);
319 m_forces.push_back(neohookean);
320 // add a grippers
321 createGrip();
322 }
323 getDeformableDynamicsWorld()->setImplicit(false);
324 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
325 }
326
exitPhysics()327 void Pinch::exitPhysics()
328 {
329 //cleanup in the reverse order of creation/initialization
330 removePickingConstraint();
331 //remove the rigidbodies from the dynamics world and delete them
332 int i;
333 for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
334 {
335 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
336 btRigidBody* body = btRigidBody::upcast(obj);
337 if (body && body->getMotionState())
338 {
339 delete body->getMotionState();
340 }
341 m_dynamicsWorld->removeCollisionObject(obj);
342 delete obj;
343 }
344 // delete forces
345 for (int j = 0; j < m_forces.size(); j++)
346 {
347 btDeformableLagrangianForce* force = m_forces[j];
348 delete force;
349 }
350 m_forces.clear();
351 //delete collision shapes
352 for (int j = 0; j < m_collisionShapes.size(); j++)
353 {
354 btCollisionShape* shape = m_collisionShapes[j];
355 delete shape;
356 }
357 m_collisionShapes.clear();
358
359 delete m_dynamicsWorld;
360
361 delete m_solver;
362
363 delete m_broadphase;
364
365 delete m_dispatcher;
366
367 delete m_collisionConfiguration;
368 }
369
370
371
PinchCreateFunc(struct CommonExampleOptions & options)372 class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options)
373 {
374 return new Pinch(options.m_guiHelper);
375 }
376
377
378