1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2015 Google Inc. http://bulletphysics.org
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
16 #include "NewtonsCradle.h"
17
18 #include <cmath>
19 #include <iterator>
20 #include <vector> // TODO: Should I use another data structure?
21
22 #include "btBulletDynamicsCommon.h"
23 #include "LinearMath/btVector3.h"
24 #include "LinearMath/btAlignedObjectArray.h"
25 #include "../CommonInterfaces/CommonRigidBodyBase.h"
26 #include "../CommonInterfaces/CommonParameterInterface.h"
27
28 static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle
29 //TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
30
31 static btScalar gDisplacedPendula = 1; // number of displaced pendula
32 //TODO: This is an int as well
33
34 static btScalar gPendulaRestitution = 1; // pendula restitution when hitting against each other
35
36 static btScalar gSphereRadius = 1; // pendula radius
37
38 static btScalar gCurrentPendulumLength = 8; // current pendula length
39
40 static btScalar gInitialPendulumLength = 8; // default pendula length
41
42 static btScalar gDisplacementForce = 30; // default force to displace the pendula
43
44 static btScalar gForceScalar = 0; // default force scalar to apply a displacement
45
46 struct NewtonsCradleExample : public CommonRigidBodyBase
47 {
NewtonsCradleExampleNewtonsCradleExample48 NewtonsCradleExample(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper)
49 {
50 }
~NewtonsCradleExampleNewtonsCradleExample51 virtual ~NewtonsCradleExample()
52 {
53 }
54 virtual void initPhysics();
55 virtual void renderScene();
56 virtual void createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass);
57 virtual void changePendulaLength(btScalar length);
58 virtual void changePendulaRestitution(btScalar restitution);
59 virtual void stepSimulation(float deltaTime);
60 virtual bool keyboardCallback(int key, int state);
61 virtual void applyPendulumForce(btScalar pendulumForce);
resetCameraNewtonsCradleExample62 void resetCamera()
63 {
64 float dist = 41;
65 float pitch = -35;
66 float yaw = 52;
67 float targetPos[3] = {0, 0.46, 0};
68 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1],
69 targetPos[2]);
70 }
71
72 std::vector<btSliderConstraint*> constraints; // keep a handle to the slider constraints
73 std::vector<btRigidBody*> pendula; // keep a handle to the pendula
74 };
75
76 static NewtonsCradleExample* nex = NULL;
77
78 void onPendulaLengthChanged(float pendulaLength, void* userPtr); // Change the pendula length
79
80 void onPendulaRestitutionChanged(float pendulaRestitution, void* userPtr); // change the pendula restitution
81
82 void applyForceWithForceScalar(float forceScalar);
83
initPhysics()84 void NewtonsCradleExample::initPhysics()
85 {
86 { // create a slider to change the number of pendula
87 SliderParams slider("Number of Pendula", &gPendulaQty);
88 slider.m_minVal = 1;
89 slider.m_maxVal = 50;
90 slider.m_clampToIntegers = true;
91 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
92 slider);
93 }
94
95 { // create a slider to change the number of displaced pendula
96 SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
97 slider.m_minVal = 0;
98 slider.m_maxVal = 49;
99 slider.m_clampToIntegers = true;
100 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
101 slider);
102 }
103
104 { // create a slider to change the pendula restitution
105 SliderParams slider("Pendula Restitution", &gPendulaRestitution);
106 slider.m_minVal = 0;
107 slider.m_maxVal = 1;
108 slider.m_clampToNotches = false;
109 slider.m_callback = onPendulaRestitutionChanged;
110 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
111 slider);
112 }
113
114 { // create a slider to change the pendulum length
115 SliderParams slider("Pendula Length", &gCurrentPendulumLength);
116 slider.m_minVal = 0;
117 slider.m_maxVal = 49;
118 slider.m_clampToNotches = false;
119 slider.m_callback = onPendulaLengthChanged;
120 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
121 slider);
122 }
123
124 { // create a slider to change the force to displace the lowest pendulum
125 SliderParams slider("Displacement force", &gDisplacementForce);
126 slider.m_minVal = 0.1;
127 slider.m_maxVal = 200;
128 slider.m_clampToNotches = false;
129 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
130 slider);
131 }
132
133 { // create a slider to apply the force by slider
134 SliderParams slider("Apply displacement force", &gForceScalar);
135 slider.m_minVal = -1;
136 slider.m_maxVal = 1;
137 slider.m_clampToNotches = false;
138 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
139 slider);
140 }
141
142 m_guiHelper->setUpAxis(1);
143
144 createEmptyDynamicsWorld();
145
146 // create a debug drawer
147 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
148 if (m_dynamicsWorld->getDebugDrawer())
149 m_dynamicsWorld->getDebugDrawer()->setDebugMode(
150 btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits);
151
152 { // create the pendula starting at the indicated position below and where each pendulum has the following mass
153 btScalar pendulumMass(1.f);
154
155 btVector3 position(0.0f, 15.0f, 0.0f); // initial left-most pendulum position
156 btQuaternion orientation(0, 0, 0, 1); // orientation of the pendula
157
158 // Re-using the same collision is better for memory usage and performance
159 btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
160 m_collisionShapes.push_back(pendulumShape);
161
162 for (int i = 0; i < std::floor(gPendulaQty); i++)
163 {
164 // create pendulum
165 createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass);
166
167 // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between
168 position.setX(position.x() - 2.1f * gSphereRadius);
169 }
170 }
171
172 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
173 }
174
stepSimulation(float deltaTime)175 void NewtonsCradleExample::stepSimulation(float deltaTime)
176 {
177 applyForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
178
179 if (m_dynamicsWorld)
180 {
181 m_dynamicsWorld->stepSimulation(deltaTime);
182 }
183 }
184
createPendulum(btSphereShape * colShape,const btVector3 & position,btScalar length,btScalar mass)185 void NewtonsCradleExample::createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass)
186 {
187 // The pendulum looks like this (names when built):
188 // O topSphere
189 // |
190 // O bottomSphere
191
192 //create a dynamic pendulum
193 btTransform startTransform;
194 startTransform.setIdentity();
195
196 // position the top sphere above ground with a moving x position
197 startTransform.setOrigin(position);
198 startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
199 btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape);
200
201 // position the bottom sphere below the top sphere
202 startTransform.setOrigin(
203 btVector3(position.x(), btScalar(position.y() - length),
204 position.z()));
205
206 startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
207 btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape);
208 bottomSphere->setFriction(0); // we do not need friction here
209 pendula.push_back(bottomSphere);
210
211 // disable the deactivation when objects do not move anymore
212 topSphere->setActivationState(DISABLE_DEACTIVATION);
213 bottomSphere->setActivationState(DISABLE_DEACTIVATION);
214
215 bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution
216
217 //make the top sphere position "fixed" to the world by attaching with a point to point constraint
218 // The pivot is defined in the reference frame of topSphere, so the attachment is exactly at the center of the topSphere
219 btVector3 constraintPivot(btVector3(0.0f, 0.0f, 0.0f));
220 btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(*topSphere,
221 constraintPivot);
222
223 p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
224
225 // add the constraint to the world
226 m_dynamicsWorld->addConstraint(p2pconst, true);
227
228 //create constraint between spheres
229 // this is represented by the constraint pivot in the local frames of reference of both constrained spheres
230 // furthermore we need to rotate the constraint appropriately to orient it correctly in space
231 btTransform constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF;
232
233 constraintPivotInTopSphereRF.setIdentity();
234 constraintPivotInBottomSphereRF.setIdentity();
235
236 // the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it
237 btQuaternion qt;
238 qt.setEuler(0, 0, -SIMD_HALF_PI);
239 constraintPivotInTopSphereRF.setRotation(qt); //we use Y like up Axis
240 constraintPivotInBottomSphereRF.setRotation(qt); //we use Y like up Axis
241
242 //Obtain the position of topSphere in local reference frame of bottomSphere (the pivot is therefore in the center of topSphere)
243 btVector3 topSphereInBottomSphereRF =
244 (bottomSphere->getWorldTransform().inverse()(
245 topSphere->getWorldTransform().getOrigin()));
246 constraintPivotInBottomSphereRF.setOrigin(topSphereInBottomSphereRF);
247
248 btSliderConstraint* sliderConst = new btSliderConstraint(*topSphere,
249 *bottomSphere, constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF, true);
250
251 sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
252
253 // set limits
254 // the initial setup of the constraint defines the origins of the limit dimensions,
255 // therefore we set both limits directly to the current position of the topSphere
256 sliderConst->setLowerLinLimit(btScalar(0));
257 sliderConst->setUpperLinLimit(btScalar(0));
258 sliderConst->setLowerAngLimit(btScalar(0));
259 sliderConst->setUpperAngLimit(btScalar(0));
260 constraints.push_back(sliderConst);
261
262 // add the constraint to the world
263 m_dynamicsWorld->addConstraint(sliderConst, true);
264 }
265
changePendulaLength(btScalar length)266 void NewtonsCradleExample::changePendulaLength(btScalar length)
267 {
268 btScalar lowerLimit = -gInitialPendulumLength;
269 for (std::vector<btSliderConstraint*>::iterator sit = constraints.begin();
270 sit != constraints.end(); sit++)
271 {
272 btAssert((*sit) && "Null constraint");
273
274 //if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one
275 if (lowerLimit <= length)
276 {
277 (*sit)->setLowerLinLimit(length + lowerLimit);
278 (*sit)->setUpperLinLimit(length + lowerLimit);
279 }
280 }
281 }
282
changePendulaRestitution(btScalar restitution)283 void NewtonsCradleExample::changePendulaRestitution(btScalar restitution)
284 {
285 for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
286 rit != pendula.end(); rit++)
287 {
288 btAssert((*rit) && "Null constraint");
289
290 (*rit)->setRestitution(restitution);
291 }
292 }
293
renderScene()294 void NewtonsCradleExample::renderScene()
295 {
296 CommonRigidBodyBase::renderScene();
297 }
298
keyboardCallback(int key,int state)299 bool NewtonsCradleExample::keyboardCallback(int key, int state)
300 {
301 //b3Printf("Key pressed: %d in state %d \n",key,state);
302
303 //key 1, key 2, key 3
304 switch (key)
305 {
306 case '1' /*ASCII for 1*/:
307 {
308 //assumption: Sphere are aligned in Z axis
309 btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1);
310
311 changePendulaLength(newLimit);
312 gCurrentPendulumLength = newLimit;
313
314 b3Printf("Increase pendulum length to %f", gCurrentPendulumLength);
315 return true;
316 }
317 case '2' /*ASCII for 2*/:
318 {
319 //assumption: Sphere are aligned in Z axis
320 btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1);
321
322 //is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one
323 if (0 <= newLimit)
324 {
325 changePendulaLength(newLimit);
326 gCurrentPendulumLength = newLimit;
327 }
328
329 b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength);
330 return true;
331 }
332 case '3' /*ASCII for 3*/:
333 {
334 applyPendulumForce(gDisplacementForce);
335 return true;
336 }
337 }
338
339 return false;
340 }
341
applyPendulumForce(btScalar pendulumForce)342 void NewtonsCradleExample::applyPendulumForce(btScalar pendulumForce)
343 {
344 if (pendulumForce != 0)
345 {
346 b3Printf("Apply %f to pendulum", pendulumForce);
347 for (int i = 0; i < gDisplacedPendula; i++)
348 {
349 if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
350 pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
351 }
352 }
353 }
354
355 // GUI parameter modifiers
356
onPendulaLengthChanged(float pendulaLength,void *)357 void onPendulaLengthChanged(float pendulaLength, void*)
358 {
359 if (nex)
360 {
361 nex->changePendulaLength(pendulaLength);
362 //b3Printf("Pendula length changed to %f \n",sliderValue );
363 }
364 }
365
onPendulaRestitutionChanged(float pendulaRestitution,void *)366 void onPendulaRestitutionChanged(float pendulaRestitution, void*)
367 {
368 if (nex)
369 {
370 nex->changePendulaRestitution(pendulaRestitution);
371 }
372 }
373
applyForceWithForceScalar(float forceScalar)374 void applyForceWithForceScalar(float forceScalar)
375 {
376 if (nex)
377 {
378 btScalar appliedForce = forceScalar * gDisplacementForce;
379
380 if (fabs(gForceScalar) < 0.2f)
381 gForceScalar = 0;
382
383 nex->applyPendulumForce(appliedForce);
384 }
385 }
386
ET_NewtonsCradleCreateFunc(CommonExampleOptions & options)387 CommonExampleInterface* ET_NewtonsCradleCreateFunc(
388 CommonExampleOptions& options)
389 {
390 nex = new NewtonsCradleExample(options.m_guiHelper);
391 return nex;
392 }
393