From bfef75f8712c2fd78c6012a613b8175284b67f66 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Sun, 30 Jan 2022 17:41:44 -0800 Subject: [PATCH] clean up: reverted some unnecessary changes --- build_cmake_pybullet_double.sh | 2 +- examples/Constraints/ConstraintDemo.cpp | 824 +++++++++--------- .../ReducedDeformableDemo/ReducedCollide.cpp | 12 +- .../RobotSimulator/HelloBulletRobotics.cpp | 14 +- .../btDeformableBackwardEulerObjective.cpp | 2 - 5 files changed, 425 insertions(+), 429 deletions(-) diff --git a/build_cmake_pybullet_double.sh b/build_cmake_pybullet_double.sh index ac40b1702..8817311f0 100755 --- a/build_cmake_pybullet_double.sh +++ b/build_cmake_pybullet_double.sh @@ -5,7 +5,7 @@ if [ -e CMakeCache.txt ]; then fi mkdir -p build_cmake cd build_cmake -cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DBT_USE_EGL=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE:FILEPATH=/opt/homebrew/Caskroom/miniforge/base/bin/python3 -DPYTHON_INCLUDE_DIR:PATH=/opt/homebrew/Caskroom/miniforge/base/include/python3.9 -DPYTHON_LIBRARY:FILEPATH=/opt/homebrew/Caskroom/miniforge/base/lib/libpython3.9.dylib .. || exit 1 +cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DBT_USE_EGL=ON -DCMAKE_BUILD_TYPE=Release .. || exit 1 make -j $(command nproc 2>/dev/null || echo 12) || exit 1 cd examples cd pybullet diff --git a/examples/Constraints/ConstraintDemo.cpp b/examples/Constraints/ConstraintDemo.cpp index f337f35c7..704b9779d 100644 --- a/examples/Constraints/ConstraintDemo.cpp +++ b/examples/Constraints/ConstraintDemo.cpp @@ -111,93 +111,93 @@ void AllConstraintDemo::initPhysics() float mass = 1.f; -// #if ENABLE_ALL_DEMOS -// ///gear constraint demo +#if ENABLE_ALL_DEMOS + ///gear constraint demo -// #define THETA SIMD_PI / 4.f -// #define L_1 (2 - std::tan(THETA)) -// #define L_2 (1 / std::cos(THETA)) -// #define RATIO L_2 / L_1 +#define THETA SIMD_PI / 4.f +#define L_1 (2 - std::tan(THETA)) +#define L_2 (1 / std::cos(THETA)) +#define RATIO L_2 / L_1 -// btRigidBody* bodyA = 0; -// btRigidBody* bodyB = 0; + btRigidBody* bodyA = 0; + btRigidBody* bodyB = 0; -// { -// btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.25, 0.2)); -// btCollisionShape* cylB = new btCylinderShape(btVector3(L_1, 0.025, L_1)); -// btCompoundShape* cyl0 = new btCompoundShape(); -// cyl0->addChildShape(btTransform::getIdentity(), cylA); -// cyl0->addChildShape(btTransform::getIdentity(), cylB); + { + btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.25, 0.2)); + btCollisionShape* cylB = new btCylinderShape(btVector3(L_1, 0.025, L_1)); + btCompoundShape* cyl0 = new btCompoundShape(); + cyl0->addChildShape(btTransform::getIdentity(), cylA); + cyl0->addChildShape(btTransform::getIdentity(), cylB); -// btScalar mass = 6.28; -// btVector3 localInertia; -// cyl0->calculateLocalInertia(mass, localInertia); -// btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia); -// ci.m_startWorldTransform.setOrigin(btVector3(-8, 1, -8)); + btScalar mass = 6.28; + btVector3 localInertia; + cyl0->calculateLocalInertia(mass, localInertia); + btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia); + ci.m_startWorldTransform.setOrigin(btVector3(-8, 1, -8)); -// btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia); -// m_dynamicsWorld->addRigidBody(body); -// body->setLinearFactor(btVector3(0, 0, 0)); -// body->setAngularFactor(btVector3(0, 1, 0)); -// bodyA = body; -// } + btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia); + m_dynamicsWorld->addRigidBody(body); + body->setLinearFactor(btVector3(0, 0, 0)); + body->setAngularFactor(btVector3(0, 1, 0)); + bodyA = body; + } -// { -// btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.26, 0.2)); -// btCollisionShape* cylB = new btCylinderShape(btVector3(L_2, 0.025, L_2)); -// btCompoundShape* cyl0 = new btCompoundShape(); -// cyl0->addChildShape(btTransform::getIdentity(), cylA); -// cyl0->addChildShape(btTransform::getIdentity(), cylB); + { + btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.26, 0.2)); + btCollisionShape* cylB = new btCylinderShape(btVector3(L_2, 0.025, L_2)); + btCompoundShape* cyl0 = new btCompoundShape(); + cyl0->addChildShape(btTransform::getIdentity(), cylA); + cyl0->addChildShape(btTransform::getIdentity(), cylB); -// btScalar mass = 6.28; -// btVector3 localInertia; -// cyl0->calculateLocalInertia(mass, localInertia); -// btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia); -// ci.m_startWorldTransform.setOrigin(btVector3(-10, 2, -8)); + btScalar mass = 6.28; + btVector3 localInertia; + cyl0->calculateLocalInertia(mass, localInertia); + btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia); + ci.m_startWorldTransform.setOrigin(btVector3(-10, 2, -8)); -// btQuaternion orn(btVector3(0, 0, 1), -THETA); -// ci.m_startWorldTransform.setRotation(orn); + btQuaternion orn(btVector3(0, 0, 1), -THETA); + ci.m_startWorldTransform.setRotation(orn); -// btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia); -// body->setLinearFactor(btVector3(0, 0, 0)); -// btHingeConstraint* hinge = new btHingeConstraint(*body, btVector3(0, 0, 0), btVector3(0, 1, 0), true); -// m_dynamicsWorld->addConstraint(hinge); -// bodyB = body; -// body->setAngularVelocity(btVector3(0, 3, 0)); + btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia); + body->setLinearFactor(btVector3(0, 0, 0)); + btHingeConstraint* hinge = new btHingeConstraint(*body, btVector3(0, 0, 0), btVector3(0, 1, 0), true); + m_dynamicsWorld->addConstraint(hinge); + bodyB = body; + body->setAngularVelocity(btVector3(0, 3, 0)); -// m_dynamicsWorld->addRigidBody(body); -// } + m_dynamicsWorld->addRigidBody(body); + } -// btVector3 axisA(0, 1, 0); -// btVector3 axisB(0, 1, 0); -// btQuaternion orn(btVector3(0, 0, 1), -THETA); -// btMatrix3x3 mat(orn); -// axisB = mat.getRow(1); + btVector3 axisA(0, 1, 0); + btVector3 axisB(0, 1, 0); + btQuaternion orn(btVector3(0, 0, 1), -THETA); + btMatrix3x3 mat(orn); + axisB = mat.getRow(1); -// btGearConstraint* gear = new btGearConstraint(*bodyA, *bodyB, axisA, axisB, RATIO); -// m_dynamicsWorld->addConstraint(gear, true); + btGearConstraint* gear = new btGearConstraint(*bodyA, *bodyB, axisA, axisB, RATIO); + m_dynamicsWorld->addConstraint(gear, true); -// #endif +#endif -// #if ENABLE_ALL_DEMOS -// //point to point constraint with a breaking threshold -// { -// trans.setIdentity(); -// trans.setOrigin(btVector3(1, 30, -5)); -// createRigidBody(mass, trans, shape); -// trans.setOrigin(btVector3(0, 0, -5)); +#if ENABLE_ALL_DEMOS + //point to point constraint with a breaking threshold + { + trans.setIdentity(); + trans.setOrigin(btVector3(1, 30, -5)); + createRigidBody(mass, trans, shape); + trans.setOrigin(btVector3(0, 0, -5)); -// btRigidBody* body0 = createRigidBody(mass, trans, shape); -// trans.setOrigin(btVector3(2 * CUBE_HALF_EXTENTS, 20, 0)); -// mass = 1.f; -// // btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape); -// btVector3 pivotInA(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, 0); -// btTypedConstraint* p2p = new btPoint2PointConstraint(*body0, pivotInA); -// m_dynamicsWorld->addConstraint(p2p); -// p2p->setBreakingImpulseThreshold(10.2); -// p2p->setDbgDrawSize(btScalar(5.f)); -// } -// #endif + btRigidBody* body0 = createRigidBody(mass, trans, shape); + trans.setOrigin(btVector3(2 * CUBE_HALF_EXTENTS, 20, 0)); + mass = 1.f; + // btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape); + btVector3 pivotInA(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, 0); + btTypedConstraint* p2p = new btPoint2PointConstraint(*body0, pivotInA); + m_dynamicsWorld->addConstraint(p2p); + p2p->setBreakingImpulseThreshold(10.2); + p2p->setDbgDrawSize(btScalar(5.f)); + } +#endif #if ENABLE_ALL_DEMOS //point to point constraint (ball socket) @@ -241,391 +241,391 @@ void AllConstraintDemo::initPhysics() } #endif -// #if ENABLE_ALL_DEMOS -// { -// btTransform trans; -// trans.setIdentity(); -// btVector3 worldPos(-20, 0, 30); -// trans.setOrigin(worldPos); +#if ENABLE_ALL_DEMOS + { + btTransform trans; + trans.setIdentity(); + btVector3 worldPos(-20, 0, 30); + trans.setOrigin(worldPos); -// btTransform frameInA, frameInB; -// frameInA = btTransform::getIdentity(); -// frameInB = btTransform::getIdentity(); + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInB = btTransform::getIdentity(); -// btRigidBody* pRbA1 = createRigidBody(mass, trans, shape); -// // btRigidBody* pRbA1 = createRigidBody(0.f, trans, shape); -// pRbA1->setActivationState(DISABLE_DEACTIVATION); + btRigidBody* pRbA1 = createRigidBody(mass, trans, shape); + // btRigidBody* pRbA1 = createRigidBody(0.f, trans, shape); + pRbA1->setActivationState(DISABLE_DEACTIVATION); -// // add dynamic rigid body B1 -// worldPos.setValue(-30, 0, 30); -// trans.setOrigin(worldPos); -// btRigidBody* pRbB1 = createRigidBody(mass, trans, shape); -// // btRigidBody* pRbB1 = createRigidBody(0.f, trans, shape); -// pRbB1->setActivationState(DISABLE_DEACTIVATION); + // add dynamic rigid body B1 + worldPos.setValue(-30, 0, 30); + trans.setOrigin(worldPos); + btRigidBody* pRbB1 = createRigidBody(mass, trans, shape); + // btRigidBody* pRbB1 = createRigidBody(0.f, trans, shape); + pRbB1->setActivationState(DISABLE_DEACTIVATION); -// // create slider constraint between A1 and B1 and add it to world + // create slider constraint between A1 and B1 and add it to world -// btSliderConstraint* spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true); -// // spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false); -// spSlider1->setLowerLinLimit(-15.0F); -// spSlider1->setUpperLinLimit(-5.0F); -// // spSlider1->setLowerLinLimit(5.0F); -// // spSlider1->setUpperLinLimit(15.0F); -// // spSlider1->setLowerLinLimit(-10.0F); -// // spSlider1->setUpperLinLimit(-10.0F); + btSliderConstraint* spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true); + // spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false); + spSlider1->setLowerLinLimit(-15.0F); + spSlider1->setUpperLinLimit(-5.0F); + // spSlider1->setLowerLinLimit(5.0F); + // spSlider1->setUpperLinLimit(15.0F); + // spSlider1->setLowerLinLimit(-10.0F); + // spSlider1->setUpperLinLimit(-10.0F); -// spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F); -// spSlider1->setUpperAngLimit(SIMD_PI / 3.0F); + spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F); + spSlider1->setUpperAngLimit(SIMD_PI / 3.0F); -// m_dynamicsWorld->addConstraint(spSlider1, true); -// spSlider1->setDbgDrawSize(btScalar(5.f)); -// } -// #endif + m_dynamicsWorld->addConstraint(spSlider1, true); + spSlider1->setDbgDrawSize(btScalar(5.f)); + } +#endif -// #if ENABLE_ALL_DEMOS -// //create a slider, using the generic D6 constraint -// { -// mass = 1.f; -// btVector3 sliderWorldPos(0, 10, 0); -// btVector3 sliderAxis(1, 0, 0); -// btScalar angle = 0.f; //SIMD_RADS_PER_DEG * 10.f; -// btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis, angle)); -// trans.setIdentity(); -// trans.setOrigin(sliderWorldPos); -// //trans.setBasis(sliderOrientation); -// sliderTransform = trans; +#if ENABLE_ALL_DEMOS + //create a slider, using the generic D6 constraint + { + mass = 1.f; + btVector3 sliderWorldPos(0, 10, 0); + btVector3 sliderAxis(1, 0, 0); + btScalar angle = 0.f; //SIMD_RADS_PER_DEG * 10.f; + btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis, angle)); + trans.setIdentity(); + trans.setOrigin(sliderWorldPos); + //trans.setBasis(sliderOrientation); + sliderTransform = trans; -// d6body0 = createRigidBody(mass, trans, shape); -// d6body0->setActivationState(DISABLE_DEACTIVATION); -// btRigidBody* fixedBody1 = createRigidBody(0, trans, 0); -// m_dynamicsWorld->addRigidBody(fixedBody1); + d6body0 = createRigidBody(mass, trans, shape); + d6body0->setActivationState(DISABLE_DEACTIVATION); + btRigidBody* fixedBody1 = createRigidBody(0, trans, 0); + m_dynamicsWorld->addRigidBody(fixedBody1); -// btTransform frameInA, frameInB; -// frameInA = btTransform::getIdentity(); -// frameInB = btTransform::getIdentity(); -// frameInA.setOrigin(btVector3(0., 5., 0.)); -// frameInB.setOrigin(btVector3(0., 5., 0.)); + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInB = btTransform::getIdentity(); + frameInA.setOrigin(btVector3(0., 5., 0.)); + frameInB.setOrigin(btVector3(0., 5., 0.)); -// // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits -// bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits -// spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0, frameInA, frameInB, useLinearReferenceFrameA); -// spSlider6Dof->setLinearLowerLimit(lowerSliderLimit); -// spSlider6Dof->setLinearUpperLimit(hiSliderLimit); + // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits + bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits + spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0, frameInA, frameInB, useLinearReferenceFrameA); + spSlider6Dof->setLinearLowerLimit(lowerSliderLimit); + spSlider6Dof->setLinearUpperLimit(hiSliderLimit); -// //range should be small, otherwise singularities will 'explode' the constraint -// // spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0)); -// // spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0)); -// // spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0)); -// // spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0)); -// spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI, 0, 0)); -// spSlider6Dof->setAngularUpperLimit(btVector3(1.5, 0, 0)); + //range should be small, otherwise singularities will 'explode' the constraint + // spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0)); + // spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0)); + // spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0)); + // spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0)); + spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI, 0, 0)); + spSlider6Dof->setAngularUpperLimit(btVector3(1.5, 0, 0)); -// spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; -// spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f; -// spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; + spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; + spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f; + spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; -// m_dynamicsWorld->addConstraint(spSlider6Dof); -// spSlider6Dof->setDbgDrawSize(btScalar(5.f)); -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { // 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 -// btVector3 btAxisA(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis + m_dynamicsWorld->addConstraint(spSlider6Dof); + spSlider6Dof->setDbgDrawSize(btScalar(5.f)); + } +#endif +#if ENABLE_ALL_DEMOS + { // 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 + btVector3 btAxisA(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis -// spDoorHinge = new btHingeConstraint(*pDoorBody, btPivotA, btAxisA); + spDoorHinge = new btHingeConstraint(*pDoorBody, btPivotA, btAxisA); -// // spDoorHinge->setLimit( 0.0f, SIMD_PI_2 ); -// // test problem values -// // spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f); + // spDoorHinge->setLimit( 0.0f, SIMD_PI_2 ); + // test problem values + // spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f); -// // spDoorHinge->setLimit( 1.f, -1.f); -// // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI); -// // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f); -// // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits" -// spDoorHinge->setLimit(-SIMD_PI * 0.25f, SIMD_PI * 0.25f); -// // spDoorHinge->setLimit( 0.0f, 0.0f ); -// m_dynamicsWorld->addConstraint(spDoorHinge); -// spDoorHinge->setDbgDrawSize(btScalar(5.f)); + // spDoorHinge->setLimit( 1.f, -1.f); + // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI); + // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f); + // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits" + spDoorHinge->setLimit(-SIMD_PI * 0.25f, SIMD_PI * 0.25f); + // spDoorHinge->setLimit( 0.0f, 0.0f ); + m_dynamicsWorld->addConstraint(spDoorHinge); + spDoorHinge->setDbgDrawSize(btScalar(5.f)); -// //doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f)); -// //btRigidBody* pDropBody = createRigidBody( 10.0, doorTrans, shape); -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { // create a generic 6DOF constraint + //doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f)); + //btRigidBody* pDropBody = createRigidBody( 10.0, doorTrans, shape); + } +#endif +#if ENABLE_ALL_DEMOS + { // create a generic 6DOF constraint -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// // btRigidBody* pBodyA = createRigidBody( mass, tr, shape); -// btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); -// // btRigidBody* pBodyA = createRigidBody( 0.0, tr, 0); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + // btRigidBody* pBodyA = createRigidBody( mass, tr, shape); + btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); + // btRigidBody* pBodyA = createRigidBody( 0.0, tr, 0); + pBodyA->setActivationState(DISABLE_DEACTIVATION); -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// btRigidBody* pBodyB = createRigidBody(mass, tr, shape); -// // btRigidBody* pBodyB = createRigidBody(0.f, tr, shape); -// pBodyB->setActivationState(DISABLE_DEACTIVATION); + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + btRigidBody* pBodyB = createRigidBody(mass, tr, shape); + // btRigidBody* pBodyB = createRigidBody(0.f, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); -// btTransform frameInA, frameInB; -// frameInA = btTransform::getIdentity(); -// frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.))); -// frameInB = btTransform::getIdentity(); -// frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.))); + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.))); + frameInB = btTransform::getIdentity(); + frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.))); -// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); -// // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); -// pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.)); -// pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.)); -// // pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.)); -// // pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.)); -// // pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.)); -// // pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.)); + btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); + // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); + pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.)); + pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.)); + // pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.)); + // pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.)); + // pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.)); + // pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.)); -// // pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true; -// // pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; -// // pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; + // pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true; + // pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; + // pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; -// // pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.)); -// // pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI)); + // pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.)); + // pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.)); + // pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI)); + // pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI)); -// pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f)); -// pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f)); -// // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); -// // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); + pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f)); + pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f)); + // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); + // pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); + // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); + // pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); -// // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5)); -// // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.)); -// // pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.)); -// // pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.)); -// // pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.)); -// // pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.)); + // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5)); + // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5)); + // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.)); + // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.)); + // pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.)); + // pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.)); + // pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.)); + // pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.)); -// m_dynamicsWorld->addConstraint(pGen6DOF, true); -// pGen6DOF->setDbgDrawSize(btScalar(5.f)); -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { // create a ConeTwist constraint + m_dynamicsWorld->addConstraint(pGen6DOF, true); + pGen6DOF->setDbgDrawSize(btScalar(5.f)); + } +#endif +#if ENABLE_ALL_DEMOS + { // create a ConeTwist constraint -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// btRigidBody* pBodyA = createRigidBody(1.0, tr, shape); -// // btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + btRigidBody* pBodyA = createRigidBody(1.0, tr, shape); + // btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// btRigidBody* pBodyB = createRigidBody(0.0, tr, shape); -// // btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + btRigidBody* pBodyB = createRigidBody(0.0, tr, shape); + // btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); -// btTransform frameInA, frameInB; -// frameInA = btTransform::getIdentity(); -// frameInA.getBasis().setEulerZYX(0, 0, SIMD_PI_2); -// frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.))); -// frameInB = btTransform::getIdentity(); -// frameInB.getBasis().setEulerZYX(0, 0, SIMD_PI_2); -// frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.))); + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInA.getBasis().setEulerZYX(0, 0, SIMD_PI_2); + frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.))); + frameInB = btTransform::getIdentity(); + frameInB.getBasis().setEulerZYX(0, 0, SIMD_PI_2); + frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.))); -// m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB); -// // m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f); -// // m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit -// m_ctc->setLimit(btScalar(SIMD_PI_4 * 0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f); -// m_dynamicsWorld->addConstraint(m_ctc, true); -// m_ctc->setDbgDrawSize(btScalar(5.f)); -// // s_bTestConeTwistMotor = true; // use only with old solver for now -// s_bTestConeTwistMotor = false; -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); -// btRigidBody* pBody = createRigidBody(1.0, tr, shape); -// pBody->setActivationState(DISABLE_DEACTIVATION); -// const btVector3 btPivotA(10.0f, 0.0f, 0.0f); -// btVector3 btAxisA(0.0f, 0.0f, 1.0f); + m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB); + // m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f); + // m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit + m_ctc->setLimit(btScalar(SIMD_PI_4 * 0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f); + m_dynamicsWorld->addConstraint(m_ctc, true); + m_ctc->setDbgDrawSize(btScalar(5.f)); + // s_bTestConeTwistMotor = true; // use only with old solver for now + s_bTestConeTwistMotor = false; + } +#endif +#if ENABLE_ALL_DEMOS + { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); + btRigidBody* pBody = createRigidBody(1.0, tr, shape); + pBody->setActivationState(DISABLE_DEACTIVATION); + const btVector3 btPivotA(10.0f, 0.0f, 0.0f); + btVector3 btAxisA(0.0f, 0.0f, 1.0f); -// btHingeConstraint* pHinge = new btHingeConstraint(*pBody, btPivotA, btAxisA); -// // pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver -// pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver -// m_dynamicsWorld->addConstraint(pHinge); -// pHinge->setDbgDrawSize(btScalar(5.f)); -// } -// #endif + btHingeConstraint* pHinge = new btHingeConstraint(*pBody, btPivotA, btAxisA); + // pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver + pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver + m_dynamicsWorld->addConstraint(pHinge); + pHinge->setDbgDrawSize(btScalar(5.f)); + } +#endif -// #if ENABLE_ALL_DEMOS -// { -// // create a universal joint using generic 6DOF constraint -// // create two rigid bodies -// // static bodyA (parent) on top: -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.))); -// btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); -// // dynamic bodyB (child) below it : -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.))); -// btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); -// pBodyB->setActivationState(DISABLE_DEACTIVATION); -// // add some (arbitrary) data to build constraint frames -// btVector3 parentAxis(1.f, 0.f, 0.f); -// btVector3 childAxis(0.f, 0.f, 1.f); -// btVector3 anchor(20.f, 2.f, 0.f); +#if ENABLE_ALL_DEMOS + { + // create a universal joint using generic 6DOF constraint + // create two rigid bodies + // static bodyA (parent) on top: + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.))); + btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); + // dynamic bodyB (child) below it : + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.))); + btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); + // add some (arbitrary) data to build constraint frames + btVector3 parentAxis(1.f, 0.f, 0.f); + btVector3 childAxis(0.f, 0.f, 1.f); + btVector3 anchor(20.f, 2.f, 0.f); -// btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); -// pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f); -// pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); -// // add constraint to world -// m_dynamicsWorld->addConstraint(pUniv, true); -// // draw constraint frames and limits for debugging -// pUniv->setDbgDrawSize(btScalar(5.f)); -// } -// #endif + btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); + pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f); + pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); + // add constraint to world + m_dynamicsWorld->addConstraint(pUniv, true); + // draw constraint frames and limits for debugging + pUniv->setDbgDrawSize(btScalar(5.f)); + } +#endif -// #if ENABLE_ALL_DEMOS -// { // create a generic 6DOF constraint with springs +#if ENABLE_ALL_DEMOS + { // create a generic 6DOF constraint with springs -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); -// tr.getBasis().setEulerZYX(0, 0, 0); -// btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); -// pBodyB->setActivationState(DISABLE_DEACTIVATION); + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); + tr.getBasis().setEulerZYX(0, 0, 0); + btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); -// btTransform frameInA, frameInB; -// frameInA = btTransform::getIdentity(); -// frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.))); -// frameInB = btTransform::getIdentity(); -// frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.))); + frameInB = btTransform::getIdentity(); + frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); -// btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); -// pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.)); -// pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.)); + btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); + pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.)); + pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.)); -// pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f)); -// pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f)); + pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f)); + pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f)); -// m_dynamicsWorld->addConstraint(pGen6DOFSpring, true); -// pGen6DOFSpring->setDbgDrawSize(btScalar(5.f)); + m_dynamicsWorld->addConstraint(pGen6DOFSpring, true); + pGen6DOFSpring->setDbgDrawSize(btScalar(5.f)); -// pGen6DOFSpring->enableSpring(0, true); -// pGen6DOFSpring->setStiffness(0, 39.478f); -// pGen6DOFSpring->setDamping(0, 0.5f); -// pGen6DOFSpring->enableSpring(5, true); -// pGen6DOFSpring->setStiffness(5, 39.478f); -// pGen6DOFSpring->setDamping(0, 0.3f); -// pGen6DOFSpring->setEquilibriumPoint(); -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { -// // create a Hinge2 joint -// // create two rigid bodies -// // static bodyA (parent) on top: -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.))); -// btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); -// // dynamic bodyB (child) below it : -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.))); -// btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); -// pBodyB->setActivationState(DISABLE_DEACTIVATION); -// // add some data to build constraint frames -// btVector3 parentAxis(0.f, 1.f, 0.f); -// btVector3 childAxis(1.f, 0.f, 0.f); -// btVector3 anchor(-20.f, 0.f, 0.f); -// btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); -// pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); -// pHinge2->setUpperLimit(SIMD_HALF_PI * 0.5f); -// // add constraint to world -// m_dynamicsWorld->addConstraint(pHinge2, true); -// // draw constraint frames and limits for debugging -// pHinge2->setDbgDrawSize(btScalar(5.f)); -// } -// #endif -// #if ENABLE_ALL_DEMOS -// { -// // create a Hinge joint between two dynamic bodies -// // create two rigid bodies -// // static bodyA (parent) on top: -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.))); -// btRigidBody* pBodyA = createRigidBody(1.0f, tr, shape); -// pBodyA->setActivationState(DISABLE_DEACTIVATION); -// // dynamic bodyB: -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.))); -// btRigidBody* pBodyB = createRigidBody(10.0, tr, shape); -// pBodyB->setActivationState(DISABLE_DEACTIVATION); -// // add some data to build constraint frames -// btVector3 axisA(0.f, 1.f, 0.f); -// btVector3 axisB(0.f, 1.f, 0.f); -// btVector3 pivotA(-5.f, 0.f, 0.f); -// btVector3 pivotB(5.f, 0.f, 0.f); -// spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB); -// spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); -// // add constraint to world -// m_dynamicsWorld->addConstraint(spHingeDynAB, true); -// // draw constraint frames and limits for debugging -// spHingeDynAB->setDbgDrawSize(btScalar(5.f)); -// } -// #endif + pGen6DOFSpring->enableSpring(0, true); + pGen6DOFSpring->setStiffness(0, 39.478f); + pGen6DOFSpring->setDamping(0, 0.5f); + pGen6DOFSpring->enableSpring(5, true); + pGen6DOFSpring->setStiffness(5, 39.478f); + pGen6DOFSpring->setDamping(0, 0.3f); + pGen6DOFSpring->setEquilibriumPoint(); + } +#endif +#if ENABLE_ALL_DEMOS + { + // create a Hinge2 joint + // create two rigid bodies + // static bodyA (parent) on top: + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.))); + btRigidBody* pBodyA = createRigidBody(0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); + // dynamic bodyB (child) below it : + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.))); + btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); + // add some data to build constraint frames + btVector3 parentAxis(0.f, 1.f, 0.f); + btVector3 childAxis(1.f, 0.f, 0.f); + btVector3 anchor(-20.f, 0.f, 0.f); + btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); + pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); + pHinge2->setUpperLimit(SIMD_HALF_PI * 0.5f); + // add constraint to world + m_dynamicsWorld->addConstraint(pHinge2, true); + // draw constraint frames and limits for debugging + pHinge2->setDbgDrawSize(btScalar(5.f)); + } +#endif +#if ENABLE_ALL_DEMOS + { + // create a Hinge joint between two dynamic bodies + // create two rigid bodies + // static bodyA (parent) on top: + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.))); + btRigidBody* pBodyA = createRigidBody(1.0f, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); + // dynamic bodyB: + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.))); + btRigidBody* pBodyB = createRigidBody(10.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); + // add some data to build constraint frames + btVector3 axisA(0.f, 1.f, 0.f); + btVector3 axisB(0.f, 1.f, 0.f); + btVector3 pivotA(-5.f, 0.f, 0.f); + btVector3 pivotB(5.f, 0.f, 0.f); + spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB); + spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); + // add constraint to world + m_dynamicsWorld->addConstraint(spHingeDynAB, true); + // draw constraint frames and limits for debugging + spHingeDynAB->setDbgDrawSize(btScalar(5.f)); + } +#endif -// #if ENABLE_ALL_DEMOS -// { // 6DOF connected to the world, with motor -// btTransform tr; -// tr.setIdentity(); -// tr.setOrigin(btVector3(btScalar(10.), btScalar(-15.), btScalar(0.))); -// btRigidBody* pBody = createRigidBody(1.0, tr, shape); -// pBody->setActivationState(DISABLE_DEACTIVATION); -// btTransform frameB; -// frameB.setIdentity(); -// btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint(*pBody, frameB, false); -// m_dynamicsWorld->addConstraint(pGen6Dof); -// pGen6Dof->setDbgDrawSize(btScalar(5.f)); +#if ENABLE_ALL_DEMOS + { // 6DOF connected to the world, with motor + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(10.), btScalar(-15.), btScalar(0.))); + btRigidBody* pBody = createRigidBody(1.0, tr, shape); + pBody->setActivationState(DISABLE_DEACTIVATION); + btTransform frameB; + frameB.setIdentity(); + btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint(*pBody, frameB, false); + m_dynamicsWorld->addConstraint(pGen6Dof); + pGen6Dof->setDbgDrawSize(btScalar(5.f)); -// pGen6Dof->setAngularLowerLimit(btVector3(0, 0, 0)); -// pGen6Dof->setAngularUpperLimit(btVector3(0, 0, 0)); -// pGen6Dof->setLinearLowerLimit(btVector3(-10., 0, 0)); -// pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0)); + pGen6Dof->setAngularLowerLimit(btVector3(0, 0, 0)); + pGen6Dof->setAngularUpperLimit(btVector3(0, 0, 0)); + pGen6Dof->setLinearLowerLimit(btVector3(-10., 0, 0)); + pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0)); -// pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; -// pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; -// pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; -// } -// #endif + pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; + pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; + pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; + } +#endif m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/ReducedDeformableDemo/ReducedCollide.cpp b/examples/ReducedDeformableDemo/ReducedCollide.cpp index 6ef3e5176..a64c01497 100644 --- a/examples/ReducedDeformableDemo/ReducedCollide.cpp +++ b/examples/ReducedDeformableDemo/ReducedCollide.cpp @@ -209,18 +209,18 @@ void ReducedCollide::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(0.5, 0.5, 0.5)); - - btTransform init_transform; - init_transform.setIdentity(); - init_transform.setOrigin(btVector3(0, 4, 0)); - // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); - rsb->transformTo(init_transform); rsb->setStiffnessScale(100); rsb->setDamping(damping_alpha, damping_beta); rsb->setTotalMass(15); + btTransform init_transform; + init_transform.setIdentity(); + init_transform.setOrigin(btVector3(0, 4, 0)); + // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); + rsb->transformTo(init_transform); + rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects rsb->m_cfg.kCHR = 1; // collision hardness with rigid body rsb->m_cfg.kDF = 0; diff --git a/examples/RobotSimulator/HelloBulletRobotics.cpp b/examples/RobotSimulator/HelloBulletRobotics.cpp index a47deed76..ec656be60 100644 --- a/examples/RobotSimulator/HelloBulletRobotics.cpp +++ b/examples/RobotSimulator/HelloBulletRobotics.cpp @@ -18,8 +18,7 @@ int main(int argc, char* argv[]) } //remove all existing objects (if any) - // sim->resetSimulation(); - sim->resetSimulation(RESET_USE_REDUCED_DEFORMABLE_WORLD); + sim->resetSimulation(); sim->setGravity(btVector3(0, 0, -9.8)); sim->setNumSolverIterations(100); b3RobotSimulatorSetPhysicsEngineParameters args; @@ -27,12 +26,11 @@ int main(int argc, char* argv[]) int planeUid = sim->loadURDF("plane.urdf"); printf("planeUid = %d\n", planeUid); - - int r2d2Uid = sim->loadURDF("reduced_cube/reduced_cube.urdf"); - // int r2d2Uid = sim->loadURDF("r2d2.urdf"); - // printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid)); - btVector3 basePosition = btVector3(0, 3, 3); + int r2d2Uid = sim->loadURDF("r2d2.urdf"); + printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid)); + + btVector3 basePosition = btVector3(0, 0, 0.5); btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1); sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation); @@ -42,7 +40,7 @@ int main(int argc, char* argv[]) btVector3 basePos; btQuaternion baseOrn; sim->getBasePositionAndOrientation(r2d2Uid, basePos, baseOrn); - // printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]); + printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]); sim->stepSimulation(); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index a5d4fa7df..2455ed213 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -136,8 +136,6 @@ void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) { - static btScalar sim_time = 0; - size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) {