diff --git a/build_cmake_pybullet_double.sh b/build_cmake_pybullet_double.sh index 92680d734..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=Debug .. || 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 092787f2b..05f505409 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/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index d6b3b21cb..bc369e9e3 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -31,9 +31,8 @@ // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.01; -static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 1; +static int num_modes = 20; class BasicTest : public CommonDeformableBodyBase { @@ -85,13 +84,13 @@ public: void Ctor_RbUpStack() { - float mass = 0.5; - btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2)); + float mass = 2; + btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0,-2,0)); - btRigidBody* rb = createRigidBody(mass, startTransform, shape); - rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0)); + startTransform.setOrigin(btVector3(0,8,1)); + btRigidBody* rb1 = createRigidBody(mass, startTransform, shape); + rb1->setActivationState(DISABLE_DEACTIVATION); } void checkMomentum(btReducedSoftBody* rsb) @@ -163,23 +162,23 @@ public: // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag); btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); - btVector3 origin = rsb->getRigidTransform().getOrigin(); - btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin; - btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin; - btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin; + // btVector3 origin = rsb->getRigidTransform().getOrigin(); + // btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin; + // btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin; + // btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin; - deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0)); - deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0)); - deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1)); + // deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0)); + // deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0)); + // deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1)); for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) { deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n"; } - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1)); } } } @@ -221,7 +220,7 @@ void BasicTest::initPhysics() // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); rsb->transform(init_transform); - rsb->setStiffnessScale(100); + rsb->setStiffnessScale(200); rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes @@ -251,7 +250,23 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies - // Ctor_RbUpStack(); // TODO: no rigid body for now + Ctor_RbUpStack(); + + // create a static rigid box as the ground + { + // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10))); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, 0, 0)); + { + btScalar mass(0.); + createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); + } + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); // { diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index 54c810ae7..3196a05e1 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -30,10 +30,10 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.01; +static btScalar damping_beta = 0.0; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 10; +static int num_modes = 20; class FreeFall : public CommonDeformableBodyBase { @@ -56,32 +56,33 @@ public: void resetCamera() { - // float dist = 10; - // float pitch = -20; - // float yaw = 90; - // float targetPos[3] = {0, 0, 0.5}; - float dist = 20; - float pitch = -30; - float yaw = 125; - float targetPos[3] = {-2, 0, 2}; + float dist = 6; + float pitch = -20; + float yaw = 90; + float targetPos[3] = {0, 2, 0}; + // float dist = 20; + // float pitch = -30; + // float yaw = 125; + // float targetPos[3] = {-2, 0, 2}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } void Ctor_RbUpStack() { float mass = 10; - btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); + btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5)); + // btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); btTransform startTransform; startTransform.setIdentity(); // startTransform.setOrigin(btVector3(0, 12, 0)); // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape); // rb0->setLinearVelocity(btVector3(0, 0, 0)); - startTransform.setOrigin(btVector3(0,4,-1)); + startTransform.setOrigin(btVector3(0,10,0)); // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0)); btRigidBody* rb1 = createRigidBody(mass, startTransform, shape); rb1->setActivationState(DISABLE_DEACTIVATION); - rb1->setLinearVelocity(btVector3(0, 0, 4)); + // rb1->setLinearVelocity(btVector3(0, 0, 4)); } void stepSimulation(float deltaTime) @@ -104,18 +105,18 @@ public: // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag); btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); - for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) - { - deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); - } - for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) - { - deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); - } + // for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) + // { + // deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); + // } + // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) + // { + // deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); + // } - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1)); + // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1)); } } } @@ -145,18 +146,18 @@ void FreeFall::initPhysics() // create volumetric reduced deformable body { - btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); + btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.01); btTransform init_transform; init_transform.setIdentity(); - init_transform.setOrigin(btVector3(0, 10, 0)); + init_transform.setOrigin(btVector3(0, 5, 0)); init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0)); rsb->transform(init_transform); - rsb->setStiffnessScale(50); + rsb->setStiffnessScale(25); rsb->setDamping(damping_alpha, damping_beta); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -171,6 +172,8 @@ void FreeFall::initPhysics() // rsb->setRigidVelocity(btVector3(0, 0, 1)); // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); } + // add a few rigid bodies + Ctor_RbUpStack(); // create a static rigid box as the ground { // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); @@ -263,8 +266,7 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; - // add a few rigid bodies - // Ctor_RbUpStack(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); m_dynamicsWorld->setGravity(gravity); } diff --git a/examples/ReducedDeformableDemo/FrictionSlope.cpp b/examples/ReducedDeformableDemo/FrictionSlope.cpp index 81de87b94..762f3ab4a 100644 --- a/examples/ReducedDeformableDemo/FrictionSlope.cpp +++ b/examples/ReducedDeformableDemo/FrictionSlope.cpp @@ -30,10 +30,10 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0; +static btScalar damping_beta = 0.001; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 1; +static int num_modes = 20; class FrictionSlope : public CommonDeformableBodyBase { @@ -64,9 +64,10 @@ public: void Ctor_RbUpStack() { - float mass = 10; + float mass = 1; + btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); // btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5)); - btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2)); + // btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2)); btTransform startTransform; startTransform.setIdentity(); @@ -82,9 +83,9 @@ public: btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0)); + // groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0)); groundTransform.setOrigin(btVector3(0, 0, 0)); - btScalar mass(0); + btScalar mass(1e6); btRigidBody* ground = createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); // ground->setFriction(1); } @@ -108,15 +109,14 @@ public: // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag); btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); - for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) - { - deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); - } - // static int num = 0; - for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) - { - deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); - } + // for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) + // { + // deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); + // } + // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) + // { + // deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); + // } } } } @@ -191,13 +191,14 @@ void FrictionSlope::initPhysics() btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); getDeformableDynamicsWorld()->addSoftBody(rsb); - rsb->getCollisionShape()->setMargin(0.1); + rsb->getCollisionShape()->setMargin(0.01); btTransform init_transform; init_transform.setIdentity(); init_transform.setOrigin(btVector3(0, 4, 0)); + init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 2.0)); rsb->transform(init_transform); - rsb->setStiffnessScale(10); + rsb->setStiffnessScale(50); rsb->setDamping(damping_alpha, damping_beta); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -215,14 +216,14 @@ void FrictionSlope::initPhysics() getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); - getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->setUseProjection(false); getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2; - getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.6; + getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; - getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; - // getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion); + getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion); m_dynamicsWorld->setGravity(gravity); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/ReducedDeformableDemo/ReducedCollide.cpp b/examples/ReducedDeformableDemo/ReducedCollide.cpp index d053be92b..112b4c8b8 100644 --- a/examples/ReducedDeformableDemo/ReducedCollide.cpp +++ b/examples/ReducedDeformableDemo/ReducedCollide.cpp @@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.0001; static btScalar COLLIDING_VELOCITY = 4; static int start_mode = 6; -static int num_modes = 20; +static int num_modes = 40; class ReducedCollide : public CommonDeformableBodyBase { @@ -133,7 +133,7 @@ void ReducedCollide::initPhysics() init_transform.setOrigin(btVector3(0, 2, 0)); rsb->transform(init_transform); - rsb->setStiffnessScale(25); + rsb->setStiffnessScale(10); rsb->setDamping(damping_alpha, damping_beta); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/ReducedDeformableDemo/ReducedGrasp.cpp b/examples/ReducedDeformableDemo/ReducedGrasp.cpp index 50b34a6dc..81c88db6e 100644 --- a/examples/ReducedDeformableDemo/ReducedGrasp.cpp +++ b/examples/ReducedDeformableDemo/ReducedGrasp.cpp @@ -119,10 +119,10 @@ public: btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); } - for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) - { - deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); - } + // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p) + // { + // deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); + // } } } diff --git a/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp b/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp index 1621a2127..433ae524f 100644 --- a/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp +++ b/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp @@ -75,6 +75,19 @@ public: void exitPhysics(); + void Ctor_RbUpStack() + { + float mass = 8; + btCollisionShape* shape = new btBoxShape(btVector3(2, 0.25, 0.5)); + btTransform startTransform; + startTransform.setIdentity(); + + startTransform.setOrigin(btVector3(0,0.25,0)); + btRigidBody* rb1 = createRigidBody(mass, startTransform, shape); + rb1->setLinearVelocity(btVector3(0, 0, 0)); + rb1->setFriction(0.7); + } + void resetCamera() { // float dist = 0.3; @@ -205,7 +218,8 @@ void ReducedMotorGrasp::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); // btVector3 gravity = btVector3(0, 0, 0); - btVector3 gravity = btVector3(0, -9.81, 0); + // btVector3 gravity = btVector3(0, -9.81, 0); + btVector3 gravity = btVector3(0, 1, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -326,6 +340,8 @@ void ReducedMotorGrasp::initPhysics() rsb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(rsb); } + + // Ctor_RbUpStack(); getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index 85dece64c..37dd1ecb5 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -92,8 +92,8 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // btVector3 deltaVa = Va - m_bufferVelocityA; // if (!m_collideStatic) // { - std::cout << "moving collision!!!\n"; - std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n"; + // std::cout << "moving collision!!!\n"; + // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n"; // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t' // << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t' // << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n'; @@ -101,11 +101,11 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont btVector3 deltaVa = getDeltaVa(); btVector3 deltaVb = getDeltaVb(); - if (!m_collideStatic) - { - std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; - std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; - } + // if (!m_collideStatic) + // { + // std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; + // std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; + // } // get delta relative velocity and magnitude (i.e., how much impulse has been applied?) btVector3 deltaV_rel = deltaVa - deltaVb; @@ -113,17 +113,17 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // if (!m_collideStatic) // { - std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n"; - std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; - std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; + // std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n"; + // std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; + // std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; // } // get the normal impulse to be applied btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor; // if (!m_collideStatic) // { - std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n"; - std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n'; + // std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n"; + // std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n'; // } { @@ -132,7 +132,6 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // if the cumulative impulse is pushing the object into the rigid body, set it zero if (sum < 0) { - if (!m_collideStatic) std::cout <<"set zeroed!!!\n"; deltaImpulse = -m_appliedNormalImpulse; m_appliedNormalImpulse = 0; } @@ -144,8 +143,8 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // if (!m_collideStatic) // { - std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; - std::cout << "deltaImpulse: " << deltaImpulse << '\n'; + // std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; + // std::cout << "deltaImpulse: " << deltaImpulse << '\n'; // } // residual is the nodal normal velocity change in current iteration @@ -179,10 +178,10 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont m_appliedTangentImpulse = sum; } - std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n"; - std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n'; - std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n'; - std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n'; + // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n"; + // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n'; + // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n'; + // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n'; } // get the total impulse vector @@ -195,35 +194,35 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // apply impulse to the rigid/multibodies involved and change their velocities if (!m_collideStatic) { - std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t' - << m_linearComponentNormal[1] << '\t' - << m_linearComponentNormal[2] << '\n'; - std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t' - << m_angularComponentNormal[1] << '\t' - << m_angularComponentNormal[2] << '\n'; + // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t' + // << m_linearComponentNormal[1] << '\t' + // << m_linearComponentNormal[2] << '\n'; + // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t' + // << m_angularComponentNormal[1] << '\t' + // << m_angularComponentNormal[2] << '\n'; const btSoftBody::sCti& cti = m_contact->m_cti; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - std::cout << "rigid impulse applied!!\n"; - std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' - << m_solverBody->getDeltaLinearVelocity()[1] << '\t' - << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; - std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' - << m_solverBody->getDeltaAngularVelocity()[1] << '\t' - << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + // std::cout << "rigid impulse applied!!\n"; + // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse); m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent); - std::cout << "after\n"; - std::cout << "rigid impulse applied!!\n"; - std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' - << m_solverBody->getDeltaLinearVelocity()[1] << '\t' - << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; - std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' - << m_solverBody->getDeltaAngularVelocity()[1] << '\t' - << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + // std::cout << "after\n"; + // std::cout << "rigid impulse applied!!\n"; + // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -233,15 +232,15 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont { const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); - const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); - } + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, deltaImpulse); + // if (impulse_tangent.norm() > SIMD_EPSILON) + // { + // // apply tangential component of the impulse + // const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + // const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + // } } } } @@ -382,10 +381,10 @@ void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3 // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); if (!m_collideStatic) { - std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; - std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; + // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; + // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; btVector3 v_after = getDeltaVb() + m_node->m_v; - std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; + // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; } // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index fae324575..48eab5d9d 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1731,7 +1731,16 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + + btMatrix3x3 local_impulse_matrix; + if (psb->m_reducedModel) + { + local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof); + } + else + { + local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + } c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1;