From 6623028d71df16b5f45e27d48bcca3039c11368e Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Sat, 29 Apr 2017 08:35:55 -0700 Subject: [PATCH] ball-and-socket constraint --- interface/src/InterfaceDynamicFactory.cpp | 3 + .../entities/src/EntityDynamicInterface.cpp | 5 + .../entities/src/EntityDynamicInterface.h | 3 +- .../networking/src/udt/PacketHeaders.cpp | 2 +- libraries/networking/src/udt/PacketHeaders.h | 1 + .../src/ObjectConstraintBallSocket.cpp | 240 ++++++++++++++++++ .../physics/src/ObjectConstraintBallSocket.h | 46 ++++ .../physics/src/ObjectConstraintSlider.cpp | 1 - 8 files changed, 298 insertions(+), 3 deletions(-) create mode 100644 libraries/physics/src/ObjectConstraintBallSocket.cpp create mode 100644 libraries/physics/src/ObjectConstraintBallSocket.h diff --git a/interface/src/InterfaceDynamicFactory.cpp b/interface/src/InterfaceDynamicFactory.cpp index 71f6b572e0..5fe938e861 100644 --- a/interface/src/InterfaceDynamicFactory.cpp +++ b/interface/src/InterfaceDynamicFactory.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "InterfaceDynamicFactory.h" @@ -41,6 +42,8 @@ EntityDynamicPointer interfaceDynamicFactory(EntityDynamicType type, const QUuid return std::make_shared(id, ownerEntity); case DYNAMIC_TYPE_SLIDER: return std::make_shared(id, ownerEntity); + case DYNAMIC_TYPE_BALL_SOCKET: + 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 da5ab7d8c4..89bba4379f 100644 --- a/libraries/entities/src/EntityDynamicInterface.cpp +++ b/libraries/entities/src/EntityDynamicInterface.cpp @@ -120,6 +120,9 @@ EntityDynamicType EntityDynamicInterface::dynamicTypeFromString(QString dynamicT if (normalizedDynamicTypeString == "slider") { return DYNAMIC_TYPE_SLIDER; } + if (normalizedDynamicTypeString == "ballsocket") { + return DYNAMIC_TYPE_BALL_SOCKET; + } qCDebug(entities) << "Warning -- EntityDynamicInterface::dynamicTypeFromString got unknown dynamic-type name" << dynamicTypeString; @@ -144,6 +147,8 @@ QString EntityDynamicInterface::dynamicTypeToString(EntityDynamicType dynamicTyp return "far-grab"; case DYNAMIC_TYPE_SLIDER: return "slider"; + case DYNAMIC_TYPE_BALL_SOCKET: + return "ball-socket"; } assert(false); return "none"; diff --git a/libraries/entities/src/EntityDynamicInterface.h b/libraries/entities/src/EntityDynamicInterface.h index cfb82dbdf8..28b1d89b99 100644 --- a/libraries/entities/src/EntityDynamicInterface.h +++ b/libraries/entities/src/EntityDynamicInterface.h @@ -32,7 +32,8 @@ enum EntityDynamicType { DYNAMIC_TYPE_TRAVEL_ORIENTED = 4000, DYNAMIC_TYPE_HINGE = 5000, DYNAMIC_TYPE_FAR_GRAB = 6000, - DYNAMIC_TYPE_SLIDER = 7000 + DYNAMIC_TYPE_SLIDER = 7000, + DYNAMIC_TYPE_BALL_SOCKET = 8000 }; diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp index 082f7ade1e..ce51ccd019 100644 --- a/libraries/networking/src/udt/PacketHeaders.cpp +++ b/libraries/networking/src/udt/PacketHeaders.cpp @@ -49,7 +49,7 @@ PacketVersion versionForPacketType(PacketType packetType) { case PacketType::EntityEdit: case PacketType::EntityData: case PacketType::EntityPhysics: - return VERSION_ENTITIES_SLIDER_CONSTRAINT; + return VERSION_ENTITIES_MORE_CONSTRAINTS; case PacketType::EntityQuery: return static_cast(EntityQueryPacketVersion::JSONFilterWithFamilyTree); case PacketType::AvatarIdentity: diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h index 80646251a9..f84b16818a 100644 --- a/libraries/networking/src/udt/PacketHeaders.h +++ b/libraries/networking/src/udt/PacketHeaders.h @@ -209,6 +209,7 @@ const PacketVersion VERSION_ENTITIES_PHYSICS_PACKET = 67; const PacketVersion VERSION_ENTITIES_ZONE_FILTERS = 68; const PacketVersion VERSION_ENTITIES_HINGE_CONSTRAINT = 69; const PacketVersion VERSION_ENTITIES_SLIDER_CONSTRAINT = 70; +const PacketVersion VERSION_ENTITIES_MORE_CONSTRAINTS = 71; enum class EntityQueryPacketVersion: PacketVersion { JSONFilter = 18, diff --git a/libraries/physics/src/ObjectConstraintBallSocket.cpp b/libraries/physics/src/ObjectConstraintBallSocket.cpp new file mode 100644 index 0000000000..35f138e840 --- /dev/null +++ b/libraries/physics/src/ObjectConstraintBallSocket.cpp @@ -0,0 +1,240 @@ +// +// ObjectConstraintBallSocket.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 "ObjectConstraintBallSocket.h" +#include "PhysicsLogging.h" + + +const uint16_t ObjectConstraintBallSocket::constraintVersion = 1; + + +ObjectConstraintBallSocket::ObjectConstraintBallSocket(const QUuid& id, EntityItemPointer ownerEntity) : + ObjectConstraint(DYNAMIC_TYPE_BALL_SOCKET, id, ownerEntity), + _pivotInA(glm::vec3(0.0f)), + _pivotInB(glm::vec3(0.0f)) +{ + #if WANT_DEBUG + qCDebug(physics) << "ObjectConstraintBallSocket::ObjectConstraintBallSocket"; + #endif +} + +ObjectConstraintBallSocket::~ObjectConstraintBallSocket() { + #if WANT_DEBUG + qCDebug(physics) << "ObjectConstraintBallSocket::~ObjectConstraintBallSocket"; + #endif +} + +QList ObjectConstraintBallSocket::getRigidBodies() { + QList result; + result += getRigidBody(); + QUuid otherEntityID; + withReadLock([&]{ + otherEntityID = _otherEntityID; + }); + if (!otherEntityID.isNull()) { + result += getOtherRigidBody(otherEntityID); + } + return result; +} + +void ObjectConstraintBallSocket::prepareForPhysicsSimulation() { +} + +void ObjectConstraintBallSocket::updateBallSocket() { + btPoint2PointConstraint* constraint { nullptr }; + + withReadLock([&]{ + constraint = static_cast(_constraint); + }); + + if (!constraint) { + return; + } + + constraint->setPivotA(glmToBullet(_pivotInA)); + constraint->setPivotB(glmToBullet(_pivotInB)); +} + + +btTypedConstraint* ObjectConstraintBallSocket::getConstraint() { + btPoint2PointConstraint* constraint { nullptr }; + QUuid otherEntityID; + glm::vec3 pivotInA; + glm::vec3 pivotInB; + + withReadLock([&]{ + constraint = static_cast(_constraint); + pivotInA = _pivotInA; + otherEntityID = _otherEntityID; + pivotInB = _pivotInB; + }); + if (constraint) { + return constraint; + } + + btRigidBody* rigidBodyA = getRigidBody(); + if (!rigidBodyA) { + qCDebug(physics) << "ObjectConstraintBallSocket::getConstraint -- no rigidBodyA"; + return nullptr; + } + + if (!otherEntityID.isNull()) { + // This constraint is between two entities... find the other rigid body. + + btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID); + if (!rigidBodyB) { + return nullptr; + } + + constraint = new btPoint2PointConstraint(*rigidBodyA, *rigidBodyB, glmToBullet(pivotInA), glmToBullet(pivotInB)); + } else { + // This constraint is between an entity and the world-frame. + + constraint = new btPoint2PointConstraint(*rigidBodyA, glmToBullet(pivotInA)); + } + + withWriteLock([&]{ + _constraint = constraint; + }); + + // if we don't wake up rigidBodyA, we may not send the dynamicData property over the network + forceBodyNonStatic(); + activateBody(); + + updateBallSocket(); + + return constraint; +} + + +bool ObjectConstraintBallSocket::updateArguments(QVariantMap arguments) { + glm::vec3 pivotInA; + QUuid otherEntityID; + glm::vec3 pivotInB; + + bool needUpdate = false; + bool somethingChanged = ObjectDynamic::updateArguments(arguments); + withReadLock([&]{ + bool ok = true; + pivotInA = EntityDynamicInterface::extractVec3Argument("ball-socket constraint", arguments, "pivot", ok, false); + if (!ok) { + pivotInA = _pivotInA; + } + + ok = true; + otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("ball-socket constraint", + arguments, "otherEntityID", ok, false)); + if (!ok) { + otherEntityID = _otherEntityID; + } + + ok = true; + pivotInB = EntityDynamicInterface::extractVec3Argument("ball-socket constraint", arguments, "otherPivot", ok, false); + if (!ok) { + pivotInB = _pivotInB; + } + + if (somethingChanged || + pivotInA != _pivotInA || + otherEntityID != _otherEntityID || + pivotInB != _pivotInB) { + // something changed + needUpdate = true; + } + }); + + if (needUpdate) { + withWriteLock([&] { + _pivotInA = pivotInA; + _otherEntityID = otherEntityID; + _pivotInB = pivotInB; + + _active = true; + + auto ownerEntity = _ownerEntity.lock(); + if (ownerEntity) { + ownerEntity->setDynamicDataDirty(true); + ownerEntity->setDynamicDataNeedsTransmit(true); + } + }); + + updateBallSocket(); + } + + return true; +} + +QVariantMap ObjectConstraintBallSocket::getArguments() { + QVariantMap arguments = ObjectDynamic::getArguments(); + withReadLock([&] { + if (_constraint) { + arguments["pivot"] = glmToQMap(_pivotInA); + arguments["otherEntityID"] = _otherEntityID; + arguments["otherPivot"] = glmToQMap(_pivotInB); + } + }); + return arguments; +} + +QByteArray ObjectConstraintBallSocket::serialize() const { + QByteArray serializedConstraintArguments; + QDataStream dataStream(&serializedConstraintArguments, QIODevice::WriteOnly); + + dataStream << DYNAMIC_TYPE_BALL_SOCKET; + dataStream << getID(); + dataStream << ObjectConstraintBallSocket::constraintVersion; + + withReadLock([&] { + dataStream << localTimeToServerTime(_expires); + dataStream << _tag; + + dataStream << _pivotInA; + dataStream << _otherEntityID; + dataStream << _pivotInB; + }); + + return serializedConstraintArguments; +} + +void ObjectConstraintBallSocket::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 != ObjectConstraintBallSocket::constraintVersion) { + assert(false); + return; + } + + withWriteLock([&] { + quint64 serverExpires; + dataStream >> serverExpires; + _expires = serverTimeToLocalTime(serverExpires); + dataStream >> _tag; + + dataStream >> _pivotInA; + dataStream >> _otherEntityID; + dataStream >> _pivotInB; + + _active = true; + }); +} diff --git a/libraries/physics/src/ObjectConstraintBallSocket.h b/libraries/physics/src/ObjectConstraintBallSocket.h new file mode 100644 index 0000000000..9e0b942a6f --- /dev/null +++ b/libraries/physics/src/ObjectConstraintBallSocket.h @@ -0,0 +1,46 @@ +// +// ObjectConstraintBallSocket.h +// 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 +// + +#ifndef hifi_ObjectConstraintBallSocket_h +#define hifi_ObjectConstraintBallSocket_h + +#include "ObjectConstraint.h" + +// http://bulletphysics.org/Bullet/BulletFull/classbtBallSocketConstraint.html + +class ObjectConstraintBallSocket : public ObjectConstraint { +public: + ObjectConstraintBallSocket(const QUuid& id, EntityItemPointer ownerEntity); + virtual ~ObjectConstraintBallSocket(); + + 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 updateBallSocket(); + + glm::vec3 _pivotInA; + + EntityItemID _otherEntityID; + glm::vec3 _pivotInB; +}; + +#endif // hifi_ObjectConstraintBallSocket_h diff --git a/libraries/physics/src/ObjectConstraintSlider.cpp b/libraries/physics/src/ObjectConstraintSlider.cpp index fb8d8b9f68..8a722d1744 100644 --- a/libraries/physics/src/ObjectConstraintSlider.cpp +++ b/libraries/physics/src/ObjectConstraintSlider.cpp @@ -55,7 +55,6 @@ void ObjectConstraintSlider::updateSlider() { btSliderConstraint* constraint { nullptr }; withReadLock([&]{ - // TODO -- write this constraint = static_cast(_constraint); });