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