diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 718686879..2074ef722 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -29,6 +29,8 @@ SET(App_ExampleBrowser_SRCS ../ForkLift/ForkLiftDemo.h ../Tutorial/Tutorial.cpp ../Tutorial/Tutorial.h + ../Tutorial/Dof6ConstraintTutorial.cpp + ../Tutorial/Dof6ConstraintTutorial.h ../GyroscopicDemo/GyroscopicSetup.cpp ../GyroscopicDemo/GyroscopicSetup.h ../Planar2D/Planar2D.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f499e21c4..ba0ca3b57 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -39,6 +39,7 @@ #include "../Constraints/TestHingeTorque.h" #include "../RenderingExamples/TimeSeriesExample.h" #include "../Tutorial/Tutorial.h" +#include "../Tutorial/Dof6ConstraintTutorial.h" #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" @@ -110,7 +111,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Tutorial"), ExampleEntry(1,"Free Rigid Body","(Preliminary work in progress) Free moving rigid body, without external or constraint forces", TutorialCreateFunc,0), - + ExampleEntry(1,"Spring constraint","A rigid body with a spring constraint attached", Dof6ConstraintTutorialCreateFunc,0), #ifdef INCLUDE_CLOTH_DEMOS ExampleEntry(0,"Soft Body"), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 373590558..63dfc7ce5 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -58,6 +58,7 @@ static int sCurrentDemoIndex = -1; static int sCurrentHightlighted = 0; static CommonExampleInterface* sCurrentDemo = 0; static b3AlignedObjectArray allNames; +static float gFixedTimeStep = 0; static class ExampleEntries* gAllExamples=0; bool sUseOpenGL2 = false; @@ -358,6 +359,8 @@ static void saveCurrentSettings(int currentEntry,const char* startFileName) fprintf(f,"--background_color_red= %f\n", red); fprintf(f,"--background_color_green= %f\n", green); fprintf(f,"--background_color_blue= %f\n", blue); + fprintf(f,"--fixed_timestep= %f\n", gFixedTimeStep); + if (enable_experimental_opencl) { @@ -663,6 +666,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) loadCurrentSettings(startFileName, args); + args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep); ///The OpenCL rigid body pipeline is experimental and ///most OpenCL drivers and OpenCL compilers have issues with our kernels. @@ -984,7 +988,13 @@ void OpenGLExampleBrowser::update(float deltaTime) } - sCurrentDemo->stepSimulation(deltaTime);//1./60.f); + if (gFixedTimeStep>0) + { + sCurrentDemo->stepSimulation(gFixedTimeStep); + } else + { + sCurrentDemo->stepSimulation(deltaTime);//1./60.f); + } } if (renderVisualGeometry && ((gDebugDrawFlags&btIDebugDraw::DBG_DrawWireframe)==0)) diff --git a/examples/RenderingExamples/TimeSeriesCanvas.cpp b/examples/RenderingExamples/TimeSeriesCanvas.cpp index f474ef115..972e83609 100644 --- a/examples/RenderingExamples/TimeSeriesCanvas.cpp +++ b/examples/RenderingExamples/TimeSeriesCanvas.cpp @@ -95,7 +95,7 @@ void TimeSeriesCanvas::addDataSource(const char* dataSourceLabel, unsigned char int numSources = m_internalData->m_dataSources.size(); int row = numSources%3; int column = numSources/3; - grapicalPrintf(dataSourceLabel, sTimeSeriesFontData, 30+200*column,m_internalData->m_height-48+row*16, + grapicalPrintf(dataSourceLabel, sTimeSeriesFontData, 50+200*column,m_internalData->m_height-48+row*16, red, green,blue,255); } diff --git a/examples/Tutorial/Dof6ConstraintTutorial.cpp b/examples/Tutorial/Dof6ConstraintTutorial.cpp new file mode 100644 index 000000000..9b65a91a8 --- /dev/null +++ b/examples/Tutorial/Dof6ConstraintTutorial.cpp @@ -0,0 +1,533 @@ +#include "Dof6ConstraintTutorial.h" + +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "../RenderingExamples/TimeSeriesCanvas.h" + +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +#ifndef M_PI_4 +#define M_PI_4 0.785398163397448309616 +#endif + + +extern float g_additionalBodyMass; + +//comment this out to compare with original spring constraint +#define USE_6DOF2 +#ifdef USE_6DOF2 + #define CONSTRAINT_TYPE btGeneric6DofSpring2Constraint + #define EXTRAPARAMS +#else + #define CONSTRAINT_TYPE btGeneric6DofSpringConstraint + #define EXTRAPARAMS ,true +#endif + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + + + + +struct Dof6ConstraintTutorial : public CommonRigidBodyBase +{ + struct Dof6ConstraintTutorialInternalData* m_data; + + Dof6ConstraintTutorial(struct GUIHelperInterface* helper); + virtual ~Dof6ConstraintTutorial(); + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + void animate(); + + virtual void resetCamera() + { + float dist = 5; + float pitch = 722; + float yaw = 35; + float targetPos[3]={4,2,-11}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } +}; + + + +struct Dof6ConstraintTutorialInternalData +{ + btRigidBody* m_TranslateSpringBody; + btRigidBody* m_TranslateSpringBody2; + btRigidBody* m_RotateSpringBody; + btRigidBody* m_RotateSpringBody2; + btRigidBody* m_BouncingTranslateBody; + btRigidBody* m_MotorBody; + btRigidBody* m_ServoMotorBody; + btRigidBody* m_ChainLeftBody; + btRigidBody* m_ChainRightBody; + CONSTRAINT_TYPE* m_ServoMotorConstraint; + CONSTRAINT_TYPE* m_ChainLeftConstraint; + CONSTRAINT_TYPE* m_ChainRightConstraint; + + TimeSeriesCanvas* m_timeSeriesCanvas; + + float mDt; + + unsigned int frameID; + Dof6ConstraintTutorialInternalData() + : mDt(1./60.),frameID(0) + { + } +}; + +Dof6ConstraintTutorial::Dof6ConstraintTutorial(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) +{ + m_data = new Dof6ConstraintTutorialInternalData; + m_data->m_timeSeriesCanvas = new TimeSeriesCanvas(helper->get2dCanvasInterface(),256,256,"Position and Velocity"); + m_data->m_timeSeriesCanvas ->setupTimeSeries(20,100, 0); + m_data->m_timeSeriesCanvas->addDataSource("X position (m)", 255,0,0); + m_data->m_timeSeriesCanvas->addDataSource("X velocity (m/s)", 0,0,255); + m_data->m_timeSeriesCanvas->addDataSource("dX/dt (m/s)", 0,0,0); + +} +Dof6ConstraintTutorial::~Dof6ConstraintTutorial() +{ + delete m_data->m_timeSeriesCanvas; + m_data->m_timeSeriesCanvas = 0; + exitPhysics(); + delete m_data; +} +void Dof6ConstraintTutorial::initPhysics() +{ + // Setup the basic world + + m_guiHelper->setUpAxis(1); + + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + btVector3 worldAabbMin(-10000,-10000,-10000); + btVector3 worldAabbMax(10000,10000,10000); + m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax); + +/////// uncomment the corresponding line to test a solver. + //m_solver = new btSequentialImpulseConstraintSolver; + m_solver = new btNNCGConstraintSolver; + //m_solver = new btMLCPSolver(new btSolveProjectedGaussSeidel()); + //m_solver = new btMLCPSolver(new btDantzigSolver()); + //m_solver = new btMLCPSolver(new btLemkeSolver()); + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld->getDispatchInfo().m_useContinuous = true; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + m_dynamicsWorld->setGravity(btVector3(0,0,0)); + + // Setup a big ground box + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(5.),btScalar(200.))); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-10,0)); +#define CREATE_GROUND_COLLISION_OBJECT 1 +#ifdef CREATE_GROUND_COLLISION_OBJECT + btCollisionObject* fixedGround = new btCollisionObject(); + fixedGround->setCollisionShape(groundShape); + fixedGround->setWorldTransform(groundTransform); + m_dynamicsWorld->addCollisionObject(fixedGround); +#else + localCreateRigidBody(btScalar(0.),groundTransform,groundShape); +#endif //CREATE_GROUND_COLLISION_OBJECT + } + + m_dynamicsWorld->getSolverInfo().m_numIterations = 100; + + btCollisionShape* shape; + btVector3 localInertia(0,0,0); + btDefaultMotionState* motionState; + btTransform bodyTransform; + btScalar mass; + btTransform localA; + btTransform localB; + CONSTRAINT_TYPE* constraint; + + + //static body centered in the origo + mass = 0.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + localInertia = btVector3(0,0,0); + bodyTransform.setIdentity(); + motionState = new btDefaultMotionState(bodyTransform); + btRigidBody* staticBody = new btRigidBody(mass,motionState,shape,localInertia); + m_dynamicsWorld->addRigidBody(staticBody); + +/////////// box with undamped translate spring attached to static body +/////////// the box should oscillate left-to-right forever + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(-2,0,-5)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_TranslateSpringBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_TranslateSpringBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_TranslateSpringBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,-5); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_TranslateSpringBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 1,-1); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + constraint->enableSpring(0, true); + constraint->setStiffness(0, 100); + #ifdef USE_6DOF2 + constraint->setDamping(0, 0); + #else + constraint->setDamping(0, 1); + #endif + constraint->setEquilibriumPoint(0, 0); + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + } +#if 0 +/////////// box with rotate spring, attached to static body +/////////// box should swing (rotate) left-to-right forever + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.getBasis().setEulerZYX(0,0,M_PI_2); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_RotateSpringBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_RotateSpringBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_RotateSpringBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,0); + localB.setIdentity();localB.setOrigin(btVector3(0,0.5,0)); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_RotateSpringBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1, -1); + constraint->enableSpring(5, true); + constraint->setStiffness(5, 100); + #ifdef USE_6DOF2 + constraint->setDamping(5, 0); + #else + constraint->setDamping(5, 1); + #endif + constraint->setEquilibriumPoint(0, 0); + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with bouncing constraint, translation is bounced at the positive x limit, but not at the negative limit +/////////// bouncing can not be set independently at low and high limits, so two constraints will be created: one that defines the low (non bouncing) limit, and one that defines the high (bouncing) limit +/////////// the box should move to the left (as an impulse will be applied to it periodically) until it reaches its limit, then bounce back + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(0,0,-3)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_BouncingTranslateBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_BouncingTranslateBody->setActivationState(DISABLE_DEACTIVATION); + m_data->m_BouncingTranslateBody->setDeactivationTime(btScalar(20000000)); + m_dynamicsWorld->addRigidBody(m_data->m_BouncingTranslateBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -2, SIMD_INFINITY); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, -3, -3); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + #ifdef USE_6DOF2 + constraint->setBounce(0,0); + #else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect + constraint->getTranslationalLimitMotor()->m_restitution = 0.0; + #endif + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -SIMD_INFINITY, 2); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, -3, -3); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + #ifdef USE_6DOF2 + constraint->setBounce(0,1); + #else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect + constraint->getTranslationalLimitMotor()->m_restitution = 1.0; + #endif + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with rotational motor, attached to static body +/////////// the box should rotate around the y axis + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(4,0,0)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_MotorBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_MotorBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_MotorBody); + localA.setIdentity();localA.getOrigin() = btVector3(4,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_MotorBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1,-1); + #ifdef USE_6DOF2 + constraint->enableMotor(5,true); + constraint->setTargetVelocity(5,3.f); + constraint->setMaxMotorForce(5,10.f); + #else + constraint->getRotationalLimitMotor(2)->m_enableMotor = true; + constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f; + constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; + #endif + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with rotational servo motor, attached to static body +/////////// the box should rotate around the y axis until it reaches its target +/////////// the target will be negated periodically + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(7,0,0)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_ServoMotorBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_ServoMotorBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_ServoMotorBody); + localA.setIdentity();localA.getOrigin() = btVector3(7,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ServoMotorBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1,-1); + #ifdef USE_6DOF2 + constraint->enableMotor(5,true); + constraint->setTargetVelocity(5,3.f); + constraint->setMaxMotorForce(5,10.f); + constraint->setServo(5,true); + constraint->setServoTarget(5, M_PI_2); + #else + constraint->getRotationalLimitMotor(2)->m_enableMotor = true; + constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f; + constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; + //servo motor is not implemented in 6dofspring constraint + #endif + constraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(constraint, true); + m_data->m_ServoMotorConstraint = constraint; + } + +////////// chain of boxes linked together with fully limited rotational and translational constraints +////////// the chain will be pulled to the left and to the right periodically. They should strictly stick together. + { + btScalar limitConstraintStrength = 0.6; + int bodycount = 10; + btRigidBody* prevBody = 0; + for(int i = 0; i < bodycount; ++i) + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(- i,0,3)); + motionState = new btDefaultMotionState(bodyTransform); + btRigidBody* body = new btRigidBody(mass,motionState,shape,localInertia); + body->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(body); + if(prevBody != 0) + { + localB.setIdentity(); + localB.setOrigin(btVector3(0.5,0,0)); + btTransform localA; + localA.setIdentity(); + localA.setOrigin(btVector3(-0.5,0,0)); + CONSTRAINT_TYPE* constraint = new CONSTRAINT_TYPE(*prevBody, *body, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -0.01, 0.01); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + for(int a = 0; a < 6; ++a) + { + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.9,a); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + constraint->setDbgDrawSize(btScalar(1.f)); + m_dynamicsWorld->addConstraint(constraint, true); + + if(i < bodycount - 1) + { + localA.setIdentity();localA.getOrigin() = btVector3(0,0,3); + localB.setIdentity(); + CONSTRAINT_TYPE* constraintZY = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS); + constraintZY->setLimit(0, 1, -1); + constraintZY->setDbgDrawSize(btScalar(1.f)); + m_dynamicsWorld->addConstraint(constraintZY, true); + + } + } + else + { + localA.setIdentity();localA.getOrigin() = btVector3(bodycount,0,3); + localB.setIdentity(); + localB.setOrigin(btVector3(0,0,0)); + m_data->m_ChainLeftBody = body; + m_data->m_ChainLeftConstraint = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS); + m_data->m_ChainLeftConstraint->setLimit(3,0,0); + m_data->m_ChainLeftConstraint->setLimit(4,0,0); + m_data->m_ChainLeftConstraint->setLimit(5,0,0); + for(int a = 0; a < 6; ++a) + { + m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a); + m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + m_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(1.f)); + m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); + } + prevBody = body; + } + m_data->m_ChainRightBody = prevBody; + localA.setIdentity();localA.getOrigin() = btVector3(-bodycount,0,3); + localB.setIdentity(); + localB.setOrigin(btVector3(0,0,0)); + m_data->m_ChainRightConstraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ChainRightBody, localA, localB EXTRAPARAMS); + m_data->m_ChainRightConstraint->setLimit(3,0,0); + m_data->m_ChainRightConstraint->setLimit(4,0,0); + m_data->m_ChainRightConstraint->setLimit(5,0,0); + for(int a = 0; a < 6; ++a) + { + m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a); + m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + } +#endif + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void Dof6ConstraintTutorial::animate() +{ + +/////// servo motor: flip its target periodically +#ifdef USE_6DOF2 + static float servoNextFrame = -1; + btScalar pos = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_currentPosition; + btScalar target = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget; + if(servoNextFrame < 0) + { + m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget *= -1; + servoNextFrame = 3.0; + } + servoNextFrame -= m_data->mDt; +#endif + +/////// constraint chain: pull the chain left and right periodically + static float chainNextFrame = -1; + static bool left = true; + if(chainNextFrame < 0) + { + if(!left) + { + m_data->m_ChainRightBody->setActivationState(ACTIVE_TAG); + m_dynamicsWorld->removeConstraint(m_data->m_ChainRightConstraint); + m_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); + } + else + { + m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG); + m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint); + m_data->m_ChainRightConstraint->setDbgDrawSize(btScalar(2.f)); + m_dynamicsWorld->addConstraint(m_data->m_ChainRightConstraint, true); + } + chainNextFrame = 3.0; + left = !left; + } + chainNextFrame -= m_data->mDt; + +/////// bouncing constraint: push the box periodically + m_data->m_BouncingTranslateBody->setActivationState(ACTIVE_TAG); + static float bounceNextFrame = -1; + if(bounceNextFrame < 0) + { + m_data->m_BouncingTranslateBody->applyCentralImpulse(btVector3(10,0,0)); + bounceNextFrame = 3.0; + } + bounceNextFrame -= m_data->mDt; + + m_data->frameID++; +} + + +void Dof6ConstraintTutorial::stepSimulation(float deltaTime) +{ + //animate(); + + + float time = m_data->m_timeSeriesCanvas->getCurrentTime(); + + float prevPos = m_data->m_TranslateSpringBody->getWorldTransform().getOrigin().x(); + m_dynamicsWorld->stepSimulation(deltaTime); + float xPos = m_data->m_TranslateSpringBody->getWorldTransform().getOrigin().x(); + + m_data->m_timeSeriesCanvas->insertDataAtCurrentTime(xPos,0,true); + m_data->m_timeSeriesCanvas->insertDataAtCurrentTime(m_data->m_TranslateSpringBody->getLinearVelocity().x(),1,true); + + if (deltaTime>0) + { + m_data->m_timeSeriesCanvas->insertDataAtCurrentTime((xPos-prevPos)/deltaTime,2,true); + } + prevPos = xPos; + m_data->m_timeSeriesCanvas->nextTick(); + +} + +class CommonExampleInterface* Dof6ConstraintTutorialCreateFunc( CommonExampleOptions& options) +{ + return new Dof6ConstraintTutorial(options.m_guiHelper); +} diff --git a/examples/Tutorial/Dof6ConstraintTutorial.h b/examples/Tutorial/Dof6ConstraintTutorial.h new file mode 100644 index 000000000..90e45351c --- /dev/null +++ b/examples/Tutorial/Dof6ConstraintTutorial.h @@ -0,0 +1,6 @@ +#ifndef GENERIC_6DOF_SPRING2_CONSTRAINT_TUTORIAL_H +#define GENERIC_6DOF_SPRING2_CONSTRAINT_TUTORIAL_H + +class CommonExampleInterface* Dof6ConstraintTutorialCreateFunc(struct CommonExampleOptions& options); + +#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_TUTORIAL_H