From 4c4e0dffcc652308ecd59e7ddadf8aaed40294e6 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Sat, 29 Apr 2017 12:30:12 -0700 Subject: [PATCH] bullet cone-twist constraint --- interface/src/InterfaceDynamicFactory.cpp | 3 + .../entities/src/EntityDynamicInterface.cpp | 5 + .../entities/src/EntityDynamicInterface.h | 3 +- .../physics/src/ObjectConstraintConeTwist.cpp | 371 ++++++++++++++++++ .../physics/src/ObjectConstraintConeTwist.h | 55 +++ 5 files changed, 436 insertions(+), 1 deletion(-) create mode 100644 libraries/physics/src/ObjectConstraintConeTwist.cpp create mode 100644 libraries/physics/src/ObjectConstraintConeTwist.h diff --git a/interface/src/InterfaceDynamicFactory.cpp b/interface/src/InterfaceDynamicFactory.cpp index 5fe938e861..5acc0700c0 100644 --- a/interface/src/InterfaceDynamicFactory.cpp +++ b/interface/src/InterfaceDynamicFactory.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "InterfaceDynamicFactory.h" @@ -44,6 +45,8 @@ EntityDynamicPointer interfaceDynamicFactory(EntityDynamicType type, const QUuid return std::make_shared(id, ownerEntity); case DYNAMIC_TYPE_BALL_SOCKET: return std::make_shared(id, ownerEntity); + case DYNAMIC_TYPE_CONE_TWIST: + return std::make_shared(id, ownerEntity); } qDebug() << "Unknown entity dynamic type"; diff --git a/libraries/entities/src/EntityDynamicInterface.cpp b/libraries/entities/src/EntityDynamicInterface.cpp index 89bba4379f..57f86105b2 100644 --- a/libraries/entities/src/EntityDynamicInterface.cpp +++ b/libraries/entities/src/EntityDynamicInterface.cpp @@ -123,6 +123,9 @@ EntityDynamicType EntityDynamicInterface::dynamicTypeFromString(QString dynamicT if (normalizedDynamicTypeString == "ballsocket") { return DYNAMIC_TYPE_BALL_SOCKET; } + if (normalizedDynamicTypeString == "conetwist") { + return DYNAMIC_TYPE_CONE_TWIST; + } qCDebug(entities) << "Warning -- EntityDynamicInterface::dynamicTypeFromString got unknown dynamic-type name" << dynamicTypeString; @@ -149,6 +152,8 @@ QString EntityDynamicInterface::dynamicTypeToString(EntityDynamicType dynamicTyp return "slider"; case DYNAMIC_TYPE_BALL_SOCKET: return "ball-socket"; + case DYNAMIC_TYPE_CONE_TWIST: + return "cone-twist"; } assert(false); return "none"; diff --git a/libraries/entities/src/EntityDynamicInterface.h b/libraries/entities/src/EntityDynamicInterface.h index 28b1d89b99..c04aaf67b2 100644 --- a/libraries/entities/src/EntityDynamicInterface.h +++ b/libraries/entities/src/EntityDynamicInterface.h @@ -33,7 +33,8 @@ enum EntityDynamicType { DYNAMIC_TYPE_HINGE = 5000, DYNAMIC_TYPE_FAR_GRAB = 6000, DYNAMIC_TYPE_SLIDER = 7000, - DYNAMIC_TYPE_BALL_SOCKET = 8000 + DYNAMIC_TYPE_BALL_SOCKET = 8000, + DYNAMIC_TYPE_CONE_TWIST = 9000 }; diff --git a/libraries/physics/src/ObjectConstraintConeTwist.cpp b/libraries/physics/src/ObjectConstraintConeTwist.cpp new file mode 100644 index 0000000000..7a7120cd09 --- /dev/null +++ b/libraries/physics/src/ObjectConstraintConeTwist.cpp @@ -0,0 +1,371 @@ +// +// ObjectConstraintConeTwist.cpp +// libraries/physics/src +// +// Created by Seth Alves 2017-4-29 +// Copyright 2017 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "QVariantGLM.h" + +#include "EntityTree.h" +#include "ObjectConstraintConeTwist.h" +#include "PhysicsLogging.h" + + +const uint16_t ObjectConstraintConeTwist::constraintVersion = 1; + + +ObjectConstraintConeTwist::ObjectConstraintConeTwist(const QUuid& id, EntityItemPointer ownerEntity) : + ObjectConstraint(DYNAMIC_TYPE_CONE_TWIST, id, ownerEntity), + _pivotInA(glm::vec3(0.0f)), + _axisInA(glm::vec3(0.0f)) +{ + #if WANT_DEBUG + qCDebug(physics) << "ObjectConstraintConeTwist::ObjectConstraintConeTwist"; + #endif +} + +ObjectConstraintConeTwist::~ObjectConstraintConeTwist() { + #if WANT_DEBUG + qCDebug(physics) << "ObjectConstraintConeTwist::~ObjectConstraintConeTwist"; + #endif +} + +QList ObjectConstraintConeTwist::getRigidBodies() { + QList result; + result += getRigidBody(); + QUuid otherEntityID; + withReadLock([&]{ + otherEntityID = _otherEntityID; + }); + if (!otherEntityID.isNull()) { + result += getOtherRigidBody(otherEntityID); + } + return result; +} + +void ObjectConstraintConeTwist::prepareForPhysicsSimulation() { +} + +void ObjectConstraintConeTwist::updateConeTwist() { + btConeTwistConstraint* constraint { nullptr }; + float swingSpan1; + float swingSpan2; + float twistSpan; + float softness; + float biasFactor; + float relaxationFactor; + + withReadLock([&]{ + constraint = static_cast(_constraint); + swingSpan1 = _swingSpan1; + swingSpan2 = _swingSpan2; + twistSpan = _twistSpan; + softness = _softness; + biasFactor = _biasFactor; + relaxationFactor = _relaxationFactor; + }); + + if (!constraint) { + return; + } + + constraint->setLimit(swingSpan1, + swingSpan2, + twistSpan, + softness, + biasFactor, + relaxationFactor); +} + + +btTypedConstraint* ObjectConstraintConeTwist::getConstraint() { + btConeTwistConstraint* constraint { nullptr }; + QUuid otherEntityID; + glm::vec3 pivotInA; + glm::vec3 axisInA; + glm::vec3 pivotInB; + glm::vec3 axisInB; + + withReadLock([&]{ + constraint = static_cast(_constraint); + pivotInA = _pivotInA; + axisInA = _axisInA; + otherEntityID = _otherEntityID; + pivotInB = _pivotInB; + axisInB = _axisInB; + }); + if (constraint) { + return constraint; + } + + btRigidBody* rigidBodyA = getRigidBody(); + if (!rigidBodyA) { + qCDebug(physics) << "ObjectConstraintConeTwist::getConstraint -- no rigidBodyA"; + return nullptr; + } + + if (!otherEntityID.isNull()) { + // This coneTwist is between two entities... find the other rigid body. + + glm::quat rotA = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); + glm::quat rotB = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInB)); + + btTransform frameInA(glmToBullet(rotA), glmToBullet(pivotInA)); + btTransform frameInB(glmToBullet(rotB), glmToBullet(pivotInB)); + + btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID); + if (!rigidBodyB) { + return nullptr; + } + + constraint = new btConeTwistConstraint(*rigidBodyA, *rigidBodyB, frameInA, frameInB); + } else { + // This coneTwist is between an entity and the world-frame. + + glm::quat rot = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); + + btTransform frameInA(glmToBullet(rot), glmToBullet(pivotInA)); + + constraint = new btConeTwistConstraint(*rigidBodyA, frameInA); + } + + withWriteLock([&]{ + _constraint = constraint; + }); + + // if we don't wake up rigidBodyA, we may not send the dynamicData property over the network + forceBodyNonStatic(); + activateBody(); + + updateConeTwist(); + + return constraint; +} + + +bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { + glm::vec3 pivotInA; + glm::vec3 axisInA; + QUuid otherEntityID; + glm::vec3 pivotInB; + glm::vec3 axisInB; + float swingSpan1; + float swingSpan2; + float twistSpan; + float softness; + float biasFactor; + float relaxationFactor; + + bool needUpdate = false; + bool somethingChanged = ObjectDynamic::updateArguments(arguments); + withReadLock([&]{ + bool ok = true; + pivotInA = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "pivot", ok, false); + if (!ok) { + pivotInA = _pivotInA; + } + + ok = true; + axisInA = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "axis", ok, false); + if (!ok) { + axisInA = _axisInA; + } + + ok = true; + otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("coneTwist constraint", + arguments, "otherEntityID", ok, false)); + if (!ok) { + otherEntityID = _otherEntityID; + } + + ok = true; + pivotInB = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "otherPivot", ok, false); + if (!ok) { + pivotInB = _pivotInB; + } + + ok = true; + axisInB = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "otherAxis", ok, false); + if (!ok) { + axisInB = _axisInB; + } + + ok = true; + swingSpan1 = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "swingSpan1", ok, false); + if (!ok) { + swingSpan1 = _swingSpan1; + } + + ok = true; + swingSpan2 = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "swingSpan2", ok, false); + if (!ok) { + swingSpan2 = _swingSpan2; + } + + ok = true; + twistSpan = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "twistSpan", ok, false); + if (!ok) { + twistSpan = _twistSpan; + } + + ok = true; + softness = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "softness", ok, false); + if (!ok) { + softness = _softness; + } + + ok = true; + biasFactor = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "biasFactor", ok, false); + if (!ok) { + biasFactor = _biasFactor; + } + + ok = true; + relaxationFactor = + EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "relaxationFactor", ok, false); + if (!ok) { + relaxationFactor = _relaxationFactor; + } + + if (somethingChanged || + pivotInA != _pivotInA || + axisInA != _axisInA || + otherEntityID != _otherEntityID || + pivotInB != _pivotInB || + axisInB != _axisInB || + swingSpan1 != _swingSpan1 || + swingSpan2 != _swingSpan2 || + twistSpan != _twistSpan || + softness != _softness || + biasFactor != _biasFactor || + relaxationFactor != _relaxationFactor) { + // something changed + needUpdate = true; + } + }); + + if (needUpdate) { + withWriteLock([&] { + _pivotInA = pivotInA; + _axisInA = axisInA; + _otherEntityID = otherEntityID; + _pivotInB = pivotInB; + _axisInB = axisInB; + _swingSpan1 = swingSpan1; + _swingSpan2 = swingSpan2; + _twistSpan = twistSpan; + _softness = softness; + _biasFactor = biasFactor; + _relaxationFactor = relaxationFactor; + + _active = true; + + auto ownerEntity = _ownerEntity.lock(); + if (ownerEntity) { + ownerEntity->setDynamicDataDirty(true); + ownerEntity->setDynamicDataNeedsTransmit(true); + } + }); + + updateConeTwist(); + } + + return true; +} + +QVariantMap ObjectConstraintConeTwist::getArguments() { + QVariantMap arguments = ObjectDynamic::getArguments(); + withReadLock([&] { + if (_constraint) { + arguments["pivot"] = glmToQMap(_pivotInA); + arguments["axis"] = glmToQMap(_axisInA); + arguments["otherEntityID"] = _otherEntityID; + arguments["otherPivot"] = glmToQMap(_pivotInB); + arguments["otherAxis"] = glmToQMap(_axisInB); + arguments["swingSpan1"] = _swingSpan1; + arguments["swingSpan2"] = _swingSpan2; + arguments["twistSpan"] = _twistSpan; + arguments["softness"] = _softness; + arguments["biasFactor"] = _biasFactor; + arguments["relaxationFactor"] = _relaxationFactor; + + + // arguments["linearPosition"] = static_cast(_constraint)->getLinearPos(); + // arguments["angularPosition"] = static_cast(_constraint)->getAngularPos(); + } + }); + return arguments; +} + +QByteArray ObjectConstraintConeTwist::serialize() const { + QByteArray serializedConstraintArguments; + QDataStream dataStream(&serializedConstraintArguments, QIODevice::WriteOnly); + + dataStream << DYNAMIC_TYPE_CONE_TWIST; + dataStream << getID(); + dataStream << ObjectConstraintConeTwist::constraintVersion; + + withReadLock([&] { + dataStream << localTimeToServerTime(_expires); + dataStream << _tag; + + dataStream << _pivotInA; + dataStream << _axisInA; + dataStream << _otherEntityID; + dataStream << _pivotInB; + dataStream << _axisInB; + dataStream << _swingSpan1; + dataStream << _swingSpan2; + dataStream << _twistSpan; + dataStream << _softness; + dataStream << _biasFactor; + dataStream << _relaxationFactor; + }); + + return serializedConstraintArguments; +} + +void ObjectConstraintConeTwist::deserialize(QByteArray serializedArguments) { + QDataStream dataStream(serializedArguments); + + EntityDynamicType type; + dataStream >> type; + assert(type == getType()); + + QUuid id; + dataStream >> id; + assert(id == getID()); + + uint16_t serializationVersion; + dataStream >> serializationVersion; + if (serializationVersion != ObjectConstraintConeTwist::constraintVersion) { + assert(false); + return; + } + + withWriteLock([&] { + quint64 serverExpires; + dataStream >> serverExpires; + _expires = serverTimeToLocalTime(serverExpires); + dataStream >> _tag; + + dataStream >> _pivotInA; + dataStream >> _axisInA; + dataStream >> _otherEntityID; + dataStream >> _pivotInB; + dataStream >> _axisInB; + dataStream >> _swingSpan1; + dataStream >> _swingSpan2; + dataStream >> _twistSpan; + dataStream >> _softness; + dataStream >> _biasFactor; + dataStream >> _relaxationFactor; + + _active = true; + }); +} diff --git a/libraries/physics/src/ObjectConstraintConeTwist.h b/libraries/physics/src/ObjectConstraintConeTwist.h new file mode 100644 index 0000000000..02297e2b91 --- /dev/null +++ b/libraries/physics/src/ObjectConstraintConeTwist.h @@ -0,0 +1,55 @@ +// +// ObjectConstraintConeTwist.h +// libraries/physics/src +// +// Created by Seth Alves 2017-4-23 +// Copyright 2017 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_ObjectConstraintConeTwist_h +#define hifi_ObjectConstraintConeTwist_h + +#include "ObjectConstraint.h" + +// http://bulletphysics.org/Bullet/BulletFull/classbtConeTwistConstraint.html + +class ObjectConstraintConeTwist : public ObjectConstraint { +public: + ObjectConstraintConeTwist(const QUuid& id, EntityItemPointer ownerEntity); + virtual ~ObjectConstraintConeTwist(); + + virtual void prepareForPhysicsSimulation() override; + + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; + + virtual QByteArray serialize() const override; + virtual void deserialize(QByteArray serializedArguments) override; + + virtual QList getRigidBodies() override; + virtual btTypedConstraint* getConstraint() override; + +protected: + static const uint16_t constraintVersion; + + void updateConeTwist(); + + glm::vec3 _pivotInA; + glm::vec3 _axisInA; + + EntityItemID _otherEntityID; + glm::vec3 _pivotInB; + glm::vec3 _axisInB; + + float _swingSpan1 { TWO_PI }; + float _swingSpan2 { TWO_PI };; + float _twistSpan { TWO_PI };; + float _softness { 1.0f }; + float _biasFactor {0.3f }; + float _relaxationFactor { 1.0f }; +}; + +#endif // hifi_ObjectConstraintConeTwist_h