diff --git a/examples/Constraints/ConstraintPhysicsSetup.cpp b/examples/Constraints/ConstraintPhysicsSetup.cpp new file mode 100644 index 000000000..3ccbb08ee --- /dev/null +++ b/examples/Constraints/ConstraintPhysicsSetup.cpp @@ -0,0 +1,150 @@ +#include "ConstraintPhysicsSetup.h" + + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + + + +struct ConstraintPhysicsSetup : public CommonRigidBodyBase +{ + ConstraintPhysicsSetup(struct GUIHelperInterface* helper); + virtual ~ConstraintPhysicsSetup(); + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + +}; + +ConstraintPhysicsSetup::ConstraintPhysicsSetup(struct GUIHelperInterface* helper) +:CommonRigidBodyBase(helper) +{ +} +ConstraintPhysicsSetup::~ConstraintPhysicsSetup() +{ +} + +static btScalar val; +static btScalar targetVel=0; +static btScalar maxImpulse=10000; +static btHingeAccumulatedAngleConstraint* spDoorHinge=0; +static btScalar actualHingeVelocity=0.f; + +static btVector3 btAxisA(0,1,0); + +void ConstraintPhysicsSetup::stepSimulation(float deltaTime) +{ + val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD; + if (m_dynamicsWorld) + { + spDoorHinge->enableAngularMotor(true,targetVel,maxImpulse); + + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + + + btHingeConstraint* hinge = spDoorHinge; + + if (hinge) + { + + const btRigidBody& bodyA = hinge->getRigidBodyA(); + const btRigidBody& bodyB = hinge->getRigidBodyB(); + + + btTransform trA = bodyA.getWorldTransform(); + btVector3 angVelA = bodyA.getAngularVelocity(); + btVector3 angVelB = bodyB.getAngularVelocity(); + + { + btVector3 ax1 = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2); + btScalar vel = angVelA.dot(ax1); + vel -= angVelB.dot(ax1); + printf("hinge velocity (q) = %f\n", vel); + actualHingeVelocity=vel; + } + btVector3 ortho0,ortho1; + btPlaneSpace1(btAxisA,ortho0,ortho1); + { + + btScalar vel2 = angVelA.dot(ortho0); + vel2 -= angVelB.dot(ortho0); + printf("hinge orthogonal1 velocity (q) = %f\n", vel2); + } + { + + btScalar vel0 = angVelA.dot(ortho1); + vel0 -= angVelB.dot(ortho1); + printf("hinge orthogonal0 velocity (q) = %f\n", vel0); + } + } + } +} + + + +void ConstraintPhysicsSetup::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + createEmptyDynamicsWorld(); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + int mode = btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawConstraintLimits; + m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); + + + { + SliderParams slider("target vel",&targetVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("max impulse",&maxImpulse); + slider.m_minVal=0; + slider.m_maxVal=1000; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("actual vel",&actualHingeVelocity); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + val=1.f; + { + SliderParams slider("angle",&val); + slider.m_minVal=-720; + slider.m_maxVal=720; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create a door using hinge constraint attached to the world + btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); + m_collisionShapes.push_back(pDoorShape); + btTransform doorTrans; + doorTrans.setIdentity(); + doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); + btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape); + pDoorBody->setActivationState(DISABLE_DEACTIVATION); + const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside + + spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA ); + + m_dynamicsWorld->addConstraint(spDoorHinge); + + spDoorHinge->setDbgDrawSize(btScalar(5.f)); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +class ExampleInterface* ConstraintCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +{ + return new ConstraintPhysicsSetup(helper); +} diff --git a/examples/Constraints/ConstraintPhysicsSetup.h b/examples/Constraints/ConstraintPhysicsSetup.h new file mode 100644 index 000000000..c34374892 --- /dev/null +++ b/examples/Constraints/ConstraintPhysicsSetup.h @@ -0,0 +1,6 @@ +#ifndef CONSTAINT_PHYSICS_SETUP_H +#define CONSTAINT_PHYSICS_SETUP_H + +class ExampleInterface* ConstraintCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); + +#endif //CONSTAINT_PHYSICS_SETUP_H diff --git a/examples/Constraints/Dof6Spring2Setup.cpp b/examples/Constraints/Dof6Spring2Setup.cpp new file mode 100644 index 000000000..b346a9fb9 --- /dev/null +++ b/examples/Constraints/Dof6Spring2Setup.cpp @@ -0,0 +1,479 @@ +#include "Dof6Spring2Setup.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 "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 Dof6Spring2Setup : public CommonRigidBodyBase +{ + struct Dof6Spring2SetupInternalData* m_data; + + Dof6Spring2Setup(struct GUIHelperInterface* helper); + virtual ~Dof6Spring2Setup(); + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + void animate(); +}; + + + +struct Dof6Spring2SetupInternalData +{ + 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; + + + float mDt; + + unsigned int frameID; + Dof6Spring2SetupInternalData() + : mDt(1./60.),frameID(0) + { + } +}; + +Dof6Spring2Setup::Dof6Spring2Setup(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) +{ + m_data = new Dof6Spring2SetupInternalData; +} +Dof6Spring2Setup::~Dof6Spring2Setup() +{ + exitPhysics(); + delete m_data; +} +void Dof6Spring2Setup::initPhysics() +{ + // Setup the basic world + + 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_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); + +/////////// 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); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// 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); + 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); + 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); + 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 + 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 + 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); + } + 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); + 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_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); + } + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void Dof6Spring2Setup::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_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); + } + else + { + m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG); + m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint); + 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 Dof6Spring2Setup::stepSimulation(float deltaTime) +{ + animate(); + m_dynamicsWorld->stepSimulation(deltaTime); +} + +class ExampleInterface* Dof6Spring2CreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +{ + return new Dof6Spring2Setup(helper); +} diff --git a/examples/Constraints/Dof6Spring2Setup.h b/examples/Constraints/Dof6Spring2Setup.h new file mode 100644 index 000000000..8d6a2df2a --- /dev/null +++ b/examples/Constraints/Dof6Spring2Setup.h @@ -0,0 +1,6 @@ +#ifndef GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H +#define GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H + +class ExampleInterface* Dof6Spring2CreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); + +#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f03ba31b2..a81474f25 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -17,6 +17,10 @@ #include "../Importers/ImportSTLDemo/ImportSTLSetup.h" #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../GyroscopicDemo/GyroscopicSetup.h" +#include "../Constraints/Dof6Spring2Setup.h" +#include "../Constraints/ConstraintPhysicsSetup.h" +#include "../MultiBody/TestJointTorqueSetup.h" +#include "../MultiBody/MultiDofDemo.h" struct ExampleEntry { @@ -42,8 +46,15 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Gyroscopic", GyroscopicCreateFunc), ExampleEntry(1,"Planar 2D",Planar2DCreateFunc), + ExampleEntry(1,"Constraints",ConstraintCreateFunc), + ExampleEntry(1,"6DofSpring2",Dof6Spring2CreateFunc), -//#ifndef _DEBUG + ExampleEntry(0,"MultiBody",0), + ExampleEntry(1,"TestJointTorque",TestJointTorqueCreateFunc), + ExampleEntry(1,"MultiDofCreateFunc",MultiDofCreateFunc), + + +#ifndef _DEBUG ExampleEntry(0,"Benchmarks", 0), ExampleEntry(1,"3000 boxes", BenchmarkCreateFunc, 1), ExampleEntry(1,"1000 stack", BenchmarkCreateFunc, 2), @@ -52,7 +63,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Prim vs Mesh", BenchmarkCreateFunc, 5), ExampleEntry(1,"Convex vs Mesh", BenchmarkCreateFunc, 6), ExampleEntry(1,"Raycast", BenchmarkCreateFunc, 7), -//#endif +#endif ExampleEntry(0,"Importers", 0), diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp new file mode 100644 index 000000000..3c8224132 --- /dev/null +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -0,0 +1,424 @@ +#include "MultiDofDemo.h" +#include "../OpenGLWindow/SimpleOpenGL3App.h" +#include "btBulletDynamicsCommon.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" + +#include "../OpenGLWindow/GLInstancingRenderer.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + + +class MultiDofDemo : public CommonMultiBodyBase +{ + +public: + + MultiDofDemo(GUIHelperInterface* helper); + virtual ~MultiDofDemo(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + + btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical = false, bool floating = false); + void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents); + void addBoxes_testMultiDof(); + + +}; + +static bool g_floatingBase = false; +static bool g_firstInit = true; +static float scaling = 0.4f; +static float friction = 1.; +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024) + + +#define START_POS_X -5 +//#define START_POS_Y 12 +#define START_POS_Y 2 +#define START_POS_Z -3 + + + +MultiDofDemo::MultiDofDemo(GUIHelperInterface* helper) +:CommonMultiBodyBase(helper) +{ + m_guiHelper->setUpAxis(1); +} +MultiDofDemo::~MultiDofDemo() +{ +} + +void MultiDofDemo::stepSimulation(float deltaTime) +{ + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1./240.f; + m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep); +} + + +void MultiDofDemo::initPhysics() +{ + + + + if(g_firstInit) + { + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10.*scaling)); + m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); + g_firstInit = false; + } + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration); + m_dynamicsWorld = world; +// m_dynamicsWorld->setDebugDrawer(&gDebugDraw); + + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + + ///create a few basic rigid bodies + btVector3 groundHalfExtents(50,50,50); + btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); + //groundShape->initializePolyhedralFeatures(); +// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,00)); + + ///////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////// + + bool damping = true; + bool gyro = true; + int numLinks = 5; + bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool multibodyOnly = false; + bool canSleep = true; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + + g_floatingBase = ! g_floatingBase; + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0)); + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 45.f * SIMD_PI/ 180.f; + if(!spherical) + if(mbC->isMultiDof()) + mbC->setJointPosMultiDof(0, &q0); + else + mbC->setJointPos(0, q0); + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents); + + + ///////////////////////////////////////////////////////////////// + btScalar groundHeight = -51.55; + if (!multibodyOnly) + { + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + groundShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,groundHeight,0)); + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2);//,1,1+2); + + + + + } + ///////////////////////////////////////////////////////////////// + if(!multibodyOnly) + { + btVector3 halfExtents(.5,.5,.5); + btBoxShape* colShape = new btBoxShape(halfExtents); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + startTransform.setOrigin(btVector3( + btScalar(0.0), + -0.95, + btScalar(0.0))); + + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + m_dynamicsWorld->addRigidBody(body);//,1,1+2); + + + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + ///////////////////////////////////////////////////////////////// +} + + +btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld *pWorld, int numLinks, const btVector3 &basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if(baseMass) + { + btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + bool isMultiDof = true; + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); +// pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + if(!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents) +{ + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2,1+2); + + + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + + btVector3 posr = local_origin[i+1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col,2,1+2); + + + pMultiBody->getLink(i).m_collider=col; + } +} + +void MultiDofDemo::addBoxes_testMultiDof() +{ + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btBoxShape* colShape = new btBoxShape(btVector3(1,1,1)); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + float start_x = START_POS_X - ARRAY_SIZE_X/2; + float start_y = START_POS_Y; + float start_z = START_POS_Z - ARRAY_SIZE_Z/2; + + for (int k=0;kaddRigidBody(body);//,1,1+2); + } + } + } +} + +class ExampleInterface* MultiDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +{ + return new MultiDofDemo(helper); +} \ No newline at end of file diff --git a/examples/MultiBody/MultiDofDemo.h b/examples/MultiBody/MultiDofDemo.h new file mode 100644 index 000000000..601d48421 --- /dev/null +++ b/examples/MultiBody/MultiDofDemo.h @@ -0,0 +1,8 @@ + +#ifndef MULTI_DOF_DEMO_H +#define MULTI_DOF_DEMO_H + +class ExampleInterface* MultiDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); + +#endif //MULTI_DOF_DEMO_H + diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp new file mode 100644 index 000000000..81c070271 --- /dev/null +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -0,0 +1,275 @@ +//test addJointTorque +#include "TestJointTorqueSetup.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +struct TestJointTorqueSetup : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + +public: + + TestJointTorqueSetup(struct GUIHelperInterface* helper); + virtual ~TestJointTorqueSetup(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + +}; + +TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper) +:CommonMultiBodyBase(helper) +{ +} + +TestJointTorqueSetup::~TestJointTorqueSetup() +{ + +} + +void TestJointTorqueSetup::initPhysics() +{ + int upAxis = 2; + + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + + + + m_guiHelper->setUpAxis(upAxis); + + this->createEmptyDynamicsWorld(); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + //btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + //create a static ground object + if (0) + { + btVector3 groundHalfExtents(20,20,20); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + m_guiHelper->createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,0); + groundOrigin[upAxis]=-1.5; + start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + m_guiHelper->createRigidBodyGraphicsObject(body,color); + } + + { + bool floating = false; + bool damping = true; + bool gyro = true; + int numLinks = 5; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if(baseMass) + { + btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool isMultiDof = false; + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + m_multiBody = pMultiBody; + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + // pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + if(!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + + //pMultiBody->finalizeMultiDof(); + + btMultiBodyDynamicsWorld* world = m_dynamicsWorld; + + /// + world->addMultiBody(pMultiBody); + btMultiBody* mbC = pMultiBody; + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + btVector3 gravity(0,0,0); + //gravity[upAxis] = -9.81; + m_dynamicsWorld->setGravity(gravity); + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 45.f * SIMD_PI/ 180.f; + if(!spherical) + if(mbC->isMultiDof()) + mbC->setJointPosMultiDof(0, &q0); + else + mbC->setJointPos(0, q0); + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + double friction = 1; + { + + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + m_guiHelper->createCollisionShapeGraphicsObject(box); + + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); +//if we don't set the initial pose of the btCollisionObject, the simulator will do this + //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider + + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + + world->addCollisionObject(col, 2,1+2); + + btVector3 color(0.0,0.0,0.5); + m_guiHelper->createCollisionObjectGraphicsObject(col,color); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + + btVector3 posr = local_origin[i+1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + m_guiHelper->createCollisionShapeGraphicsObject(box); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + world->addCollisionObject(col,2,1+2); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + m_guiHelper->createCollisionObjectGraphicsObject(col,color); + + + pMultiBody->getLink(i).m_collider=col; + } + } + +} + +void TestJointTorqueSetup::stepSimulation(float deltaTime) +{ + m_multiBody->addJointTorque(0, 10.0); + m_dynamicsWorld->stepSimulation(deltaTime); +} + + +class ExampleInterface* TestJointTorqueCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +{ + return new TestJointTorqueSetup(helper); +} \ No newline at end of file diff --git a/examples/MultiBody/TestJointTorqueSetup.h b/examples/MultiBody/TestJointTorqueSetup.h new file mode 100644 index 000000000..19ab5004f --- /dev/null +++ b/examples/MultiBody/TestJointTorqueSetup.h @@ -0,0 +1,7 @@ +#ifndef TEST_JOINT_TORQUE_SETUP_H +#define TEST_JOINT_TORQUE_SETUP_H + +class ExampleInterface* TestJointTorqueCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); + +#endif //TEST_JOINT_TORQUE_SETUP_H +