Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-17 15:54:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

demo setup

Browse Source
This commit is contained in:
jingyuc
2021-09-13 22:48:08 -04:00
parent 1c8afcf54f
commit 593f387a06
10 changed files with 584 additions and 542 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build_cmake_pybullet_double.sh
View File

@@ -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

824
examples/Constraints/ConstraintDemo.cpp
View File

@@ -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);
}

53
examples/ReducedDeformableDemo/BasicTest.cpp
View File

@@ -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);
// {

60
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@@ -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);
}

43
examples/ReducedDeformableDemo/FrictionSlope.cpp
View File

@@ -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);
}

4
examples/ReducedDeformableDemo/ReducedCollide.cpp
View File

@@ -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

8
examples/ReducedDeformableDemo/ReducedGrasp.cpp
View File

@@ -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));
// }
}
}

18
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@@ -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);

103
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
View File

@@ -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';
}

11
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -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;