MATLAB: Bullet: The following is my implementation file for my spring class:
Code: Select all
#include "spring.h"
spring::spring( )
{
setDynamicMass( (new btScalar(1.0f)) );
setDirectionOfMotion( 0 );
setSpringConstant( (new btScalar(30.0f)) );
setDampingConstant( (new btScalar(0.01f)) );
setSpringTravel( (new btScalar(10.0f)), (new btScalar(7.0f)) );
offsetFromEqualibrium = new btScalar( -5.0f );
initializeStaticBody( );
initializeDynamicBody( );
initializeSpringConstraint( );
}
spring::spring( btScalar* springConstant, btScalar* dampingConstant,
btScalar* restLength, btScalar* maxDisplacement,
btScalar* dynamicMass, int directionOfMotion )
{
setDynamicMass( dynamicMass );
setDirectionOfMotion( directionOfMotion );
setSpringConstant( springConstant );
setDampingConstant( dampingConstant );
setSpringTravel( restLength, maxDisplacement );
initializeStaticBody( );
initializeDynamicBody( );
initializeSpringConstraint( );
}
spring::~spring( )
{
delete staticBaseShape;
delete translationalMassShape;
delete staticBaseMotionState;
delete translationalMassMotionState;
delete staticBaseBody;
delete translationalMassBody;
delete springConstraint;
delete frameInA;
delete frameInB;
delete springConstant;
delete dampingConstant;
delete restLength;
delete maxDisplacement;
delete dynamicMass;
}
void spring::initializeStaticBody( )
{
frameInA = new btTransform( btTransform::getIdentity() );
staticBaseShape = new btBoxShape( btVector3(0.5f, 0.5f, 0.5f) );
staticBaseMotionState = new btDefaultMotionState( btTransform(
btQuaternion( 0, 0, 0, 1 ),
btVector3( 0.0f, 1.0f, 0.0f ) ) );
btRigidBody::btRigidBodyConstructionInfo staticBodyCI( 0,
staticBaseMotionState, staticBaseShape,
btVector3( 0, 0, 0 ) );
staticBaseBody = new btRigidBody( staticBodyCI );
staticBaseBody->setActivationState( DISABLE_DEACTIVATION );
staticBaseBody->getMotionState( )->getWorldTransform( *frameInA );
}
void spring::initializeDynamicBody( )
{
frameInB = new btTransform( btTransform::getIdentity() );
translationalMassShape = new btBoxShape( btVector3(0.5f, 0.5f, 0.5f) );
translationalMassMotionState = new btDefaultMotionState( btTransform(
btQuaternion( 0, 0, 0, 1 ),
btVector3( *restLength -
*offsetFromEqualibrium, 1.0f, 0.0f ) ) );
btRigidBody::btRigidBodyConstructionInfo translationalMassBodyCI(
*dynamicMass, translationalMassMotionState,
translationalMassShape,
btVector3( 0, 0, 0 ) );
translationalMassBody = new btRigidBody( translationalMassBodyCI );
translationalMassBody->setActivationState( DISABLE_DEACTIVATION );
translationalMassBody->getMotionState( )->getWorldTransform( *frameInB );
}
void spring::initializeSpringConstraint( )
{
springConstraint = new btGeneric6DofSpringConstraint( *staticBaseBody,
*translationalMassBody, *frameInA,
*frameInB, true );
springConstraint->setLinearUpperLimit( btVector3( *restLength + *maxDisplacement, 0., 0.) );
springConstraint->setLinearLowerLimit( btVector3( *restLength - *maxDisplacement, 0., 0. ) );
springConstraint->setAngularLowerLimit( btVector3( 0.f, 0.f, 0.f ) );
springConstraint->setAngularUpperLimit( btVector3( 0.f, 0.f, 0.f ) );
springConstraint->enableSpring( 0, true );
// period 1 sec for !kG body
springConstraint->setStiffness( 0, *springConstant );
// add some damping
springConstraint->setDamping( 0, *dampingConstant );
// constraint force mixing prevents "locking" on limits, might be bad
springConstraint->setParam( BT_CONSTRAINT_STOP_CFM, 1.0e-5f, 5 );
// Set the cfm so the spring behaves... doesn't have any effect (seemingly)
//springConstraint->setParam( BT_CONSTRAINT_CFM, 1.0/((1/60.)*(*springConstant)+(*dampingConstant)),0 );
// set current position as equilibrium point
springConstraint->setEquilibriumPoint( );
}
void spring::setDynamicMass( btScalar* dynamicMass )
{
this->dynamicMass = dynamicMass;
}
void spring::setSpringConstant( btScalar* springConstant )
{
this->springConstant = springConstant;
}
void spring::setDampingConstant( btScalar* dampingConstant )
{
this->dampingConstant = dampingConstant;
}
void spring::setSpringTravel( btScalar* restLength,
btScalar* maxDisplacement )
{
this->maxDisplacement = maxDisplacement;
this->restLength = restLength;
}
void spring::setDynamicBodyOffset( btScalar* offsetFromEqualibrium )
{
this->offsetFromEqualibrium = offsetFromEqualibrium;
}
void spring::setDirectionOfMotion( int directionOfMotion )
{
this->directionOfMotion = directionOfMotion;
}
btRigidBody* spring::getDynamicBody( )
{
return translationalMassBody;
}
btMotionState* spring::getDynamicBodyMotionState( )
{
return translationalMassMotionState;
}
void spring::addToWorld( btDynamicsWorld* world )
{
try
{
world->addRigidBody( translationalMassBody );
world->addRigidBody( staticBaseBody );
world->addConstraint( springConstraint, true );
}
catch( std::exception& e )
{
std::cout << "Could not add the contraint to the world: "
<< e.what( ) << std::endl;
}
}
Code: Select all
#include <stdio.h>
#include <iostream>
#include <btBulletDynamicsCommon.h>
#include "world.h"
#include "spring.h"
using namespace std;
int main( int argc, char * argv[] )
{
world* physicsWorld = new world( );
spring* testSpring = new spring( );
btVector3* gravity = new btVector3( 0, 0, 0 );
physicsWorld->setGravity( gravity );
testSpring->addToWorld( physicsWorld->getDynamicsWorld( ) );
btTransform trans;
//testSpring->getDynamicBody( )->applyForce( btVector3( -1.0f, 0, 0 ),
// btVector3( 5, 0, 0 ) );
//trans = btTransform::getIdentity( );
//trans.setOrigin( btVector3( 6.0f, 1.0f, 0.0f ) );
//testSpring->getDynamicBodyMotionState( )->setWorldTransform( trans );
std::cout << "#Dynamic mass\n#x location,y location,z location, time" << std::endl;
for (int i=0 ; i<300 ; i++) {
physicsWorld->getDynamicsWorld()->stepSimulation(1/60.f,10);
testSpring->getDynamicBodyMotionState( )->getWorldTransform(trans);
std::cout << trans.getOrigin().getX()
<< "," << trans.getOrigin().getY()
<< "," << trans.getOrigin().getZ()
<< "," << i*1/60.0f
<< std::endl;
}
delete physicsWorld;
delete testSpring;
return 0;
}