PhysicsHingeConstraint::PhysicsHingeConstraint(PhysicsRigidBody* a, const kmQuaternion& rotationOffsetA, const kmVec3& translationOffsetA,
PhysicsRigidBody* b, const kmQuaternion& rotationOffsetB, const kmVec3& translationOffsetB)
: PhysicsConstraint(a, b)
{
GP_ASSERT(a && a->_body && a->getNode());
// Take scale into account for the first node's translation offset.
kmVec3 sA = vec3Zero;
//a->getNode()->getWorldMatrix().getScale(&sA);
kmMat4Decompose(&a->getNode()->getWorldMatrix(), &sA, NULL, NULL);
kmVec3 tA = { translationOffsetA.x * sA.x, translationOffsetA.y * sA.y, translationOffsetA.z * sA.z };
if (b)
{
GP_ASSERT(b->_body && b->getNode());
// Take scale into account for the second node's translation offset.
kmVec3 sB = vec3Zero;
//b->getNode()->getWorldMatrix().getScale(&sB);
kmMat4Decompose(&b->getNode()->getWorldMatrix(), &sB, NULL, NULL);
kmVec3 tB = { translationOffsetB.x * sB.x, translationOffsetB.y * sB.y, translationOffsetB.z * sB.z };
btTransform frameInA(BQ(rotationOffsetA), BV(tA));
btTransform frameInB(BQ(rotationOffsetB), BV(tB));
_constraint = bullet_new<btHingeConstraint>(*a->_body, *b->_body, frameInA, frameInB);
}
else
{
btTransform frameInA(BQ(rotationOffsetA), BV(tA));
_constraint = bullet_new<btHingeConstraint>(*a->_body, frameInA);
}
}
PhysicsHingeConstraint::PhysicsHingeConstraint(PhysicsRigidBody* a, const Quaternion& rotationOffsetA, const Vector3& translationOffsetA,
PhysicsRigidBody* b, const Quaternion& rotationOffsetB, const Vector3& translationOffsetB)
: PhysicsConstraint(a, b)
{
GP_ASSERT(a && a->_body && a->getNode());
// Take scale into account for the first node's translation offset.
Vector3 sA;
a->getNode()->getWorldMatrix().getScale(&sA);
Vector3 tA(translationOffsetA.x * sA.x, translationOffsetA.y * sA.y, translationOffsetA.z * sA.z);
if (b)
{
GP_ASSERT(b->_body && b->getNode());
// Take scale into account for the second node's translation offset.
Vector3 sB;
b->getNode()->getWorldMatrix().getScale(&sB);
Vector3 tB(translationOffsetB.x * sB.x, translationOffsetB.y * sB.y, translationOffsetB.z * sB.z);
btTransform frameInA(BQ(rotationOffsetA), BV(tA));
btTransform frameInB(BQ(rotationOffsetB), BV(tB));
_constraint = new btHingeConstraint(*a->_body, *b->_body, frameInA, frameInB);
}
else
{
btTransform frameInA(BQ(rotationOffsetA), BV(tA));
_constraint = new btHingeConstraint(*a->_body, frameInA);
}
}
// -------------------------------------------------------------------------
SixDofConstraint::SixDofConstraint(RigidBody *rbA,
RigidBody *rbB,
const Vector3 &FrameInAVector3,
const Quaternion &FrameInAOrientation,
const Vector3 &FrameInBVector3,
const Quaternion &FrameInBOrientation)
: TypedConstraint(rbA, rbB)
{
btTransform frameInA(OgreBulletCollisions::convert(FrameInAOrientation),
OgreBulletCollisions::convert(FrameInAVector3));
btTransform frameInB(OgreBulletCollisions::convert(FrameInBOrientation),
OgreBulletCollisions::convert(FrameInBVector3));
mConstraint = new btGeneric6DofConstraint(*rbA->getBulletRigidBody(),
*rbB->getBulletRigidBody(),
frameInA,
frameInB,
true); // Eric added this because Bullet 2.61 has a new argument (useLinearReferenceFrameA)
}
btTypedConstraint* ObjectConstraintSlider::getConstraint() {
btSliderConstraint* constraint { nullptr };
QUuid otherEntityID;
glm::vec3 pointInA;
glm::vec3 axisInA;
glm::vec3 pointInB;
glm::vec3 axisInB;
withReadLock([&]{
constraint = static_cast<btSliderConstraint*>(_constraint);
pointInA = _pointInA;
axisInA = _axisInA;
otherEntityID = _otherID;
pointInB = _pointInB;
axisInB = _axisInB;
});
if (constraint) {
return constraint;
}
static int repeatMessageID = LogHandler::getInstance().newRepeatedMessageID();
btRigidBody* rigidBodyA = getRigidBody();
if (!rigidBodyA) {
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintSlider::getConstraint -- no rigidBodyA");
return nullptr;
}
if (glm::length(axisInA) < FLT_EPSILON) {
qCWarning(physics) << "slider axis cannot be a zero vector";
axisInA = DEFAULT_SLIDER_AXIS;
} else {
axisInA = glm::normalize(axisInA);
}
if (!otherEntityID.isNull()) {
// This slider is between two entities... find the other rigid body.
if (glm::length(axisInB) < FLT_EPSILON) {
qCWarning(physics) << "slider axis cannot be a zero vector";
axisInB = DEFAULT_SLIDER_AXIS;
} else {
axisInB = glm::normalize(axisInB);
}
glm::quat rotA = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA);
glm::quat rotB = glm::rotation(DEFAULT_SLIDER_AXIS, axisInB);
btTransform frameInA(glmToBullet(rotA), glmToBullet(pointInA));
btTransform frameInB(glmToBullet(rotB), glmToBullet(pointInB));
btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
if (!rigidBodyB) {
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintSlider::getConstraint -- no rigidBodyB");
return nullptr;
}
constraint = new btSliderConstraint(*rigidBodyA, *rigidBodyB, frameInA, frameInB, true);
} else {
// This slider is between an entity and the world-frame.
glm::quat rot = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA);
btTransform frameInA(glmToBullet(rot), glmToBullet(pointInA));
constraint = new btSliderConstraint(*rigidBodyA, frameInA, true);
}
withWriteLock([&]{
_constraint = constraint;
});
// if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
forceBodyNonStatic();
activateBody();
updateSlider();
return constraint;
}
