mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
448 lines
16 KiB
C++
448 lines
16 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
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.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "MultiPendulum.h"
|
|
|
|
#include <cmath>
|
|
#include <iterator>
|
|
#include <vector> // TODO: Should I use another data structure?
|
|
|
|
#include "btBulletDynamicsCommon.h"
|
|
#include "LinearMath/btVector3.h"
|
|
#include "LinearMath/btAlignedObjectArray.h"
|
|
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
|
|
|
static btScalar gPendulaQty = 2; //TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
|
|
|
|
static btScalar gDisplacedPendula = 1; //TODO: This is an int as well
|
|
|
|
static btScalar gPendulaRestitution = 1; // Default pendulum restitution is 1 to restore all force
|
|
|
|
static btScalar gSphereRadius = 1; // The sphere radius
|
|
|
|
static btScalar gCurrentPendulumLength = 8;
|
|
|
|
static btScalar gInitialPendulumLength = 8; // Default pendulum length (distance between two spheres)
|
|
|
|
static btScalar gDisplacementForce = 30; // The default force with which we move the pendulum
|
|
|
|
static btScalar gForceScalar = 0; // default force scalar to apply a displacement
|
|
|
|
struct MultiPendulumExample : public CommonRigidBodyBase
|
|
{
|
|
MultiPendulumExample(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper)
|
|
{
|
|
}
|
|
|
|
virtual ~MultiPendulumExample()
|
|
{
|
|
}
|
|
|
|
virtual void initPhysics(); // build a multi pendulum
|
|
virtual void renderScene(); // render the scene to screen
|
|
virtual void createMultiPendulum(btSphereShape* colShape, btScalar pendulaQty, const btVector3& position, btScalar length, btScalar mass); // create a multi pendulum at the indicated x and y position, the specified number of pendula formed into a chain, each with indicated length and mass
|
|
virtual void changePendulaLength(btScalar length); // change the pendulum length
|
|
virtual void changePendulaRestitution(btScalar restitution); // change the pendula restitution
|
|
virtual void stepSimulation(float deltaTime); // step the simulation
|
|
virtual bool keyboardCallback(int key, int state); // handle keyboard callbacks
|
|
virtual void applyPendulumForce(btScalar pendulumForce);
|
|
void resetCamera()
|
|
{
|
|
float dist = 41;
|
|
float pitch = -35;
|
|
float yaw = 52;
|
|
float targetPos[3] = {0, 0.46, 0};
|
|
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1],
|
|
targetPos[2]);
|
|
}
|
|
|
|
std::vector<btSliderConstraint*> constraints; // keep a handle to the slider constraints
|
|
std::vector<btRigidBody*> pendula; // keep a handle to the pendula
|
|
};
|
|
|
|
static MultiPendulumExample* mex = NULL; // Handle to the example to access it via functions. Do not use this in your simulation!
|
|
|
|
void onMultiPendulaLengthChanged(float pendulaLength, void*); // Change the pendula length
|
|
|
|
void onMultiPendulaRestitutionChanged(float pendulaRestitution, void*); // change the pendula restitution
|
|
|
|
void applyMForceWithForceScalar(float forceScalar);
|
|
|
|
void MultiPendulumExample::initPhysics()
|
|
{ // Setup your physics scene
|
|
|
|
{ // create a slider to change the number of pendula
|
|
SliderParams slider("Number of Pendula", &gPendulaQty);
|
|
slider.m_minVal = 1;
|
|
slider.m_maxVal = 50;
|
|
slider.m_clampToIntegers = true;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
{ // create a slider to change the number of displaced pendula
|
|
SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
|
|
slider.m_minVal = 0;
|
|
slider.m_maxVal = 49;
|
|
slider.m_clampToIntegers = true;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
{ // create a slider to change the pendula restitution
|
|
SliderParams slider("Pendula Restitution", &gPendulaRestitution);
|
|
slider.m_minVal = 0;
|
|
slider.m_maxVal = 1;
|
|
slider.m_clampToNotches = false;
|
|
slider.m_callback = onMultiPendulaRestitutionChanged;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
{ // create a slider to change the pendulum length
|
|
SliderParams slider("Pendula Length", &gCurrentPendulumLength);
|
|
slider.m_minVal = 0;
|
|
slider.m_maxVal = 49;
|
|
slider.m_clampToNotches = false;
|
|
slider.m_callback = onMultiPendulaLengthChanged;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
{ // create a slider to change the force to displace the lowest pendulum
|
|
SliderParams slider("Displacement force", &gDisplacementForce);
|
|
slider.m_minVal = 0.1;
|
|
slider.m_maxVal = 200;
|
|
slider.m_clampToNotches = false;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
{ // create a slider to apply the force by slider
|
|
SliderParams slider("Apply displacement force", &gForceScalar);
|
|
slider.m_minVal = -1;
|
|
slider.m_maxVal = 1;
|
|
slider.m_clampToNotches = false;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
|
|
slider);
|
|
}
|
|
|
|
m_guiHelper->setUpAxis(1);
|
|
|
|
createEmptyDynamicsWorld();
|
|
|
|
// create a debug drawer
|
|
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
|
if (m_dynamicsWorld->getDebugDrawer())
|
|
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
|
btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits);
|
|
|
|
{ // create the multipendulum starting at the indicated position below and where each pendulum has the following mass
|
|
btScalar pendulumMass(1.f);
|
|
|
|
btVector3 position(0.0f, 15.0f, 0.0f); // initial top-most pendulum position
|
|
|
|
// Re-using the same collision is better for memory usage and performance
|
|
btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
|
|
m_collisionShapes.push_back(pendulumShape);
|
|
|
|
// create multi-pendulum
|
|
createMultiPendulum(pendulumShape, std::floor(gPendulaQty), position,
|
|
gInitialPendulumLength, pendulumMass);
|
|
}
|
|
|
|
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
|
}
|
|
|
|
void MultiPendulumExample::stepSimulation(float deltaTime)
|
|
{
|
|
applyMForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
|
|
|
|
if (m_dynamicsWorld)
|
|
{
|
|
m_dynamicsWorld->stepSimulation(deltaTime);
|
|
}
|
|
}
|
|
|
|
void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape,
|
|
btScalar pendulaQty, const btVector3& position,
|
|
btScalar length, btScalar mass)
|
|
{
|
|
// The multi-pendulum looks like this (names when built):
|
|
//..........0......./.......1...../.......2......./..etc...:pendulum build iterations
|
|
// O parentSphere
|
|
// |
|
|
// O childSphere / parentSphere
|
|
// |
|
|
// O ............./ childSphere / parentSphere
|
|
// |
|
|
// O .........................../ childSphere
|
|
// etc.
|
|
|
|
//create the top element of the pendulum
|
|
btTransform startTransform;
|
|
startTransform.setIdentity();
|
|
|
|
// position the top sphere
|
|
startTransform.setOrigin(position);
|
|
|
|
startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
|
|
|
|
btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape);
|
|
|
|
// disable the deactivation when object does not move anymore
|
|
topSphere->setActivationState(DISABLE_DEACTIVATION);
|
|
|
|
//make top sphere position "fixed" in the world by attaching it to a the world with a point to point constraint
|
|
// The pivot is defined in the reference frame of topSphere, so the attachment should be exactly at the center of topSphere
|
|
btVector3 constraintPivot(0.0f, 0.0f, 0.0f);
|
|
btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(
|
|
*topSphere, constraintPivot);
|
|
|
|
p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
|
|
|
|
// add the constraint to the world
|
|
m_dynamicsWorld->addConstraint(p2pconst, true);
|
|
|
|
btRigidBody* parentSphere = topSphere; // set the top sphere as the parent sphere for the next sphere to be created
|
|
|
|
for (int i = 0; i < pendulaQty; i++)
|
|
{ // produce the number of pendula
|
|
|
|
// create joint element to make the pendulum rotate it
|
|
|
|
// position the joint sphere at the same position as the top sphere
|
|
startTransform.setOrigin(position - btVector3(0, length * (i), 0));
|
|
|
|
startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
|
|
|
|
btRigidBody* jointSphere = createRigidBody(mass, startTransform,
|
|
colShape);
|
|
jointSphere->setFriction(0); // we do not need friction here
|
|
|
|
// disable the deactivation when object does not move anymore
|
|
jointSphere->setActivationState(DISABLE_DEACTIVATION);
|
|
|
|
//create constraint between parentSphere and jointSphere
|
|
// this is represented by the constraint pivot in the local frames of reference of both constrained spheres
|
|
btTransform constraintPivotInParentSphereRF, constraintPivotInJointSphereRF;
|
|
|
|
constraintPivotInParentSphereRF.setIdentity();
|
|
constraintPivotInJointSphereRF.setIdentity();
|
|
|
|
// the orientation of a point-to-point constraint does not matter, as is has no rotational limits
|
|
|
|
//Obtain the position of parentSphere in local reference frame of the jointSphere (the pivot is therefore in the center of parentSphere)
|
|
btVector3 parentSphereInJointSphereRF =
|
|
(jointSphere->getWorldTransform().inverse()(
|
|
parentSphere->getWorldTransform().getOrigin()));
|
|
constraintPivotInJointSphereRF.setOrigin(parentSphereInJointSphereRF);
|
|
|
|
btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(
|
|
*parentSphere, *jointSphere, constraintPivotInParentSphereRF.getOrigin(), constraintPivotInJointSphereRF.getOrigin());
|
|
|
|
p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
|
|
|
|
// add the constraint to the world
|
|
m_dynamicsWorld->addConstraint(p2pconst, true);
|
|
|
|
// create a slider constraint to change the length of the pendula while it swings
|
|
|
|
startTransform.setIdentity(); // reset start transform
|
|
|
|
// position the child sphere below the joint sphere
|
|
startTransform.setOrigin(position - btVector3(0, length * (i + 1), 0));
|
|
|
|
startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
|
|
|
|
btRigidBody* childSphere = createRigidBody(mass, startTransform,
|
|
colShape);
|
|
childSphere->setFriction(0); // we do not need friction here
|
|
pendula.push_back(childSphere);
|
|
|
|
// disable the deactivation when object does not move anymore
|
|
childSphere->setActivationState(DISABLE_DEACTIVATION);
|
|
|
|
//create slider constraint between jointSphere and childSphere
|
|
// this is represented by the constraint pivot in the local frames of reference of both constrained spheres
|
|
// furthermore we need to rotate the constraint appropriately to orient it correctly in space
|
|
btTransform constraintPivotInChildSphereRF;
|
|
|
|
constraintPivotInJointSphereRF.setIdentity();
|
|
constraintPivotInChildSphereRF.setIdentity();
|
|
|
|
// the orientation of a point-to-point constraint does not matter, as is has no rotational limits
|
|
|
|
//Obtain the position of jointSphere in local reference frame of the childSphere (the pivot is therefore in the center of jointSphere)
|
|
btVector3 jointSphereInChildSphereRF =
|
|
(childSphere->getWorldTransform().inverse()(
|
|
jointSphere->getWorldTransform().getOrigin()));
|
|
constraintPivotInChildSphereRF.setOrigin(jointSphereInChildSphereRF);
|
|
|
|
// the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it
|
|
btQuaternion qt;
|
|
qt.setEuler(0, 0, -SIMD_HALF_PI);
|
|
constraintPivotInJointSphereRF.setRotation(qt); //we use Y like up Axis
|
|
constraintPivotInChildSphereRF.setRotation(qt); //we use Y like up Axis
|
|
|
|
btSliderConstraint* sliderConst = new btSliderConstraint(*jointSphere,
|
|
*childSphere, constraintPivotInJointSphereRF, constraintPivotInChildSphereRF, true);
|
|
|
|
sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
|
|
|
|
// set limits
|
|
// the initial setup of the constraint defines the origins of the limit dimensions,
|
|
// therefore we set both limits directly to the current position of the parentSphere
|
|
sliderConst->setLowerLinLimit(btScalar(0));
|
|
sliderConst->setUpperLinLimit(btScalar(0));
|
|
sliderConst->setLowerAngLimit(btScalar(0));
|
|
sliderConst->setUpperAngLimit(btScalar(0));
|
|
constraints.push_back(sliderConst);
|
|
|
|
// add the constraint to the world
|
|
m_dynamicsWorld->addConstraint(sliderConst, true);
|
|
parentSphere = childSphere;
|
|
}
|
|
}
|
|
|
|
void MultiPendulumExample::changePendulaLength(btScalar length)
|
|
{
|
|
btScalar lowerLimit = -gInitialPendulumLength;
|
|
for (std::vector<btSliderConstraint*>::iterator sit = constraints.begin();
|
|
sit != constraints.end(); sit++)
|
|
{
|
|
btAssert((*sit) && "Null constraint");
|
|
|
|
// if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one
|
|
if (lowerLimit <= length)
|
|
{
|
|
(*sit)->setLowerLinLimit(length + lowerLimit);
|
|
(*sit)->setUpperLinLimit(length + lowerLimit);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MultiPendulumExample::changePendulaRestitution(btScalar restitution)
|
|
{
|
|
for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
|
|
rit != pendula.end(); rit++)
|
|
{
|
|
btAssert((*rit) && "Null constraint");
|
|
|
|
(*rit)->setRestitution(restitution);
|
|
}
|
|
}
|
|
|
|
void MultiPendulumExample::renderScene()
|
|
{
|
|
CommonRigidBodyBase::renderScene();
|
|
}
|
|
|
|
bool MultiPendulumExample::keyboardCallback(int key, int state)
|
|
{
|
|
//b3Printf("Key pressed: %d in state %d \n",key,state);
|
|
|
|
//key 1, key 2, key 3
|
|
switch (key)
|
|
{
|
|
case '1' /*ASCII for 1*/:
|
|
{
|
|
//assumption: Sphere are aligned in Z axis
|
|
btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1);
|
|
|
|
changePendulaLength(newLimit);
|
|
gCurrentPendulumLength = newLimit;
|
|
|
|
b3Printf("Increase pendulum length to %f", gCurrentPendulumLength);
|
|
return true;
|
|
}
|
|
case '2' /*ASCII for 2*/:
|
|
{
|
|
//assumption: Sphere are aligned in Z axis
|
|
btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1);
|
|
|
|
//is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one
|
|
if (0 <= newLimit)
|
|
{
|
|
changePendulaLength(newLimit);
|
|
gCurrentPendulumLength = newLimit;
|
|
}
|
|
|
|
b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength);
|
|
return true;
|
|
}
|
|
case '3' /*ASCII for 3*/:
|
|
{
|
|
applyPendulumForce(gDisplacementForce);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce)
|
|
{
|
|
if (pendulumForce != 0)
|
|
{
|
|
b3Printf("Apply %f to pendulum", pendulumForce);
|
|
for (int i = 0; i < gDisplacedPendula; i++)
|
|
{
|
|
if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
|
|
pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
|
|
}
|
|
}
|
|
}
|
|
|
|
// GUI parameter modifiers
|
|
|
|
void onMultiPendulaLengthChanged(float pendulaLength, void*)
|
|
{ // Change the pendula length
|
|
if (mex)
|
|
{
|
|
mex->changePendulaLength(pendulaLength);
|
|
}
|
|
//b3Printf("Pendula length changed to %f \n",sliderValue );
|
|
}
|
|
|
|
void onMultiPendulaRestitutionChanged(float pendulaRestitution, void*)
|
|
{ // change the pendula restitution
|
|
if (mex)
|
|
{
|
|
mex->changePendulaRestitution(pendulaRestitution);
|
|
}
|
|
}
|
|
|
|
void applyMForceWithForceScalar(float forceScalar)
|
|
{
|
|
if (mex)
|
|
{
|
|
btScalar appliedForce = forceScalar * gDisplacementForce;
|
|
|
|
if (fabs(gForceScalar) < 0.2f)
|
|
gForceScalar = 0;
|
|
|
|
mex->applyPendulumForce(appliedForce);
|
|
}
|
|
}
|
|
|
|
CommonExampleInterface* ET_MultiPendulumCreateFunc(
|
|
CommonExampleOptions& options)
|
|
{
|
|
mex = new MultiPendulumExample(options.m_guiHelper);
|
|
return mex;
|
|
}
|