// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = m_axis1.normalize();
btVector3 yAxis = m_axis2.normalize();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
// sei limits
setLinearLowerLimit(btVector3(0., 0., 0.));
setLinearUpperLimit(btVector3(0., 0., 0.));
setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
}
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = axis1.normalize();
btVector3 xAxis = axis2.normalize();
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
// sei limits
setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
// like front wheels of a car
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
// enable suspension
enableSpring(2, true);
setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
setDamping(2, 0.01f);
setEquilibriumPoint();
}
PhysicsInterface::JointObject Bullet::createBallAndSocketJoint(BodyObject firstBodyObject, BodyObject secondBodyObject,
const Vec3& globalAnchor, const Vec3& angularLimits)
{
if (!firstBodyObject || !secondBodyObject || firstBodyObject == secondBodyObject)
{
LOG_ERROR << "Invalid bodies";
return nullptr;
}
auto firstBody = reinterpret_cast<Body*>(firstBodyObject);
auto secondBody = reinterpret_cast<Body*>(secondBodyObject);
auto firstBodyTransform = btTransform();
auto secondBodyTransform = btTransform();
firstBody->bulletBody->getMotionState()->getWorldTransform(firstBodyTransform);
secondBody->bulletBody->getMotionState()->getWorldTransform(secondBodyTransform);
auto firstLocalAnchor =
btTransform(btQuaternion::getIdentity(), firstBodyTransform.inverse() * toBullet(globalAnchor));
auto secondLocalAnchor =
btTransform(btQuaternion::getIdentity(), secondBodyTransform.inverse() * toBullet(globalAnchor));
auto btConstraint = new btGeneric6DofConstraint(*firstBody->bulletBody, *secondBody->bulletBody, firstLocalAnchor,
secondLocalAnchor, true);
if (angularLimits != Vec3::Zero)
{
btConstraint->setAngularLowerLimit(toBullet(-angularLimits));
btConstraint->setAngularUpperLimit(toBullet(angularLimits));
}
joints_.append(new Joint(firstBody, secondBody, btConstraint));
dynamicsWorld_->addConstraint(joints_.back()->bulletConstraint, true);
return joints_.back();
}
void BConstraint::setAU(UT_Vector3 limit_ah)
{
setAngularUpperLimit( get_bullet_V3(limit_ah) );
}
void ofxBulletJoint::setAngularUpperLimit( ofVec3f a_limit ) {
setAngularUpperLimit( a_limit.x, a_limit.y, a_limit.z );
}
