diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 356065a9bb..bad1d40be9 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -161,6 +161,7 @@ #include "avatar/AvatarManager.h" #include "avatar/MyHead.h" #include "avatar/AvatarPackager.h" +#include "avatar/MyCharacterController.h" #include "CrashRecoveryHandler.h" #include "CrashHandler.h" #include "devices/DdeFaceTracker.h" @@ -2713,7 +2714,14 @@ Application::~Application() { avatarManager->handleProcessedPhysicsTransaction(transaction); avatarManager->deleteAllAvatars(); - + + auto myCharacterController = getMyAvatar()->getCharacterController(); + myCharacterController->clearDetailedMotionStates(); + + myCharacterController->buildPhysicsTransaction(transaction); + _physicsEngine->processTransaction(transaction); + myCharacterController->handleProcessedPhysicsTransaction(transaction); + _physicsEngine->setCharacterController(nullptr); // the _shapeManager should have zero references @@ -6287,7 +6295,9 @@ void Application::update(float deltaTime) { avatarManager->buildPhysicsTransaction(transaction); _physicsEngine->processTransaction(transaction); avatarManager->handleProcessedPhysicsTransaction(transaction); - + myAvatar->getCharacterController()->buildPhysicsTransaction(transaction); + _physicsEngine->processTransaction(transaction); + myAvatar->getCharacterController()->handleProcessedPhysicsTransaction(transaction); myAvatar->prepareForPhysicsSimulation(); _physicsEngine->forEachDynamic([&](EntityDynamicPointer dynamic) { dynamic->prepareForPhysicsSimulation(); diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 188d7d70a7..1eb87c16f0 100755 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -43,6 +43,7 @@ #include "InterfaceLogging.h" #include "Menu.h" #include "MyAvatar.h" +#include "DebugDraw.h" #include "SceneScriptingInterface.h" // 50 times per second - target is 45hz, but this helps account for any small deviations @@ -396,21 +397,45 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact if (isInPhysics) { transaction.objectsToRemove.push_back(avatar->_motionState); avatar->_motionState = nullptr; + auto& detailedMotionStates = avatar->getDetailedMotionStates(); + for (auto& mState : detailedMotionStates) { + transaction.objectsToRemove.push_back(mState); + } + avatar->resetDetailedMotionStates(); + } else { - ShapeInfo shapeInfo; - avatar->computeShapeInfo(shapeInfo); - btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); - if (shape) { - AvatarMotionState* motionState = new AvatarMotionState(avatar, shape); - motionState->setMass(avatar->computeMass()); - avatar->_motionState = motionState; - transaction.objectsToAdd.push_back(motionState); + if (avatar->getDetailedMotionStates().size() == 0) { + avatar->createDetailedMotionStates(avatar); + for (auto dMotionState : avatar->getDetailedMotionStates()) { + transaction.objectsToAdd.push_back(dMotionState); + } + } + if (avatar->getDetailedMotionStates().size() > 0) { + ShapeInfo shapeInfo; + avatar->computeShapeInfo(shapeInfo); + btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + if (shape) { + AvatarMotionState* motionState = new AvatarMotionState(avatar, shape); + motionState->setMass(avatar->computeMass()); + avatar->_motionState = motionState; + transaction.objectsToAdd.push_back(motionState); + } else { + failedShapeBuilds.insert(avatar); + } } else { failedShapeBuilds.insert(avatar); } } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); + + auto& detailedMotionStates = avatar->getDetailedMotionStates(); + for (auto& mState : detailedMotionStates) { + if (mState) { + transaction.objectsToChange.push_back(mState); + } + } + } } _avatarsToChangeInPhysics.swap(failedShapeBuilds); @@ -510,6 +535,7 @@ void AvatarManager::deleteAllAvatars() { if (avatar != _myAvatar) { auto otherAvatar = std::static_pointer_cast(avatar); assert(!otherAvatar->_motionState); + assert(otherAvatar->getDetailedMotionStates().size() == 0); } } } @@ -602,103 +628,135 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, - const QScriptValue& avatarIdsToDiscard) { + const QScriptValue& avatarIdsToDiscard, + bool pickAgainstMesh) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); - - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, pickAgainstMesh); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, - const QVector& avatarsToDiscard) { + const QVector& avatarsToDiscard, + bool pickAgainstMesh) { RayToAvatarIntersectionResult result; if (QThread::currentThread() != thread()) { BLOCKING_INVOKE_METHOD(const_cast(this), "findRayIntersectionVector", Q_RETURN_ARG(RayToAvatarIntersectionResult, result), Q_ARG(const PickRay&, ray), Q_ARG(const QVector&, avatarsToInclude), - Q_ARG(const QVector&, avatarsToDiscard)); + Q_ARG(const QVector&, avatarsToDiscard), + Q_ARG(bool, pickAgainstMesh)); return result; } - // It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to - // do, because the transformed mesh data only exists over in GPU-land. As a compromise, this code - // intersects against the avatars capsule and then against the (T-pose) mesh. The end effect is that picking - // against the avatar is sort-of right, but you likely wont be able to pick against the arms. + PROFILE_RANGE(simulation_physics, __FUNCTION__); - // TODO -- find a way to extract transformed avatar mesh data from the rendering engine. + float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results + glm::vec3 rayDirection = glm::normalize(ray.direction); + std::vector physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(rayDirection), distance, QVector()); + if (physicsResults.size() > 0) { + glm::vec3 rayDirectionInv = { rayDirection.x != 0.0f ? 1.0f / rayDirection.x : INFINITY, + rayDirection.y != 0.0f ? 1.0f / rayDirection.y : INFINITY, + rayDirection.z != 0.0f ? 1.0f / rayDirection.z : INFINITY }; - std::vector sortedAvatars; - auto avatarHashCopy = getHashCopy(); - for (auto avatarData : avatarHashCopy) { - auto avatar = std::static_pointer_cast(avatarData); - if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatar->getID())) || - (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatar->getID()))) { - continue; - } - - float distance = FLT_MAX; -#if 0 - // if we weren't picking against the capsule, we would want to pick against the avatarBounds... - SkeletonModelPointer avatarModel = avatar->getSkeletonModel(); - AABox avatarBounds = avatarModel->getRenderableMeshBound(); - if (avatarBounds.contains(ray.origin)) { - distance = 0.0f; - } else { - float boundDistance = FLT_MAX; - BoxFace face; - glm::vec3 surfaceNormal; - if (avatarBounds.findRayIntersection(ray.origin, ray.direction, boundDistance, face, surfaceNormal)) { - distance = boundDistance; - } - } -#else - glm::vec3 start; - glm::vec3 end; - float radius; - avatar->getCapsule(start, end, radius); - findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance); -#endif - - if (distance < FLT_MAX) { - sortedAvatars.emplace_back(distance, avatar); - } - } - - if (sortedAvatars.size() > 1) { - static auto comparator = [](const SortedAvatar& left, const SortedAvatar& right) { return left.first < right.first; }; - std::sort(sortedAvatars.begin(), sortedAvatars.end(), comparator); - } - - for (auto it = sortedAvatars.begin(); it != sortedAvatars.end(); ++it) { - const SortedAvatar& sortedAvatar = *it; - // We can exit once avatarCapsuleDistance > bestDistance - if (sortedAvatar.first > result.distance) { - break; - } - - float distance = FLT_MAX; - BoxFace face; + MyCharacterController::RayAvatarResult rayAvatarResult; + AvatarPointer avatar = nullptr; + + BoxFace face = BoxFace::UNKNOWN_FACE; glm::vec3 surfaceNormal; QVariantMap extraInfo; - SkeletonModelPointer avatarModel = sortedAvatar.second->getSkeletonModel(); - if (avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction, distance, face, surfaceNormal, extraInfo, true)) { - if (distance < result.distance) { - result.intersects = true; - result.avatarID = sortedAvatar.second->getID(); - result.distance = distance; - result.face = face; - result.surfaceNormal = surfaceNormal; - result.extraInfo = extraInfo; + + for (auto &hit : physicsResults) { + auto avatarID = hit._intersectWithAvatar; + if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || + (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) { + continue; + } + + if (_myAvatar->getSessionUUID() != avatarID) { + auto avatarMap = getHashCopy(); + AvatarHash::iterator itr = avatarMap.find(avatarID); + if (itr != avatarMap.end()) { + avatar = std::static_pointer_cast(*itr); + } + } else { + avatar = _myAvatar; + } + if (!hit._isBound) { + rayAvatarResult = hit; + } else if (avatar) { + auto &multiSpheres = avatar->getMultiSphereShapes(); + if (multiSpheres.size() > 0) { + std::vector boxHits; + for (size_t i = 0; i < hit._boundJoints.size(); i++) { + assert(hit._boundJoints[i] < multiSpheres.size()); + auto &mSphere = multiSpheres[hit._boundJoints[i]]; + if (mSphere.isValid()) { + float boundDistance = FLT_MAX; + BoxFace face; + glm::vec3 surfaceNormal; + auto &bbox = mSphere.getBoundingBox(); + if (bbox.findRayIntersection(ray.origin, rayDirection, rayDirectionInv, boundDistance, face, surfaceNormal)) { + MyCharacterController::RayAvatarResult boxHit; + boxHit._distance = boundDistance; + boxHit._intersect = true; + boxHit._intersectionNormal = surfaceNormal; + boxHit._intersectionPoint = ray.origin + boundDistance * rayDirection; + boxHit._intersectWithAvatar = avatarID; + boxHit._intersectWithJoint = mSphere.getJointIndex(); + boxHits.push_back(boxHit); + } + } + } + if (boxHits.size() > 0) { + if (boxHits.size() > 1) { + std::sort(boxHits.begin(), boxHits.end(), [](const MyCharacterController::RayAvatarResult& hitA, + const MyCharacterController::RayAvatarResult& hitB) { + return hitA._distance < hitB._distance; + }); + } + rayAvatarResult = boxHits[0]; + } + } + } + if (pickAgainstMesh) { + glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint); + glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + rayDirection, rayAvatarResult._intersectWithJoint); + + auto avatarOrientation = avatar->getWorldOrientation(); + auto avatarPosition = avatar->getWorldPosition(); + + auto jointOrientation = avatarOrientation * avatar->getAbsoluteDefaultJointRotationInObjectFrame(rayAvatarResult._intersectWithJoint); + auto jointPosition = avatarPosition + (avatarOrientation * avatar->getAbsoluteDefaultJointTranslationInObjectFrame(rayAvatarResult._intersectWithJoint)); + + auto defaultFrameRayOrigin = jointPosition + jointOrientation * localRayOrigin; + auto defaultFrameRayPoint = jointPosition + jointOrientation * localRayPoint; + auto defaultFrameRayDirection = defaultFrameRayPoint - defaultFrameRayOrigin; + + if (avatar->getSkeletonModel()->findRayIntersectionAgainstSubMeshes(defaultFrameRayOrigin, defaultFrameRayDirection, distance, face, surfaceNormal, extraInfo, true, false)) { + auto newDistance = glm::length(vec3FromVariant(extraInfo["worldIntersectionPoint"]) - defaultFrameRayOrigin); + rayAvatarResult._distance = newDistance; + rayAvatarResult._intersectionPoint = ray.origin + newDistance * rayDirection; + rayAvatarResult._intersectionNormal = surfaceNormal; + extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint); + break; + } + } else if (rayAvatarResult._intersect){ + break; } } + if (rayAvatarResult._intersect) { + result.intersects = true; + result.avatarID = rayAvatarResult._intersectWithAvatar; + result.distance = rayAvatarResult._distance; + result.surfaceNormal = rayAvatarResult._intersectionNormal; + result.jointIndex = rayAvatarResult._intersectWithJoint; + result.intersection = ray.origin + rayAvatarResult._distance * rayDirection; + result.extraInfo = extraInfo; + result.face = face; + } } - - if (result.intersects) { - result.intersection = ray.origin + ray.direction * result.distance; - } - return result; } diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 7bd4e8236a..50d9e80e8b 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -29,6 +29,7 @@ #include // for SetOfEntities #include "AvatarMotionState.h" +#include "DetailedMotionState.h" #include "MyAvatar.h" #include "OtherAvatar.h" @@ -136,21 +137,25 @@ public: * @param {PickRay} ray * @param {Uuid[]} [avatarsToInclude=[]] * @param {Uuid[]} [avatarsToDiscard=[]] + * @param {boolean} pickAgainstMesh * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), - const QScriptValue& avatarIdsToDiscard = QScriptValue()); + const QScriptValue& avatarIdsToDiscard = QScriptValue(), + bool pickAgainstMesh = true); /**jsdoc * @function AvatarManager.findRayIntersectionVector * @param {PickRay} ray * @param {Uuid[]} avatarsToInclude * @param {Uuid[]} avatarsToDiscard + * @param {boolean} pickAgainstMesh * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, - const QVector& avatarsToDiscard); + const QVector& avatarsToDiscard, + bool pickAgainstMesh); /**jsdoc * @function AvatarManager.findParabolaIntersectionVector diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp new file mode 100644 index 0000000000..cec27108ca --- /dev/null +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -0,0 +1,188 @@ +// +// DetailedMotionState.cpp +// interface/src/avatar/ +// +// Created by Luis Cuenca 1/11/2019 +// Copyright 2019 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 "DetailedMotionState.h" + +#include +#include +#include +#include "MyAvatar.h" + + +DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : + ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { + assert(_avatar); + if (!_avatar->isMyAvatar()) { + _otherAvatar = std::static_pointer_cast(_avatar); + } + _type = MOTIONSTATE_TYPE_DETAILED; +} + +void DetailedMotionState::handleEasyChanges(uint32_t& flags) { + ObjectMotionState::handleEasyChanges(flags); + if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && !_body->isActive()) { + _body->activate(); + } +} + +bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) { + return ObjectMotionState::handleHardAndEasyChanges(flags, engine); +} + +DetailedMotionState::~DetailedMotionState() { + assert(_avatar); + _avatar = nullptr; +} + +// virtual +uint32_t DetailedMotionState::getIncomingDirtyFlags() { + return _body ? _dirtyFlags : 0; +} + +void DetailedMotionState::clearIncomingDirtyFlags() { + if (_body) { + _dirtyFlags = 0; + } +} + +PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const { + // TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting) + return MOTION_TYPE_KINEMATIC; +} + +// virtual and protected +const btCollisionShape* DetailedMotionState::computeNewShape() { + btCollisionShape* shape = nullptr; + if (!_avatar->isMyAvatar()) { + if (_otherAvatar != nullptr) { + shape = _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints); + } + } else { + std::shared_ptr myAvatar = std::static_pointer_cast(_avatar); + if (myAvatar) { + shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex); + } + } + return shape; +} + +// virtual +bool DetailedMotionState::isMoving() const { + return false; +} + +// virtual +void DetailedMotionState::getWorldTransform(btTransform& worldTrans) const { + worldTrans.setOrigin(glmToBullet(getObjectPosition())); + worldTrans.setRotation(glmToBullet(getObjectRotation())); +} + +// virtual +void DetailedMotionState::setWorldTransform(const btTransform& worldTrans) { + _body->setWorldTransform(worldTrans); +} + +// These pure virtual methods must be implemented for each MotionState type +// and make it possible to implement more complicated methods in this base class. + +// virtual +float DetailedMotionState::getObjectRestitution() const { + return 0.5f; +} + +// virtual +float DetailedMotionState::getObjectFriction() const { + return 0.5f; +} + +// virtual +float DetailedMotionState::getObjectLinearDamping() const { + return 0.5f; +} + +// virtual +float DetailedMotionState::getObjectAngularDamping() const { + return 0.5f; +} + +// virtual +glm::vec3 DetailedMotionState::getObjectPosition() const { + if (_otherAvatar != nullptr) { + auto bodyLOD = _otherAvatar->getBodyLOD(); + if (bodyLOD == OtherAvatar::BodyLOD::Sphere) { + return _avatar->getFitBounds().calcCenter(); + } + } + return _avatar->getJointPosition(_jointIndex); +} + +// virtual +glm::quat DetailedMotionState::getObjectRotation() const { + return _avatar->getWorldOrientation() * _avatar->getAbsoluteJointRotationInObjectFrame(_jointIndex); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectLinearVelocity() const { + return glm::vec3(0.0f); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectAngularVelocity() const { + return glm::vec3(0.0f); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectGravity() const { + return glm::vec3(0.0f); +} + +// virtual +const QUuid DetailedMotionState::getObjectID() const { + return _avatar->getSessionUUID(); +} + +QString DetailedMotionState::getName() const { + return _avatar->getName() + "_" + _jointIndex; +} + +// virtual +QUuid DetailedMotionState::getSimulatorID() const { + return _avatar->getSessionUUID(); +} + +// virtual +void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const { + group = BULLET_COLLISION_GROUP_DETAILED_AVATAR; + mask = Physics::getDefaultCollisionMask(group); +} + +// virtual +float DetailedMotionState::getMass() const { + return 0.0f; +} + +void DetailedMotionState::setRigidBody(btRigidBody* body) { + ObjectMotionState::setRigidBody(body); + if (_body) { + // remove angular dynamics from this body + _body->setAngularFactor(0.0f); + } +} + +void DetailedMotionState::setShape(const btCollisionShape* shape) { + ObjectMotionState::setShape(shape); +} + +void DetailedMotionState::forceActive() { + if (_body && !_body->isActive()) { + _body->setActivationState(ACTIVE_TAG); + } +} \ No newline at end of file diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h new file mode 100644 index 0000000000..a9b4b4bb64 --- /dev/null +++ b/interface/src/avatar/DetailedMotionState.h @@ -0,0 +1,100 @@ +// +// DetailedMotionState.h +// interface/src/avatar/ +// +// Created by Luis Cuenca 1/11/2019 +// Copyright 2019 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_DetailedMotionState_h +#define hifi_DetailedMotionState_h + +#include + +#include +#include + +#include "OtherAvatar.h" + +class DetailedMotionState : public ObjectMotionState { +public: + DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex); + + virtual void handleEasyChanges(uint32_t& flags) override; + virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override; + + virtual PhysicsMotionType getMotionType() const override { return _motionType; } + + virtual uint32_t getIncomingDirtyFlags() override; + virtual void clearIncomingDirtyFlags() override; + + virtual PhysicsMotionType computePhysicsMotionType() const override; + + virtual bool isMoving() const override; + + // this relays incoming position/rotation to the RigidBody + virtual void getWorldTransform(btTransform& worldTrans) const override; + + // this relays outgoing position/rotation to the EntityItem + virtual void setWorldTransform(const btTransform& worldTrans) override; + + + // These pure virtual methods must be implemented for each MotionState type + // and make it possible to implement more complicated methods in this base class. + + // pure virtual overrides from ObjectMotionState + virtual float getObjectRestitution() const override; + virtual float getObjectFriction() const override; + virtual float getObjectLinearDamping() const override; + virtual float getObjectAngularDamping() const override; + + virtual glm::vec3 getObjectPosition() const override; + virtual glm::quat getObjectRotation() const override; + virtual glm::vec3 getObjectLinearVelocity() const override; + virtual glm::vec3 getObjectAngularVelocity() const override; + virtual glm::vec3 getObjectGravity() const override; + + virtual const QUuid getObjectID() const override; + + virtual QString getName() const override; + virtual QUuid getSimulatorID() const override; + + void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; } + + virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override; + + virtual float getMass() const override; + void forceActive(); + QUuid getAvatarID() const { return _avatar->getID(); } + int getJointIndex() const { return _jointIndex; } + void setIsBound(bool isBound, std::vector boundJoints) { _isBound = isBound; _boundJoints = boundJoints; } + bool getIsBound(std::vector& boundJoints) const { boundJoints = _boundJoints; return _isBound; } + + friend class AvatarManager; + friend class Avatar; + +protected: + void setRigidBody(btRigidBody* body) override; + void setShape(const btCollisionShape* shape) override; + + // the dtor had been made protected to force the compiler to verify that it is only + // ever called by the Avatar class dtor. + ~DetailedMotionState(); + + virtual bool isReadyToComputeShape() const override { return true; } + virtual const btCollisionShape* computeNewShape() override; + + AvatarPointer _avatar; + float _diameter { 0.0f }; + + uint32_t _dirtyFlags; + int _jointIndex { -1 }; + OtherAvatarPointer _otherAvatar { nullptr }; + bool _isBound { false }; + std::vector _boundJoints; +}; + +#endif // hifi_DetailedMotionState_h diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c0c259c0bc..92d9270d20 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -134,7 +134,7 @@ MyAvatar::MyAvatar(QThread* thread) : _scriptedMotorFrame(SCRIPTED_MOTOR_CAMERA_FRAME), _scriptedMotorMode(SCRIPTED_MOTOR_SIMPLE_MODE), _motionBehaviors(AVATAR_MOTION_DEFAULTS), - _characterController(this), + _characterController(std::shared_ptr(this)), _eyeContactTarget(LEFT_EYE), _realWorldFieldOfView("realWorldFieldOfView", DEFAULT_REAL_WORLD_FIELD_OF_VIEW_DEGREES), @@ -1171,77 +1171,6 @@ controller::Pose MyAvatar::getRightHandTipPose() const { return pose; } -glm::vec3 MyAvatar::worldToJointPoint(const glm::vec3& position, const int jointIndex) const { - glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - if (jointIndex != -1) { - if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) { - _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot); - } else { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - } - glm::vec3 modelOffset = position - jointPos; - glm::vec3 jointSpacePosition = glm::inverse(jointRot) * modelOffset; - - return jointSpacePosition; -} - -glm::vec3 MyAvatar::worldToJointDirection(const glm::vec3& worldDir, const int jointIndex) const { - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - - glm::vec3 jointSpaceDir = glm::inverse(jointRot) * worldDir; - return jointSpaceDir; -} - -glm::quat MyAvatar::worldToJointRotation(const glm::quat& worldRot, const int jointIndex) const { - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - glm::quat jointSpaceRot = glm::inverse(jointRot) * worldRot; - return jointSpaceRot; -} - -glm::vec3 MyAvatar::jointToWorldPoint(const glm::vec3& jointSpacePos, const int jointIndex) const { - glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - - if (jointIndex != -1) { - if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) { - _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot); - } else { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - } - - glm::vec3 worldOffset = jointRot * jointSpacePos; - glm::vec3 worldPos = jointPos + worldOffset; - - return worldPos; -} - -glm::vec3 MyAvatar::jointToWorldDirection(const glm::vec3& jointSpaceDir, const int jointIndex) const { - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - glm::vec3 worldDir = jointRot * jointSpaceDir; - return worldDir; -} - -glm::quat MyAvatar::jointToWorldRotation(const glm::quat& jointSpaceRot, const int jointIndex) const { - glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified - if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { - qWarning() << "Invalid joint index specified: " << jointIndex; - } - glm::quat worldRot = jointRot * jointSpaceRot; - return worldRot; -} - // virtual void MyAvatar::render(RenderArgs* renderArgs) { // don't render if we've been asked to disable local rendering @@ -3114,16 +3043,15 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { if (_skeletonModel && _skeletonModel->isLoaded()) { const Rig& rig = _skeletonModel->getRig(); - const HFMModel& hfmModel = _skeletonModel->getHFMModel(); - for (int i = 0; i < rig.getJointStateCount(); i++) { - AnimPose jointPose; - rig.getAbsoluteJointPoseInRigFrame(i, jointPose); - const HFMJointShapeInfo& shapeInfo = hfmModel.joints[i].shapeInfo; - const AnimPose pose = rigToWorldPose * jointPose; - for (size_t j = 0; j < shapeInfo.debugLines.size() / 2; j++) { - glm::vec3 pointA = pose.xformPoint(shapeInfo.debugLines[2 * j]); - glm::vec3 pointB = pose.xformPoint(shapeInfo.debugLines[2 * j + 1]); - DebugDraw::getInstance().drawRay(pointA, pointB, DEBUG_COLORS[i % NUM_DEBUG_COLORS]); + int jointCount = rig.getJointStateCount(); + if (jointCount == (int)_multiSphereShapes.size()) { + for (int i = 0; i < jointCount; i++) { + AnimPose jointPose; + rig.getAbsoluteJointPoseInRigFrame(i, jointPose); + const AnimPose pose = rigToWorldPose * jointPose; + auto &multiSphere = _multiSphereShapes[i]; + auto debugLines = multiSphere.getDebugLines(); + DebugDraw::getInstance().drawRays(debugLines, DEBUG_COLORS[i % NUM_DEBUG_COLORS], pose.trans(), pose.rot()); } } } @@ -5383,3 +5311,4 @@ void MyAvatar::releaseGrab(const QUuid& grabID) { _clientTraitsHandler->markInstancedTraitDeleted(AvatarTraits::Grab, grabID); } } + diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 91e1b67fcc..0d27988543 100755 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -37,6 +37,7 @@ class AvatarActionHold; class ModelItemID; class MyHead; +class DetailedMotionState; enum eyeContactTarget { LEFT_EYE, @@ -802,56 +803,6 @@ public: */ Q_INVOKABLE controller::Pose getRightHandTipPose() const; - // world-space to avatar-space rigconversion functions - /**jsdoc - * @function MyAvatar.worldToJointPoint - * @param {Vec3} position - * @param {number} [jointIndex=-1] - * @returns {Vec3} - */ - Q_INVOKABLE glm::vec3 worldToJointPoint(const glm::vec3& position, const int jointIndex = -1) const; - - /**jsdoc - * @function MyAvatar.worldToJointDirection - * @param {Vec3} direction - * @param {number} [jointIndex=-1] - * @returns {Vec3} - */ - Q_INVOKABLE glm::vec3 worldToJointDirection(const glm::vec3& direction, const int jointIndex = -1) const; - - /**jsdoc - * @function MyAvatar.worldToJointRotation - * @param {Quat} rotation - * @param {number} [jointIndex=-1] - * @returns {Quat} - */ - Q_INVOKABLE glm::quat worldToJointRotation(const glm::quat& rotation, const int jointIndex = -1) const; - - - /**jsdoc - * @function MyAvatar.jointToWorldPoint - * @param {vec3} position - * @param {number} [jointIndex=-1] - * @returns {Vec3} - */ - Q_INVOKABLE glm::vec3 jointToWorldPoint(const glm::vec3& position, const int jointIndex = -1) const; - - /**jsdoc - * @function MyAvatar.jointToWorldDirection - * @param {Vec3} direction - * @param {number} [jointIndex=-1] - * @returns {Vec3} - */ - Q_INVOKABLE glm::vec3 jointToWorldDirection(const glm::vec3& direction, const int jointIndex = -1) const; - - /**jsdoc - * @function MyAvatar.jointToWorldRotation - * @param {Quat} rotation - * @param {number} [jointIndex=-1] - * @returns {Quat} - */ - Q_INVOKABLE glm::quat jointToWorldRotation(const glm::quat& rotation, const int jointIndex = -1) const; - AvatarWeakPointer getLookAtTargetAvatar() const { return _lookAtTargetAvatar; } void updateLookAtTargetAvatar(); void computeMyLookAtTarget(const AvatarHash& hash); diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 821b01c2c6..b0123abe8d 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -12,8 +12,10 @@ #include "MyCharacterController.h" #include +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "MyAvatar.h" +#include "DetailedMotionState.h" // TODO: make avatars stand on steep slope // TODO: make avatars not snag on low ceilings @@ -24,7 +26,7 @@ void MyCharacterController::RayShotgunResult::reset() { walkable = true; } -MyCharacterController::MyCharacterController(MyAvatar* avatar) { +MyCharacterController::MyCharacterController(std::shared_ptr avatar) { assert(avatar); _avatar = avatar; @@ -44,7 +46,6 @@ void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; - if (_radius > 0.0f) { // create RigidBody if it doesn't exist if (!_rigidBody) { @@ -375,3 +376,121 @@ void MyCharacterController::updateMassProperties() { _rigidBody->setMassProps(mass, inertia); } + +btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int jointIndex) { + ShapeInfo shapeInfo; + _avatar->computeDetailedShapeInfo(shapeInfo, jointIndex); + if (shapeInfo.getType() != SHAPE_TYPE_NONE) { + btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + if (shape) { + shape->setMargin(0.001f); + } + return shape; + } + return nullptr; +} + +DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) { + auto shape = createDetailedCollisionShapeForJoint(jointIndex); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(_avatar, shape, jointIndex); + motionState->setMass(_avatar->computeMass()); + return motionState; + } + return nullptr; +} + +void MyCharacterController::clearDetailedMotionStates() { + _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; + // We make sure we don't add them again + _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION; +} + +void MyCharacterController::resetDetailedMotionStates() { + _detailedMotionStates.clear(); +} + +void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) { + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + _detailedMotionStates[i]->forceActive(); + } + if (_pendingFlags & PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION) { + _pendingFlags &= ~PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + transaction.objectsToRemove.push_back(_detailedMotionStates[i]); + } + _detailedMotionStates.clear(); + } + if (_pendingFlags & PENDING_FLAG_ADD_DETAILED_TO_SIMULATION) { + _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION; + for (int i = 0; i < _avatar->getJointCount(); i++) { + auto dMotionState = createDetailedMotionStateForJoint(i); + if (dMotionState) { + _detailedMotionStates.push_back(dMotionState); + transaction.objectsToAdd.push_back(dMotionState); + } + } + } +} + +void MyCharacterController::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) { + // things on objectsToRemove are ready for delete + for (auto object : transaction.objectsToRemove) { + delete object; + } + transaction.clear(); +} + + +class DetailedRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback { +public: + DetailedRayResultCallback() + : btCollisionWorld::AllHitsRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) { + // the RayResultCallback's group and mask must match MY_AVATAR + m_collisionFilterGroup = BULLET_COLLISION_GROUP_DETAILED_RAY; + m_collisionFilterMask = BULLET_COLLISION_MASK_DETAILED_RAY; + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override { + return AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +}; + +std::vector MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction, + const btScalar& length, const QVector& jointsToExclude) const { + std::vector foundAvatars; + if (_dynamicsWorld) { + btVector3 end = origin + length * direction; + DetailedRayResultCallback rayCallback = DetailedRayResultCallback(); + rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal; + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest; + _dynamicsWorld->rayTest(origin, end, rayCallback); + if (rayCallback.m_hitFractions.size() > 0) { + foundAvatars.reserve(rayCallback.m_hitFractions.size()); + for (int i = 0; i < rayCallback.m_hitFractions.size(); i++) { + auto object = rayCallback.m_collisionObjects[i]; + ObjectMotionState* motionState = static_cast(object->getUserPointer()); + if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) { + DetailedMotionState* detailedMotionState = dynamic_cast(motionState); + MyCharacterController::RayAvatarResult result; + result._intersect = true; + result._intersectWithAvatar = detailedMotionState->getAvatarID(); + result._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[i]); + result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[i]); + result._distance = length * rayCallback.m_hitFractions[i]; + result._intersectWithJoint = detailedMotionState->getJointIndex(); + result._isBound = detailedMotionState->getIsBound(result._boundJoints); + btVector3 center; + btScalar radius; + detailedMotionState->getShape()->getBoundingSphere(center, radius); + result._maxDistance = (float)radius; + foundAvatars.push_back(result); + } + } + std::sort(foundAvatars.begin(), foundAvatars.end(), [](const RayAvatarResult& resultA, const RayAvatarResult& resultB) { + return resultA._distance < resultB._distance; + }); + } + } + return foundAvatars; +} \ No newline at end of file diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h old mode 100755 new mode 100644 index 76fe588e71..f861f82422 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -15,13 +15,15 @@ #include //#include +#include class btCollisionShape; class MyAvatar; +class DetailedMotionState; class MyCharacterController : public CharacterController { public: - explicit MyCharacterController(MyAvatar* avatar); + explicit MyCharacterController(std::shared_ptr avatar); ~MyCharacterController (); void setDynamicsWorld(btDynamicsWorld* world) override; @@ -42,6 +44,31 @@ public: void setDensity(btScalar density) { _density = density; } + btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); + DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex); + std::vector& getDetailedMotionStates() { return _detailedMotionStates; } + void clearDetailedMotionStates(); + void resetDetailedMotionStates(); + + void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction); + void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction); + + + struct RayAvatarResult { + bool _intersect { false }; + bool _isBound { false }; + QUuid _intersectWithAvatar; + int _intersectWithJoint { -1 }; + float _distance { 0.0f }; + float _maxDistance { 0.0f }; + QVariantMap _extraInfo; + glm::vec3 _intersectionPoint; + glm::vec3 _intersectionNormal; + std::vector _boundJoints; + }; + std::vector rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, + const QVector& jointsToExclude) const; + int32_t computeCollisionMask() const override; void handleChangedCollisionMask() override; @@ -56,12 +83,14 @@ private: btConvexHullShape* computeShape() const; protected: - MyAvatar* _avatar { nullptr }; + std::shared_ptr _avatar { nullptr }; // shotgun scan data btAlignedObjectArray _topPoints; btAlignedObjectArray _bottomPoints; btScalar _density { 1.0f }; + + std::vector _detailedMotionStates; }; #endif // hifi_MyCharacterController_h diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index de5b7bc00c..a3950c8e96 100755 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -15,6 +15,7 @@ #include "Application.h" #include "AvatarMotionState.h" +#include "DetailedMotionState.h" const float DISPLAYNAME_FADE_TIME = 0.5f; const float DISPLAYNAME_FADE_FACTOR = pow(0.01f, 1.0f / DISPLAYNAME_FADE_TIME); @@ -110,29 +111,155 @@ void OtherAvatar::updateSpaceProxy(workload::Transaction& transaction) const { int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { int32_t bytesRead = Avatar::parseDataFromBuffer(buffer); + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + _detailedMotionStates[i]->forceActive(); + } if (_moving && _motionState) { _motionState->addDirtyFlags(Simulation::DIRTY_POSITION); } return bytesRead; } +btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector& boundJoints) { + ShapeInfo shapeInfo; + isBound = false; + QString jointName = ""; + if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) { + jointName = _multiSphereShapes[jointIndex].getJointName(); + } + switch (_bodyLOD) { + case BodyLOD::Sphere: + shapeInfo.setSphere(0.5f * getFitBounds().getDimensions().y); + boundJoints.clear(); + for (auto &spheres : _multiSphereShapes) { + if (spheres.isValid()) { + boundJoints.push_back(spheres.getJointIndex()); + } + } + isBound = true; + break; + case BodyLOD::MultiSphereLow: + if (jointName.contains("RightHand", Qt::CaseInsensitive) || jointName.contains("LeftHand", Qt::CaseInsensitive)) { + if (jointName.size() <= QString("RightHand").size()) { + AABox handBound; + for (auto &spheres : _multiSphereShapes) { + if (spheres.isValid() && spheres.getJointName().contains(jointName, Qt::CaseInsensitive)) { + boundJoints.push_back(spheres.getJointIndex()); + handBound += spheres.getBoundingBox(); + } + } + shapeInfo.setSphere(0.5f * handBound.getLargestDimension()); + glm::vec3 jointPosition; + glm::quat jointRotation; + _skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPosition); + _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRotation); + glm::vec3 positionOffset = glm::inverse(jointRotation) * (handBound.calcCenter() - jointPosition); + shapeInfo.setOffset(positionOffset); + isBound = true; + } + break; + } + case BodyLOD::MultiSphereHigh: + computeDetailedShapeInfo(shapeInfo, jointIndex); + break; + default: + break; + } + if (shapeInfo.getType() != SHAPE_TYPE_NONE) { + auto shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + if (shape) { + shape->setMargin(0.001f); + } + return shape; + } + return nullptr; +} + +DetailedMotionState* OtherAvatar::createMotionState(std::shared_ptr avatar, int jointIndex) { + bool isBound = false; + std::vector boundJoints; + btCollisionShape* shape = createCollisionShape(jointIndex, isBound, boundJoints); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex); + motionState->setMass(computeMass()); + motionState->setIsBound(isBound, boundJoints); + return motionState; + } + return nullptr; +} + +void OtherAvatar::resetDetailedMotionStates() { + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + _detailedMotionStates[i] = nullptr; + } + _detailedMotionStates.clear(); +} + void OtherAvatar::setWorkloadRegion(uint8_t region) { _workloadRegion = region; + QString printRegion = ""; + if (region == workload::Region::R1) { + printRegion = "R1"; + } else if (region == workload::Region::R2) { + printRegion = "R2"; + } else if (region == workload::Region::R3) { + printRegion = "R3"; + } else { + printRegion = "invalid"; + } + qCDebug(avatars) << "Setting workload region to " << printRegion; + computeShapeLOD(); +} + +void OtherAvatar::computeShapeLOD() { + // auto newBodyLOD = _workloadRegion < workload::Region::R3 ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape; + // auto newBodyLOD = BodyLOD::CapsuleShape; + BodyLOD newLOD; + switch (_workloadRegion) { + case workload::Region::R1: + newLOD = BodyLOD::MultiSphereHigh; + break; + case workload::Region::R2: + newLOD = BodyLOD::MultiSphereLow; + break; + case workload::Region::UNKNOWN: + case workload::Region::INVALID: + case workload::Region::R3: + default: + newLOD = BodyLOD::Sphere; + break; + } + if (newLOD != _bodyLOD) { + _bodyLOD = newLOD; + if (isInPhysicsSimulation()) { + qCDebug(avatars) << "Changing to body LOD " << newLOD; + _needsReinsertion = true; + } + } +} + +bool OtherAvatar::isInPhysicsSimulation() const { + return _motionState != nullptr && _detailedMotionStates.size() > 0; } bool OtherAvatar::shouldBeInPhysicsSimulation() const { - return (_workloadRegion < workload::Region::R3 && !isDead()); + return !isDead() && !(isInPhysicsSimulation() && _needsReinsertion); } bool OtherAvatar::needsPhysicsUpdate() const { constexpr uint32_t FLAGS_OF_INTEREST = Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS | Simulation::DIRTY_POSITION | Simulation::DIRTY_COLLISION_GROUP; - return (_motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST)); + return (_needsReinsertion || (_motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST))); } void OtherAvatar::rebuildCollisionShape() { if (_motionState) { _motionState->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS); } + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + if (_detailedMotionStates[i]) { + _detailedMotionStates[i]->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS); + } + } } void OtherAvatar::setCollisionWithOtherAvatarsFlags() { @@ -141,6 +268,25 @@ void OtherAvatar::setCollisionWithOtherAvatarsFlags() { } } +void OtherAvatar::createDetailedMotionStates(const std::shared_ptr& avatar) { + auto& detailedMotionStates = getDetailedMotionStates(); + assert(detailedMotionStates.empty()); + if (_bodyLOD == BodyLOD::Sphere) { + auto dMotionState = createMotionState(avatar, -1); + if (dMotionState) { + detailedMotionStates.push_back(dMotionState); + } + } else { + for (int i = 0; i < getJointCount(); i++) { + auto dMotionState = createMotionState(avatar, i); + if (dMotionState) { + detailedMotionStates.push_back(dMotionState); + } + } + } + _needsReinsertion = false; +} + void OtherAvatar::simulate(float deltaTime, bool inView) { PROFILE_RANGE(simulation, "simulate"); diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h old mode 100755 new mode 100644 index 969f551783..3ecd35413f --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -21,12 +21,19 @@ class AvatarManager; class AvatarMotionState; +class DetailedMotionState; class OtherAvatar : public Avatar { public: explicit OtherAvatar(QThread* thread); virtual ~OtherAvatar(); + enum BodyLOD { + Sphere = 0, + MultiSphereLow, // No finger joints + MultiSphereHigh // All joints + }; + virtual void instantiableAvatar() override { }; virtual void createOrb() override; virtual void indicateLoadingStatus(LoadingStatus loadingStatus) override; @@ -39,13 +46,22 @@ public: int parseDataFromBuffer(const QByteArray& buffer) override; - bool isInPhysicsSimulation() const { return _motionState != nullptr; } + bool isInPhysicsSimulation() const; void rebuildCollisionShape() override; void setWorkloadRegion(uint8_t region); bool shouldBeInPhysicsSimulation() const; bool needsPhysicsUpdate() const; + btCollisionShape* createCollisionShape(int jointIndex, bool& isBound, std::vector& boundJoints); + DetailedMotionState* createMotionState(std::shared_ptr avatar, int jointIndex); + void createDetailedMotionStates(const std::shared_ptr& avatar); + std::vector& getDetailedMotionStates() { return _detailedMotionStates; } + void resetDetailedMotionStates(); + BodyLOD getBodyLOD() { return _bodyLOD; } + void computeShapeLOD(); + + void updateCollisionGroup(bool myAvatarCollide); bool getCollideWithOtherAvatars() const { return _collideWithOtherAvatars; } void setCollisionWithOtherAvatarsFlags() override; @@ -64,10 +80,14 @@ protected: std::shared_ptr _otherAvatarOrbMeshPlaceholder { nullptr }; OverlayID _otherAvatarOrbMeshPlaceholderID { UNKNOWN_OVERLAY_ID }; AvatarMotionState* _motionState { nullptr }; + std::vector _detailedMotionStates; int32_t _spaceIndex { -1 }; uint8_t _workloadRegion { workload::Region::INVALID }; + BodyLOD _bodyLOD { BodyLOD::Sphere }; + bool _needsReinsertion { false }; }; using OtherAvatarPointer = std::shared_ptr; +using AvatarPointer = std::shared_ptr; #endif // hifi_OtherAvatar_h diff --git a/interface/src/raypick/RayPick.cpp b/interface/src/raypick/RayPick.cpp index 507e45b470..24ba4435e2 100644 --- a/interface/src/raypick/RayPick.cpp +++ b/interface/src/raypick/RayPick.cpp @@ -56,7 +56,7 @@ PickResultPointer RayPick::getOverlayIntersection(const PickRay& pick) { } PickResultPointer RayPick::getAvatarIntersection(const PickRay& pick) { - RayToAvatarIntersectionResult avatarRes = DependencyManager::get()->findRayIntersectionVector(pick, getIncludeItemsAs(), getIgnoreItemsAs()); + RayToAvatarIntersectionResult avatarRes = DependencyManager::get()->findRayIntersectionVector(pick, getIncludeItemsAs(), getIgnoreItemsAs(), true); if (avatarRes.intersects) { return std::make_shared(IntersectionType::AVATAR, avatarRes.avatarID, avatarRes.distance, avatarRes.intersection, pick, avatarRes.surfaceNormal, avatarRes.extraInfo); } else { diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 87d189696a..07c1ca9a32 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -298,7 +298,9 @@ void Avatar::setTargetScale(float targetScale) { _scaleChanged = usecTimestampNow(); _avatarScaleChanged = _scaleChanged; _isAnimatingScale = true; - + for (auto& sphere : _multiSphereShapes) { + sphere.setScale(_targetScale); + } emit targetScaleChanged(targetScale); } } @@ -722,6 +724,7 @@ void Avatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { } fixupModelsInScene(scene); + updateFitBoundingBox(); } void Avatar::render(RenderArgs* renderArgs) { @@ -1288,6 +1291,79 @@ glm::vec3 Avatar::getAbsoluteJointScaleInObjectFrame(int index) const { } } + +glm::vec3 Avatar::worldToJointPoint(const glm::vec3& position, const int jointIndex) const { + glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + if (jointIndex != -1) { + if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) { + _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot); + } else { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + } + glm::vec3 modelOffset = position - jointPos; + glm::vec3 jointSpacePosition = glm::inverse(jointRot) * modelOffset; + + return jointSpacePosition; +} + +glm::vec3 Avatar::worldToJointDirection(const glm::vec3& worldDir, const int jointIndex) const { + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + + glm::vec3 jointSpaceDir = glm::inverse(jointRot) * worldDir; + return jointSpaceDir; +} + +glm::quat Avatar::worldToJointRotation(const glm::quat& worldRot, const int jointIndex) const { + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + glm::quat jointSpaceRot = glm::inverse(jointRot) * worldRot; + return jointSpaceRot; +} + +glm::vec3 Avatar::jointToWorldPoint(const glm::vec3& jointSpacePos, const int jointIndex) const { + glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + + if (jointIndex != -1) { + if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) { + _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot); + } else { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + } + + glm::vec3 worldOffset = jointRot * jointSpacePos; + glm::vec3 worldPos = jointPos + worldOffset; + + return worldPos; +} + +glm::vec3 Avatar::jointToWorldDirection(const glm::vec3& jointSpaceDir, const int jointIndex) const { + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + glm::vec3 worldDir = jointRot * jointSpaceDir; + return worldDir; +} + +glm::quat Avatar::jointToWorldRotation(const glm::quat& jointSpaceRot, const int jointIndex) const { + glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified + if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) { + qWarning() << "Invalid joint index specified: " << jointIndex; + } + glm::quat worldRot = jointRot * jointSpaceRot; + return worldRot; +} + + void Avatar::invalidateJointIndicesCache() const { QWriteLocker writeLock(&_modelJointIndicesCacheLock); _modelJointsCached = false; @@ -1425,6 +1501,7 @@ void Avatar::setModelURLFinished(bool success) { // rig is ready void Avatar::rigReady() { buildUnscaledEyeHeightCache(); + computeMultiSphereShapes(); } // rig has been reset. @@ -1432,6 +1509,48 @@ void Avatar::rigReset() { clearUnscaledEyeHeightCache(); } +void Avatar::computeMultiSphereShapes() { + const Rig& rig = getSkeletonModel()->getRig(); + glm::vec3 scale = extractScale(rig.getGeometryOffsetPose()); + const HFMModel& geometry = getSkeletonModel()->getHFMModel(); + int jointCount = rig.getJointStateCount(); + _multiSphereShapes.clear(); + _multiSphereShapes.reserve(jointCount); + for (int i = 0; i < jointCount; i++) { + const HFMJointShapeInfo& shapeInfo = geometry.joints[i].shapeInfo; + std::vector btPoints; + int lineCount = (int)shapeInfo.debugLines.size(); + btPoints.reserve(lineCount); + for (int j = 0; j < lineCount; j++) { + const glm::vec3 &point = shapeInfo.debugLines[j]; + auto rigPoint = scale * point; + btVector3 btPoint = glmToBullet(rigPoint); + btPoints.push_back(btPoint); + } + auto jointName = rig.nameOfJoint(i).toUpper(); + MultiSphereShape multiSphereShape; + if (multiSphereShape.computeMultiSphereShape(i, jointName, btPoints)) { + multiSphereShape.calculateDebugLines(); + multiSphereShape.setScale(_targetScale); + } + _multiSphereShapes.push_back(multiSphereShape); + } +} + +void Avatar::updateFitBoundingBox() { + _fitBoundingBox = AABox(); + if (getJointCount() == (int)_multiSphereShapes.size()) { + for (int i = 0; i < getJointCount(); i++) { + auto &shape = _multiSphereShapes[i]; + glm::vec3 jointPosition; + glm::quat jointRotation; + _skeletonModel->getJointPositionInWorldFrame(i, jointPosition); + _skeletonModel->getJointRotationInWorldFrame(i, jointRotation); + _fitBoundingBox += shape.updateBoundingBox(jointPosition, jointRotation); + } + } +} + // create new model, can return an instance of a SoftAttachmentModel rather then Model static std::shared_ptr allocateAttachmentModel(bool isSoft, const Rig& rigOverride, bool isCauterized) { if (isSoft) { @@ -1611,6 +1730,21 @@ void Avatar::computeShapeInfo(ShapeInfo& shapeInfo) { shapeInfo.setOffset(offset); } +void Avatar::computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex) { + if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) { + auto& data = _multiSphereShapes[jointIndex].getSpheresData(); + std::vector positions; + std::vector radiuses; + positions.reserve(data.size()); + radiuses.reserve(data.size()); + for (auto& sphere : data) { + positions.push_back(sphere._position); + radiuses.push_back(sphere._radius); + } + shapeInfo.setMultiSphere(positions, radiuses); + } +} + void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) { ShapeInfo shapeInfo; computeShapeInfo(shapeInfo); diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h index 1c3768e937..b43fe012b7 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h @@ -34,6 +34,7 @@ #include "Rig.h" #include "MetaModelPayload.h" +#include "MultiSphereShape.h" namespace render { template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar); @@ -227,12 +228,63 @@ public: */ Q_INVOKABLE virtual glm::vec3 getAbsoluteDefaultJointTranslationInObjectFrame(int index) const; + virtual glm::vec3 getAbsoluteJointScaleInObjectFrame(int index) const override; virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; virtual bool setAbsoluteJointRotationInObjectFrame(int index, const glm::quat& rotation) override { return false; } virtual bool setAbsoluteJointTranslationInObjectFrame(int index, const glm::vec3& translation) override { return false; } + // world-space to avatar-space rigconversion functions + /**jsdoc + * @function MyAvatar.worldToJointPoint + * @param {Vec3} position + * @param {number} [jointIndex=-1] + * @returns {Vec3} + */ + Q_INVOKABLE glm::vec3 worldToJointPoint(const glm::vec3& position, const int jointIndex = -1) const; + + /**jsdoc + * @function MyAvatar.worldToJointDirection + * @param {Vec3} direction + * @param {number} [jointIndex=-1] + * @returns {Vec3} + */ + Q_INVOKABLE glm::vec3 worldToJointDirection(const glm::vec3& direction, const int jointIndex = -1) const; + + /**jsdoc + * @function MyAvatar.worldToJointRotation + * @param {Quat} rotation + * @param {number} [jointIndex=-1] + * @returns {Quat} + */ + Q_INVOKABLE glm::quat worldToJointRotation(const glm::quat& rotation, const int jointIndex = -1) const; + + + /**jsdoc + * @function MyAvatar.jointToWorldPoint + * @param {vec3} position + * @param {number} [jointIndex=-1] + * @returns {Vec3} + */ + Q_INVOKABLE glm::vec3 jointToWorldPoint(const glm::vec3& position, const int jointIndex = -1) const; + + /**jsdoc + * @function MyAvatar.jointToWorldDirection + * @param {Vec3} direction + * @param {number} [jointIndex=-1] + * @returns {Vec3} + */ + Q_INVOKABLE glm::vec3 jointToWorldDirection(const glm::vec3& direction, const int jointIndex = -1) const; + + /**jsdoc + * @function MyAvatar.jointToWorldRotation + * @param {Quat} rotation + * @param {number} [jointIndex=-1] + * @returns {Quat} + */ + Q_INVOKABLE glm::quat jointToWorldRotation(const glm::quat& rotation, const int jointIndex = -1) const; + virtual void setSkeletonModelURL(const QUrl& skeletonModelURL) override; virtual void setAttachmentData(const QVector& attachmentData) override; @@ -320,6 +372,8 @@ public: virtual void rebuildCollisionShape() = 0; virtual void computeShapeInfo(ShapeInfo& shapeInfo); + virtual void computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex); + void getCapsule(glm::vec3& start, glm::vec3& end, float& radius); float computeMass(); /**jsdoc @@ -398,6 +452,7 @@ public: float getBoundingRadius() const; AABox getRenderBounds() const; // THis call is accessible from rendering thread only to report the bounding box of the avatar during the frame. + AABox getFitBounds() const { return _fitBoundingBox; } void addToScene(AvatarSharedPointer self, const render::ScenePointer& scene); void ensureInScene(AvatarSharedPointer self, const render::ScenePointer& scene); @@ -441,6 +496,7 @@ public: void accumulateGrabPositions(std::map& grabAccumulators); + const std::vector& getMultiSphereShapes() const { return _multiSphereShapes; } void tearDownGrabs(); signals: @@ -510,6 +566,8 @@ protected: virtual const QString& getSessionDisplayNameForTransport() const override { return _empty; } // Save a tiny bit of bandwidth. Mixer won't look at what we send. QString _empty{}; virtual void maybeUpdateSessionDisplayNameFromTransport(const QString& sessionDisplayName) override { _sessionDisplayName = sessionDisplayName; } // don't use no-op setter! + void computeMultiSphereShapes(); + void updateFitBoundingBox(); SkeletonModelPointer _skeletonModel; @@ -632,6 +690,9 @@ protected: static void metaBlendshapeOperator(render::ItemID renderItemID, int blendshapeNumber, const QVector& blendshapeOffsets, const QVector& blendedMeshSizes, const render::ItemIDs& subItemIDs); + + std::vector _multiSphereShapes; + AABox _fitBoundingBox; void clearAvatarGrabData(const QUuid& grabID) override; using SetOfIDs = std::set; diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 29f0d2b33d..4e95774efb 100755 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -2900,6 +2900,7 @@ QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, c obj.setProperty("intersection", intersection); QScriptValue surfaceNormal = vec3ToScriptValue(engine, value.surfaceNormal); obj.setProperty("surfaceNormal", surfaceNormal); + obj.setProperty("jointIndex", value.jointIndex); obj.setProperty("extraInfo", engine->toScriptValue(value.extraInfo)); return obj; } @@ -2919,6 +2920,7 @@ void RayToAvatarIntersectionResultFromScriptValue(const QScriptValue& object, Ra if (surfaceNormal.isValid()) { vec3FromScriptValue(surfaceNormal, value.surfaceNormal); } + value.jointIndex = object.property("jointIndex").toInt32(); value.extraInfo = object.property("extraInfo").toVariant().toMap(); } diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index d87f00e1ea..d071b74d6e 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -1615,6 +1615,7 @@ public: BoxFace face; glm::vec3 intersection; glm::vec3 surfaceNormal; + int jointIndex { -1 }; QVariantMap extraInfo; }; Q_DECLARE_METATYPE(RayToAvatarIntersectionResult) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index d5ded6f909..13a1328065 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -138,7 +138,8 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION; + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION | + PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; } else { _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION; } @@ -446,10 +447,10 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const if (_dynamicsWorld) { // must REMOVE from world prior to shape update - _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; + _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; } _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; - _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION; + _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_ADD_DETAILED_TO_SIMULATION; } // it's ok to change offset immediately -- there are no thread safety issues here diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index cac37da0b9..d59374a94a 100755 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -32,6 +32,9 @@ const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2; const uint32_t PENDING_FLAG_JUMP = 1U << 3; const uint32_t PENDING_FLAG_UPDATE_COLLISION_MASK = 1U << 4; const uint32_t PENDING_FLAG_RECOMPUTE_FLYING = 1U << 5; +const uint32_t PENDING_FLAG_ADD_DETAILED_TO_SIMULATION = 1U << 6; +const uint32_t PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION = 1U << 7; + const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f); class btRigidBody; diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp new file mode 100644 index 0000000000..549c8042d5 --- /dev/null +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -0,0 +1,538 @@ +// +// MultiSphereShape.cpp +// libraries/physics/src +// +// Created by Luis Cuenca 5/11/2018 +// Copyright 2018 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 "MultiSphereShape.h" + +void SphereRegion::translate(const glm::vec3& translation) { + for (auto &line : _lines) { + line.first += translation; + line.second += translation; + } +} +void SphereRegion::dump(std::vector>& outLines) { + for (auto &line : _lines) { + outLines.push_back(line); + } +} + +void SphereRegion::insertUnique(const glm::vec3& point, std::vector& pointSet) { + auto hit = std::find_if(pointSet.begin(), pointSet.end(), [point](const glm::vec3& pointFromSet) -> bool { + return (glm::length(pointFromSet-point) < FLT_EPSILON); + }); + if (hit == pointSet.end()) { + pointSet.push_back(point); + } +} + +void SphereRegion::extractEdges(bool reverseY) { + if (_lines.size() == 0) { + return; + } + float yVal = _lines[0].first.y; + for (size_t i = 0; i < _lines.size(); i++) { + yVal = reverseY ? glm::max(yVal, _lines[i].first.y) : glm::min(yVal, _lines[i].first.y); + } + for (size_t i = 0; i < _lines.size(); i++) { + auto line = _lines[i]; + auto p1 = line.first; + auto p2 = line.second; + auto vec = p1 - p2; + if (vec.z == 0.0f) { + insertUnique(p1, _edgesX); + insertUnique(p2, _edgesX); + } else if (vec.y == 0.0f && p1.y == yVal && p2.y == yVal) { + insertUnique(p1, _edgesY); + insertUnique(p2, _edgesY); + } else if (vec.x == 0.0f) { + insertUnique(p1, _edgesZ); + insertUnique(p2, _edgesZ); + } + } +} + +void SphereRegion::extractSphereRegion(std::vector>& outLines) { + for (size_t i = 0; i < outLines.size(); i++) { + auto &line = outLines[i]; + auto &p1 = line.first; + auto &p2 = line.second; + p1.x = glm::abs(p1.x) < 0.001f ? 0.0f : p1.x; + p1.y = glm::abs(p1.y) < 0.001f ? 0.0f : p1.y; + p1.z = glm::abs(p1.z) < 0.001f ? 0.0f : p1.z; + p2.x = glm::abs(p2.x) < 0.001f ? 0.0f : p2.x; + p2.y = glm::abs(p2.y) < 0.001f ? 0.0f : p2.y; + p2.z = glm::abs(p2.z) < 0.001f ? 0.0f : p2.z; + + glm::vec3 point1 = { p1.x != 0.0f ? glm::abs(p1.x) / p1.x : _direction.x, + p1.y != 0.0f ? glm::abs(p1.y) / p1.y : _direction.y, + p1.z != 0.0f ? glm::abs(p1.z) / p1.z : _direction.z }; + glm::vec3 point2 = { p2.x != 0.0f ? glm::abs(p2.x) / p2.x : _direction.x, + p2.y != 0.0f ? glm::abs(p2.y) / p2.y : _direction.y, + p2.z != 0.0f ? glm::abs(p2.z) / p2.z : _direction.z }; + if (point1 == _direction && point2 == _direction) { + _lines.push_back(line); + } + } +} + +CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QString& name) { + CollisionShapeExtractionMode mode = CollisionShapeExtractionMode::Automatic; + bool isSim = name.indexOf("SIM") == 0; + bool isFlow = name.indexOf("FLOW") == 0; + bool isEye = name.indexOf("EYE") > -1; + bool isToe = name.indexOf("TOE") > -1; + bool isShoulder = name.indexOf("SHOULDER") > -1; + bool isNeck = name.indexOf("NECK") > -1; + bool isRightHand = name == "RIGHTHAND"; + bool isLeftHand = name == "LEFTHAND"; + bool isRightFinger = name.indexOf("RIGHTHAND") == 0 && !isRightHand; + bool isLeftFinger = name.indexOf("LEFTHAND") == 0 && !isLeftHand; + + //bool isFinger = + if (isNeck || isLeftFinger || isRightFinger) { + mode = CollisionShapeExtractionMode::SpheresY; + } else if (isShoulder) { + mode = CollisionShapeExtractionMode::SphereCollapse; + } else if (isRightHand || isLeftHand) { + mode = CollisionShapeExtractionMode::SpheresXY; + } else if (isSim || isFlow || isEye || isToe) { + mode = CollisionShapeExtractionMode::None; + } + return mode; +} + +void MultiSphereShape::filterUniquePoints(const std::vector& kdop, std::vector& uniquePoints) { + for (size_t j = 0; j < kdop.size(); j++) { + btVector3 btPoint = kdop[j]; + auto hit = std::find_if(uniquePoints.begin(), uniquePoints.end(), [btPoint](const glm::vec3& point) -> bool { + return (glm::length(btPoint.getX() - point.x) < FLT_EPSILON + && glm::length(btPoint.getY() - point.y) < FLT_EPSILON + && glm::length(btPoint.getZ() - point.z) < FLT_EPSILON); + }); + if (hit == uniquePoints.end()) { + uniquePoints.push_back(bulletToGLM(btPoint)); + } + } +} + +bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& name, const std::vector& kdop, float scale) { + _scale = scale; + _jointIndex = jointIndex; + _name = name; + _mode = getExtractionModeByName(_name); + if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) { + return false; + } + std::vector points; + filterUniquePoints(kdop, points); + glm::vec3 min = glm::vec3(100.0f, 100.0f, 100.0f); + glm::vec3 max = glm::vec3(-100.0f, -100.0f, -100.0f); + _midPoint = glm::vec3(0.0f, 0.0f, 0.0f); + std::vector relPoints; + for (size_t i = 0; i < points.size(); i++) { + + min.x = points[i].x < min.x ? points[i].x : min.x; + min.y = points[i].y < min.y ? points[i].y : min.y; + min.z = points[i].z < min.z ? points[i].z : min.z; + + max.x = points[i].x > max.x ? points[i].x : max.x; + max.y = points[i].y > max.y ? points[i].y : max.y; + max.z = points[i].z > max.z ? points[i].z : max.z; + + _midPoint += points[i]; + } + + _midPoint /= (int)points.size(); + glm::vec3 dimensions = max - min; + + for (size_t i = 0; i < points.size(); i++) { + glm::vec3 relPoint = points[i] - _midPoint; + relPoints.push_back(relPoint); + } + CollisionShapeExtractionMode applyMode = _mode; + float xCorrector = dimensions.x > dimensions.y && dimensions.x > dimensions.z ? -1.0f + (dimensions.x / (0.5f * (dimensions.y + dimensions.z))) : 0.0f; + float yCorrector = dimensions.y > dimensions.x && dimensions.y > dimensions.z ? -1.0f + (dimensions.y / (0.5f * (dimensions.x + dimensions.z))) : 0.0f; + float zCorrector = dimensions.z > dimensions.x && dimensions.z > dimensions.y ? -1.0f + (dimensions.z / (0.5f * (dimensions.x + dimensions.y))) : 0.0f; + + float xyDif = glm::abs(dimensions.x - dimensions.y); + float xzDif = glm::abs(dimensions.x - dimensions.z); + float yzDif = glm::abs(dimensions.y - dimensions.z); + + float xyEpsilon = (0.05f + zCorrector) * glm::max(dimensions.x, dimensions.y); + float xzEpsilon = (0.05f + yCorrector) * glm::max(dimensions.x, dimensions.z); + float yzEpsilon = (0.05f + xCorrector) * glm::max(dimensions.y, dimensions.z); + + if (xyDif < 0.5f * xyEpsilon && xzDif < 0.5f * xzEpsilon && yzDif < 0.5f * yzEpsilon) { + applyMode = CollisionShapeExtractionMode::Sphere; + } else if (xzDif < xzEpsilon) { + applyMode = dimensions.y > dimensions.z ? CollisionShapeExtractionMode::SpheresY : CollisionShapeExtractionMode::SpheresXZ; + } else if (xyDif < xyEpsilon) { + applyMode = dimensions.z > dimensions.y ? CollisionShapeExtractionMode::SpheresZ : CollisionShapeExtractionMode::SpheresXY; + } else if (yzDif < yzEpsilon) { + applyMode = dimensions.x > dimensions.y ? CollisionShapeExtractionMode::SpheresX : CollisionShapeExtractionMode::SpheresYZ; + } else { + applyMode = CollisionShapeExtractionMode::SpheresXYZ; + } + + if (_mode != CollisionShapeExtractionMode::Automatic && applyMode != _mode) { + bool isModeSphereAxis = (_mode >= CollisionShapeExtractionMode::SpheresX && _mode <= CollisionShapeExtractionMode::SpheresZ); + bool isApplyModeComplex = (applyMode >= CollisionShapeExtractionMode::SpheresXY && applyMode <= CollisionShapeExtractionMode::SpheresXYZ); + applyMode = (isModeSphereAxis && isApplyModeComplex) ? CollisionShapeExtractionMode::Sphere : _mode; + } + + std::vector axes; + glm::vec3 axis, axis1, axis2; + SphereShapeData sphere; + switch (applyMode) { + case CollisionShapeExtractionMode::None: + break; + case CollisionShapeExtractionMode::Automatic: + break; + case CollisionShapeExtractionMode::Box: + break; + case CollisionShapeExtractionMode::Sphere: + sphere._radius = 0.5f * (dimensions.x + dimensions.y + dimensions.z) / 3.0f; + sphere._position = glm::vec3(0.0f); + _spheres.push_back(sphere); + break; + case CollisionShapeExtractionMode::SphereCollapse: + sphere._radius = 0.5f * glm::min(glm::min(dimensions.x, dimensions.y), dimensions.z); + sphere._position = glm::vec3(0.0f); + _spheres.push_back(sphere); + break; + case CollisionShapeExtractionMode::SpheresX: + axis = 0.5f* dimensions.x * Vectors::UNIT_NEG_X; + axes = { axis, -axis }; + break; + case CollisionShapeExtractionMode::SpheresY: + axis = 0.5f* dimensions.y * Vectors::UNIT_NEG_Y; + axes = { axis, -axis }; + break; + case CollisionShapeExtractionMode::SpheresZ: + axis = 0.5f* dimensions.z * Vectors::UNIT_NEG_Z; + axes = { axis, -axis }; + break; + case CollisionShapeExtractionMode::SpheresXY: + axis1 = glm::vec3(0.5f * dimensions.x, 0.5f * dimensions.y, 0.0f); + axis2 = glm::vec3(0.5f * dimensions.x, -0.5f * dimensions.y, 0.0f); + axes = { axis1, axis2, -axis1, -axis2 }; + break; + case CollisionShapeExtractionMode::SpheresYZ: + axis1 = glm::vec3(0.0f, 0.5f * dimensions.y, 0.5f * dimensions.z); + axis2 = glm::vec3(0.0f, 0.5f * dimensions.y, -0.5f * dimensions.z); + axes = { axis1, axis2, -axis1, -axis2 }; + break; + case CollisionShapeExtractionMode::SpheresXZ: + axis1 = glm::vec3(0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z); + axis2 = glm::vec3(-0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z); + axes = { axis1, axis2, -axis1, -axis2 }; + break; + case CollisionShapeExtractionMode::SpheresXYZ: + for (size_t i = 0; i < CORNER_SIGNS.size(); i++) { + axes.push_back(0.5f * (dimensions * CORNER_SIGNS[i])); + } + break; + default: + break; + } + if (axes.size() > 0) { + spheresFromAxes(relPoints, axes, _spheres); + } + for (size_t i = 0; i < _spheres.size(); i++) { + _spheres[i]._position += _midPoint; + } + + return _mode != CollisionShapeExtractionMode::None; +} + +void MultiSphereShape::spheresFromAxes(const std::vector& points, const std::vector& axes, std::vector& spheres) { + float maxRadius = 0.0f; + float maxAverageRadius = 0.0f; + float minAverageRadius = glm::length(points[0]); + size_t sphereCount = axes.size(); + spheres.clear(); + for (size_t j = 0; j < sphereCount; j++) { + SphereShapeData sphere = SphereShapeData(); + sphere._axis = axes[j]; + spheres.push_back(sphere); + } + for (size_t j = 0; j < sphereCount; j++) { + auto axis = _spheres[j]._axis; + float averageRadius = 0.0f; + float maxDistance = glm::length(axis); + glm::vec3 axisDir = glm::normalize(axis); + for (size_t i = 0; i < points.size(); i++) { + float dot = glm::dot(points[i], axisDir); + if (dot > 0.0f) { + float distancePow = glm::distance2(Vectors::ZERO, points[i]); + float dotPow = glm::pow(dot, 2); + float radius = (dot / maxDistance) * glm::sqrt(distancePow - dotPow); + averageRadius += radius; + maxRadius = radius > maxRadius ? radius : maxRadius; + } + } + if (points.size() > 0) { + averageRadius /= (int)points.size(); + } + maxAverageRadius = glm::max(averageRadius, maxAverageRadius); + minAverageRadius = glm::min(averageRadius, minAverageRadius); + spheres[j]._radius = averageRadius; + } + if (maxAverageRadius == 0.0f) { + maxAverageRadius = 1.0f; + } + float radiusRatio = maxRadius / maxAverageRadius; + // Push the sphere into the bounding box + float contractionRatio = 0.8f; + for (size_t j = 0; j < sphereCount; j++) { + auto axis = _spheres[j]._axis; + float distance = glm::length(axis); + float correntionRatio = radiusRatio * (spheres[j]._radius / maxAverageRadius); + float radius = (correntionRatio < 0.8f * radiusRatio ? 0.8f * radiusRatio : correntionRatio) * spheres[j]._radius; + if (sphereCount > 3) { + distance = contractionRatio * distance; + } + spheres[j]._radius = radius; + if (distance - radius > 0.0f) { + spheres[j]._position = ((distance - radius) / distance) * axis; + } else { + spheres[j]._position = glm::vec3(0.0f); + } + } + // Collapse spheres if too close + if (sphereCount == 2) { + int maxRadiusIndex = spheres[0]._radius > spheres[1]._radius ? 0 : 1; + if (glm::length(spheres[0]._position - spheres[1]._position) < 0.2f * spheres[maxRadiusIndex]._radius) { + SphereShapeData newSphere; + newSphere._position = 0.5f * (spheres[0]._position + spheres[1]._position); + newSphere._radius = spheres[maxRadiusIndex]._radius; + spheres.clear(); + spheres.push_back(newSphere); + } + } +} + +void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) { + auto sphere1 = _spheres[index1]._radius > _spheres[index2]._radius ? _spheres[index1] : _spheres[index2]; + auto sphere2 = _spheres[index1]._radius <= _spheres[index2]._radius ? _spheres[index1] : _spheres[index2]; + float distance = glm::length(sphere1._position - sphere2._position); + + auto axis = sphere1._position - sphere2._position; + + float angleOffset = glm::asin((sphere1._radius - sphere2._radius) / distance); + float ratio1 = ((0.5f * PI) + angleOffset) / PI; + float ratio2 = ((0.5f * PI) - angleOffset) / PI; + + std::vector edge1, edge2; + if (onlyEdges) { + std::vector> debugLines; + calculateSphereLines(debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1); + calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2); + } else { + calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1); + calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2); + } + connectEdges(_debugLines, edge1, edge2); +} + +void MultiSphereShape::calculateDebugLines() { + if (_spheres.size() == 1) { + auto sphere = _spheres[0]; + calculateSphereLines(_debugLines, sphere._position, sphere._radius); + } else if (_spheres.size() == 2) { + connectSpheres(0, 1); + } else if (_spheres.size() == 4) { + std::vector axes; + axes.resize(8); + for (size_t i = 0; i < CORNER_SIGNS.size(); i++) { + for (size_t j = 0; j < 4; j++) { + auto axis = _spheres[j]._position - _midPoint; + glm::vec3 sign = { axis.x != 0.0f ? glm::abs(axis.x) / axis.x : 0.0f, + axis.x != 0.0f ? glm::abs(axis.y) / axis.y : 0.0f , + axis.z != 0.0f ? glm::abs(axis.z) / axis.z : 0.0f }; + bool add = false; + if (sign.x == 0.0f) { + if (sign.y == CORNER_SIGNS[i].y && sign.z == CORNER_SIGNS[i].z) { + add = true; + } + } else if (sign.y == 0.0f) { + if (sign.x == CORNER_SIGNS[i].x && sign.z == CORNER_SIGNS[i].z) { + add = true; + } + } else if (sign.z == 0.0f) { + if (sign.x == CORNER_SIGNS[i].x && sign.y == CORNER_SIGNS[i].y) { + add = true; + } + } else if (sign == CORNER_SIGNS[i]) { + add = true; + } + if (add) { + axes[i] = axis; + break; + } + } + } + calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint); + } else if (_spheres.size() == 8) { + std::vector axes; + for (size_t i = 0; i < _spheres.size(); i++) { + axes.push_back(_spheres[i]._position - _midPoint); + } + calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint); + } +} + +void MultiSphereShape::connectEdges(std::vector>& outLines, const std::vector& edge1, const std::vector& edge2, bool reverse) { + if (edge1.size() == edge2.size()) { + for (size_t i = 0; i < edge1.size(); i++) { + size_t j = reverse ? edge1.size() - i - 1 : i; + outLines.push_back({ edge1[i], edge2[j] }); + } + } +} + +void MultiSphereShape::calculateChamferBox(std::vector>& outLines, const float& radius, const std::vector& axes, const glm::vec3& translation) { + std::vector> sphereLines; + calculateSphereLines(sphereLines, glm::vec3(0.0f), radius); + + std::vector regions = { + SphereRegion({ 1.0f, 1.0f, 1.0f }), + SphereRegion({ -1.0f, 1.0f, 1.0f }), + SphereRegion({ -1.0f, 1.0f, -1.0f }), + SphereRegion({ 1.0f, 1.0f, -1.0f }), + SphereRegion({ 1.0f, -1.0f, 1.0f }), + SphereRegion({ -1.0f, -1.0f, 1.0f }), + SphereRegion({ -1.0f, -1.0f, -1.0f }), + SphereRegion({ 1.0f, -1.0f, -1.0f }) + }; + + assert(axes.size() == regions.size()); + + for (size_t i = 0; i < regions.size(); i++) { + regions[i].extractSphereRegion(sphereLines); + regions[i].translate(translation + axes[i]); + regions[i].extractEdges(axes[i].y < 0); + regions[i].dump(outLines); + } + + connectEdges(outLines, regions[0].getEdgesZ(), regions[1].getEdgesZ()); + connectEdges(outLines, regions[1].getEdgesX(), regions[2].getEdgesX()); + connectEdges(outLines, regions[2].getEdgesZ(), regions[3].getEdgesZ()); + connectEdges(outLines, regions[3].getEdgesX(), regions[0].getEdgesX()); + + connectEdges(outLines, regions[4].getEdgesZ(), regions[5].getEdgesZ()); + connectEdges(outLines, regions[5].getEdgesX(), regions[6].getEdgesX()); + connectEdges(outLines, regions[6].getEdgesZ(), regions[7].getEdgesZ()); + connectEdges(outLines, regions[7].getEdgesX(), regions[4].getEdgesX()); + + connectEdges(outLines, regions[0].getEdgesY(), regions[4].getEdgesY()); + connectEdges(outLines, regions[1].getEdgesY(), regions[5].getEdgesY()); + connectEdges(outLines, regions[2].getEdgesY(), regions[6].getEdgesY()); + connectEdges(outLines, regions[3].getEdgesY(), regions[7].getEdgesY()); + +} + +void MultiSphereShape::calculateSphereLines(std::vector>& outLines, const glm::vec3& center, const float& radius, + const int& subdivisions, const glm::vec3& direction, const float& percentage, std::vector* edge) { + + float uTotalAngle = percentage * PI; + float vTotalAngle = 2.0f * PI; + + int uSubdivisions = (int)glm::ceil(subdivisions * 0.5f * percentage); + int vSubdivisions = subdivisions; + + float uDeltaAngle = uTotalAngle / uSubdivisions; + float vDeltaAngle = vTotalAngle / vSubdivisions; + + float uAngle = 0.0f; + + glm::vec3 uAxis, vAxis; + glm::vec3 mainAxis = glm::normalize(direction); + if (mainAxis.y == 1.0f || mainAxis.y == -1.0f) { + uAxis = glm::vec3(1.0f, 0.0f, 0.0f); + vAxis = glm::vec3(0.0f, 0.0f, 1.0f); + } else { + uAxis = glm::normalize(glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), mainAxis)); + vAxis = glm::normalize(glm::cross(mainAxis, uAxis)); + if ((uAxis.z == 0.0f && uAxis.x < 0.0f) || (uAxis.x == 0.0f && uAxis.z < 0.0f)) { + uAxis = -uAxis; + } else if (uAxis.x < 0.0f) { + uAxis = -uAxis; + } + if ((vAxis.z == 0.0f && vAxis.x < 0.0f) || (vAxis.x == 0.0f && vAxis.z < 0.0f)) { + vAxis = -vAxis; + } else if (vAxis.x < 0.0f) { + vAxis = -vAxis; + } + } + + std::vector> arcs; + auto origin = center; + for (int u = 0; u < uSubdivisions + 1; u++) { + std::vector arc; + glm::vec3 arcCenter = origin + mainAxis * (glm::cos(uAngle) * radius); + float vAngle = 0.0f; + for (int v = 0; v < vSubdivisions + 1; v++) { + float arcRadius = glm::abs(glm::sin(uAngle) * radius); + glm::vec3 arcPoint = arcCenter + (arcRadius * glm::cos(vAngle)) * uAxis + (arcRadius * glm::sin(vAngle)) * vAxis; + arc.push_back(arcPoint); + if (u == uSubdivisions && edge != nullptr) { + edge->push_back(arcPoint); + } + vAngle += vDeltaAngle; + } + arc.push_back(arc[0]); + arcs.push_back(arc); + uAngle += uDeltaAngle; + } + + for (size_t i = 1; i < arcs.size(); i++) { + auto arc1 = arcs[i]; + auto arc2 = arcs[i - 1]; + for (size_t j = 1; j < arc1.size(); j++) { + auto point1 = arc1[j]; + auto point2 = arc1[j - 1]; + auto point3 = arc2[j]; + std::pair line1 = { point1, point2 }; + std::pair line2 = { point1, point3 }; + outLines.push_back(line1); + outLines.push_back(line2); + } + } +} + +void MultiSphereShape::setScale(float scale) { + if (scale != _scale) { + float deltaScale = scale / _scale; + for (auto& sphere : _spheres) { + sphere._axis *= deltaScale; + sphere._position *= deltaScale; + sphere._radius *= deltaScale; + } + for (auto& line : _debugLines) { + line.first *= deltaScale; + line.second *= deltaScale; + } + _scale = scale; + } +} + +AABox& MultiSphereShape::updateBoundingBox(const glm::vec3& position, const glm::quat& rotation) { + _boundingBox = AABox(); + auto spheres = getSpheresData(); + for (size_t i = 0; i < spheres.size(); i++) { + auto sphere = spheres[i]; + auto worldPosition = position + rotation * sphere._position; + glm::vec3 corner = worldPosition - glm::vec3(sphere._radius); + glm::vec3 dimensions = glm::vec3(2.0f * sphere._radius); + _boundingBox += AABox(corner, dimensions); + } + return _boundingBox; +} \ No newline at end of file diff --git a/libraries/physics/src/MultiSphereShape.h b/libraries/physics/src/MultiSphereShape.h new file mode 100644 index 0000000000..e5f19ba91a --- /dev/null +++ b/libraries/physics/src/MultiSphereShape.h @@ -0,0 +1,112 @@ +// +// MultiSphereShape.h +// libraries/physics/src +// +// Created by Luis Cuenca 5/11/2018 +// Copyright 2018 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_MultiSphereShape_h +#define hifi_MultiSphereShape_h + +#include +#include +#include +#include +#include "BulletUtil.h" + + +enum CollisionShapeExtractionMode { + None = 0, + Automatic, + Box, + Sphere, + SphereCollapse, + SpheresX, + SpheresY, + SpheresZ, + SpheresXY, + SpheresYZ, + SpheresXZ, + SpheresXYZ +}; + +struct SphereShapeData { + SphereShapeData() {} + glm::vec3 _position; + glm::vec3 _axis; + float _radius; +}; + +class SphereRegion { +public: + SphereRegion() {} + SphereRegion(const glm::vec3& direction) : _direction(direction) {} + void extractSphereRegion(std::vector>& outLines); + void extractEdges(bool reverseY = false); + void translate(const glm::vec3& translation); + void dump(std::vector>& outLines); + const glm::vec3& getDirection() const { return _direction; } + const std::vector& getEdgesX() const { return _edgesX; } + const std::vector& getEdgesY() const { return _edgesY; } + const std::vector& getEdgesZ() const { return _edgesZ; } +private: + void insertUnique(const glm::vec3& point, std::vector& pointSet); + + std::vector> _lines; + std::vector _edgesX; + std::vector _edgesY; + std::vector _edgesZ; + glm::vec3 _direction; +}; + +const int DEFAULT_SPHERE_SUBDIVISIONS = 16; + +const std::vector CORNER_SIGNS = { + glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(-1.0f, 1.0f, 1.0f), + glm::vec3(-1.0f, 1.0f, -1.0f), glm::vec3(1.0f, 1.0f, -1.0f), + glm::vec3(1.0f, -1.0f, 1.0f), glm::vec3(-1.0f, -1.0f, 1.0f), + glm::vec3(-1.0f, -1.0f, -1.0f), glm::vec3(1.0f, -1.0f, -1.0f) }; + +class MultiSphereShape { +public: + MultiSphereShape() {}; + bool computeMultiSphereShape(int jointIndex, const QString& name, const std::vector& points, float scale = 1.0f); + void calculateDebugLines(); + const std::vector& getSpheresData() const { return _spheres; } + const std::vector>& getDebugLines() const { return _debugLines; } + void setScale(float scale); + AABox& updateBoundingBox(const glm::vec3& position, const glm::quat& rotation); + const AABox& getBoundingBox() const { return _boundingBox; } + int getJointIndex() const { return _jointIndex; } + QString getJointName() const { return _name; } + bool isValid() const { return _spheres.size() > 0; } + +private: + CollisionShapeExtractionMode getExtractionModeByName(const QString& name); + void filterUniquePoints(const std::vector& kdop, std::vector& uniquePoints); + void spheresFromAxes(const std::vector& points, const std::vector& axes, + std::vector& spheres); + + void calculateSphereLines(std::vector>& outLines, const glm::vec3& center, const float& radius, + const int& subdivisions = DEFAULT_SPHERE_SUBDIVISIONS, const glm::vec3& direction = Vectors::UNIT_Y, + const float& percentage = 1.0f, std::vector* edge = nullptr); + void calculateChamferBox(std::vector>& outLines, const float& radius, const std::vector& axes, const glm::vec3& translation); + void connectEdges(std::vector>& outLines, const std::vector& edge1, + const std::vector& edge2, bool reverse = false); + void connectSpheres(int index1, int index2, bool onlyEdges = false); + + int _jointIndex { -1 }; + QString _name; + std::vector _spheres; + std::vector> _debugLines; + CollisionShapeExtractionMode _mode; + glm::vec3 _midPoint; + float _scale { 1.0f }; + AABox _boundingBox; +}; + +#endif // hifi_MultiSphereShape_h \ No newline at end of file diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index acfb0c9236..0ab051fa96 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -198,7 +198,7 @@ void ObjectMotionState::setShape(const btCollisionShape* shape) { getShapeManager()->releaseShape(_shape); } _shape = shape; - if (_body) { + if (_body && _type != MOTIONSTATE_TYPE_DETAILED) { updateCCDConfiguration(); } } @@ -310,7 +310,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* // the shape didn't actually change, so we clear the DIRTY_SHAPE flag flags &= ~Simulation::DIRTY_SHAPE; // and clear the reference we just created - getShapeManager()->releaseShape(_shape); + getShapeManager()->releaseShape(_shape); } else { _body->setCollisionShape(const_cast(newShape)); setShape(newShape); diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index e5a61834e4..fe175a2c7d 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -58,7 +58,8 @@ inline QString motionTypeToString(PhysicsMotionType motionType) { enum MotionStateType { MOTIONSTATE_TYPE_INVALID, MOTIONSTATE_TYPE_ENTITY, - MOTIONSTATE_TYPE_AVATAR + MOTIONSTATE_TYPE_AVATAR, + MOTIONSTATE_TYPE_DETAILED }; // The update flags trigger two varieties of updates: "hard" which require the body to be pulled diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index bf6e2463e5..a571a81109 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -327,7 +327,7 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) } if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) { _activeStaticBodies.insert(object->getRigidBody()); - } + } } // activeStaticBodies have changed (in an Easy way) and need their Aabbs updated // but we've configured Bullet to NOT update them automatically (for improved performance) diff --git a/libraries/physics/src/ShapeFactory.cpp b/libraries/physics/src/ShapeFactory.cpp index d7ba2f0661..5808a539d6 100644 --- a/libraries/physics/src/ShapeFactory.cpp +++ b/libraries/physics/src/ShapeFactory.cpp @@ -284,6 +284,17 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info) shape = new btSphereShape(radius); } break; + case SHAPE_TYPE_MULTISPHERE: { + std::vector positions; + std::vector radiuses; + auto sphereCollection = info.getSphereCollection(); + for (auto &sphereData : sphereCollection) { + positions.push_back(glmToBullet(glm::vec3(sphereData))); + radiuses.push_back(sphereData.w); + } + shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size()); + } + break; case SHAPE_TYPE_ELLIPSOID: { glm::vec3 halfExtents = info.getHalfExtents(); float radius = halfExtents.x; diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index 4094097741..f6189121a9 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -174,3 +174,46 @@ void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) { } } } + +void ThreadSafeDynamicsWorld::drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, const btVector3& position2, const btVector3& color) { + float stepRadians = PI/6.0f; // 30 degrees + btVector3 direction = position2 - position1; + btVector3 xAxis = direction.cross(btVector3(0.0f, 1.0f, 0.0f)); + xAxis = xAxis.length() < EPSILON ? btVector3(1.0f, 0.0f, 0.0f) : xAxis.normalize(); + btVector3 zAxis = xAxis.cross(btVector3(0.0f, 1.0f, 0.0f)); + zAxis = (direction.normalize().getY() < EPSILON) ? btVector3(0.0f, 1.0f, 0.0f) : zAxis.normalize(); + float fullCircle = 2.0f * PI; + for (float i = 0; i < fullCircle; i += stepRadians) { + float x1 = btSin(btScalar(i)) * radius1; + float z1 = btCos(btScalar(i)) * radius1; + float x2 = btSin(btScalar(i)) * radius2; + float z2 = btCos(btScalar(i)) * radius2; + + btVector3 addVector1 = xAxis * x1 + zAxis * z1; + btVector3 addVector2 = xAxis * x2 + zAxis * z2; + drawer->drawLine(position1 + addVector1, position2 + addVector2, color); + } +} + +void ThreadSafeDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { + btCollisionWorld::debugDrawObject(worldTransform, shape, color); + if (shape->getShapeType() == MULTI_SPHERE_SHAPE_PROXYTYPE) { + const btMultiSphereShape* multiSphereShape = static_cast(shape); + for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) { + btTransform sphereTransform1, sphereTransform2; + sphereTransform1.setIdentity(); + sphereTransform2.setIdentity(); + int sphereIndex1 = i; + int sphereIndex2 = i > 0 ? i - 1 : multiSphereShape->getSphereCount() - 1; + sphereTransform1.setOrigin(multiSphereShape->getSpherePosition(sphereIndex1)); + sphereTransform2.setOrigin(multiSphereShape->getSpherePosition(sphereIndex2)); + sphereTransform1 = worldTransform * sphereTransform1; + sphereTransform2 = worldTransform * sphereTransform2; + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(sphereIndex1), sphereTransform1, color); + drawConnectedSpheres(getDebugDrawer(), multiSphereShape->getSphereRadius(sphereIndex1), multiSphereShape->getSphereRadius(sphereIndex2), sphereTransform1.getOrigin(), sphereTransform2.getOrigin(), color); + } + } else { + btCollisionWorld::debugDrawObject(worldTransform, shape, color); + } +} + diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h index d8cee4d2de..021142d91e 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h @@ -53,10 +53,13 @@ public: const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; } void addChangedMotionState(ObjectMotionState* motionState) { _changedMotionStates.push_back(motionState); } + virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) override; private: // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState() void synchronizeMotionState(btRigidBody* body); + void drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, + const btVector3& position2, const btVector3& color); VectorOfMotionStates _changedMotionStates; VectorOfMotionStates _deactivatedStates; diff --git a/libraries/shared/src/DebugDraw.cpp b/libraries/shared/src/DebugDraw.cpp index f17671da4d..1b2418f7c7 100644 --- a/libraries/shared/src/DebugDraw.cpp +++ b/libraries/shared/src/DebugDraw.cpp @@ -74,3 +74,13 @@ void DebugDraw::clearRays() { Lock lock(_mapMutex); _rays.clear(); } + +void DebugDraw::drawRays(const std::vector>& lines, + const glm::vec4& color, const glm::vec3& translation, const glm::quat& rotation) { + Lock lock(_mapMutex); + for (std::pair line : lines) { + auto point1 = translation + rotation * line.first; + auto point2 = translation + rotation * line.second; + _rays.push_back(Ray(point1, point2, color)); + } +} \ No newline at end of file diff --git a/libraries/shared/src/DebugDraw.h b/libraries/shared/src/DebugDraw.h index 7dd19415c9..81acbf554c 100644 --- a/libraries/shared/src/DebugDraw.h +++ b/libraries/shared/src/DebugDraw.h @@ -47,6 +47,16 @@ public: * @param {Vec4} color - color of line, each component should be in the zero to one range. x = red, y = blue, z = green, w = alpha. */ Q_INVOKABLE void drawRay(const glm::vec3& start, const glm::vec3& end, const glm::vec4& color); + + /**jsdoc + * Draws a line in world space, but it will only be visible for a single frame. + * @function DebugDraw.drawRay + * @param {Vec3} start - start position of line in world space. + * @param {Vec3} end - end position of line in world space. + * @param {Vec4} color - color of line, each component should be in the zero to one range. x = red, y = blue, z = green, w = alpha. + */ + Q_INVOKABLE void drawRays(const std::vector>& lines, const glm::vec4& color, + const glm::vec3& translation = glm::vec3(0.0f, 0.0f, 0.0f), const glm::quat& rotation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f)); /**jsdoc * Adds a debug marker to the world. This marker will be drawn every frame until it is removed with DebugDraw.removeMarker. diff --git a/libraries/shared/src/PhysicsCollisionGroups.h b/libraries/shared/src/PhysicsCollisionGroups.h index cae3918a3f..be641b5cd2 100644 --- a/libraries/shared/src/PhysicsCollisionGroups.h +++ b/libraries/shared/src/PhysicsCollisionGroups.h @@ -39,6 +39,8 @@ const int32_t BULLET_COLLISION_GROUP_DYNAMIC = 1 << 1; const int32_t BULLET_COLLISION_GROUP_KINEMATIC = 1 << 2; const int32_t BULLET_COLLISION_GROUP_MY_AVATAR = 1 << 3; const int32_t BULLET_COLLISION_GROUP_OTHER_AVATAR = 1 << 4; +const int32_t BULLET_COLLISION_GROUP_DETAILED_AVATAR = 1 << 5; +const int32_t BULLET_COLLISION_GROUP_DETAILED_RAY = 1 << 6; // ... const int32_t BULLET_COLLISION_GROUP_COLLISIONLESS = 1 << 31; @@ -64,7 +66,8 @@ const int32_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISI // to move its avatar around correctly and to communicate its motion through the avatar-mixer. // Therefore, they only need to collide against things that can be affected by their motion: dynamic and MyAvatar const int32_t BULLET_COLLISION_MASK_OTHER_AVATAR = BULLET_COLLISION_GROUP_DYNAMIC | BULLET_COLLISION_GROUP_MY_AVATAR; - +const int32_t BULLET_COLLISION_MASK_DETAILED_AVATAR = BULLET_COLLISION_GROUP_DETAILED_RAY; +const int32_t BULLET_COLLISION_MASK_DETAILED_RAY = BULLET_COLLISION_GROUP_DETAILED_AVATAR; // COLLISIONLESS gets an empty mask. const int32_t BULLET_COLLISION_MASK_COLLISIONLESS = 0; diff --git a/libraries/shared/src/PhysicsHelpers.cpp b/libraries/shared/src/PhysicsHelpers.cpp index 988af98c46..092b9d078a 100644 --- a/libraries/shared/src/PhysicsHelpers.cpp +++ b/libraries/shared/src/PhysicsHelpers.cpp @@ -78,6 +78,10 @@ int32_t Physics::getDefaultCollisionMask(int32_t group) { return BULLET_COLLISION_MASK_MY_AVATAR; case BULLET_COLLISION_GROUP_OTHER_AVATAR: return BULLET_COLLISION_MASK_OTHER_AVATAR; + case BULLET_COLLISION_GROUP_DETAILED_AVATAR: + return BULLET_COLLISION_MASK_DETAILED_AVATAR; + case BULLET_COLLISION_GROUP_DETAILED_RAY: + return BULLET_COLLISION_MASK_DETAILED_RAY; default: break; }; diff --git a/libraries/shared/src/ShapeInfo.cpp b/libraries/shared/src/ShapeInfo.cpp index 3426a79782..564d79bfda 100644 --- a/libraries/shared/src/ShapeInfo.cpp +++ b/libraries/shared/src/ShapeInfo.cpp @@ -38,6 +38,7 @@ * sub-meshes. * "static-mesh"The exact shape of the model. * "plane"A plane. + * "multisphere"A convex hull generated from a set of spheres. * * * @typedef {string} ShapeType @@ -59,7 +60,9 @@ const char* shapeTypeNames[] = { "simple-hull", "simple-compound", "static-mesh", - "ellipsoid" + "ellipsoid", + "circle", + "multisphere" }; static const size_t SHAPETYPE_NAME_COUNT = (sizeof(shapeTypeNames) / sizeof((shapeTypeNames)[0])); @@ -90,6 +93,7 @@ void ShapeInfo::clear() { _url.clear(); _pointCollection.clear(); _triangleIndices.clear(); + _sphereCollection.clear(); _halfExtents = glm::vec3(0.0f); _offset = glm::vec3(0.0f); _hashKey.clear(); @@ -106,6 +110,7 @@ void ShapeInfo::setParams(ShapeType type, const glm::vec3& halfExtents, QString break; case SHAPE_TYPE_BOX: case SHAPE_TYPE_HULL: + case SHAPE_TYPE_MULTISPHERE: break; case SHAPE_TYPE_SPHERE: { float radius = glm::length(halfExtents) / SQUARE_ROOT_OF_3; @@ -144,6 +149,17 @@ void ShapeInfo::setSphere(float radius) { _hashKey.clear(); } +void ShapeInfo::setMultiSphere(const std::vector& centers, const std::vector& radiuses) { + _url = ""; + _type = SHAPE_TYPE_MULTISPHERE; + assert(centers.size() == radiuses.size() && centers.size() > 0); + for (size_t i = 0; i < centers.size(); i++) { + SphereData sphere = SphereData(centers[i], radiuses[i]); + _sphereCollection.push_back(sphere); + } + _hashKey.clear(); +} + void ShapeInfo::setPointCollection(const ShapeInfo::PointCollection& pointCollection) { _pointCollection = pointCollection; _hashKey.clear(); @@ -170,6 +186,7 @@ uint32_t ShapeInfo::getNumSubShapes() const { case SHAPE_TYPE_COMPOUND: case SHAPE_TYPE_SIMPLE_COMPOUND: return _pointCollection.size(); + case SHAPE_TYPE_MULTISPHERE: case SHAPE_TYPE_SIMPLE_HULL: case SHAPE_TYPE_STATIC_MESH: assert(_pointCollection.size() == 1); @@ -257,7 +274,12 @@ const HashKey& ShapeInfo::getHash() const { // The key is not yet cached therefore we must compute it. _hashKey.hashUint64((uint64_t)_type); - if (_type != SHAPE_TYPE_SIMPLE_HULL) { + if (_type == SHAPE_TYPE_MULTISPHERE) { + for (auto &sphereData : _sphereCollection) { + _hashKey.hashVec3(glm::vec3(sphereData)); + _hashKey.hashFloat(sphereData.w); + } + } else if (_type != SHAPE_TYPE_SIMPLE_HULL) { _hashKey.hashVec3(_halfExtents); _hashKey.hashVec3(_offset); } else { @@ -283,9 +305,12 @@ const HashKey& ShapeInfo::getHash() const { if (_type == SHAPE_TYPE_COMPOUND || _type == SHAPE_TYPE_SIMPLE_COMPOUND) { uint64_t numHulls = (uint64_t)_pointCollection.size(); _hashKey.hashUint64(numHulls); + } else if (_type == SHAPE_TYPE_MULTISPHERE) { + uint64_t numSpheres = (uint64_t)_sphereCollection.size(); + _hashKey.hashUint64(numSpheres); } else if (_type == SHAPE_TYPE_SIMPLE_HULL) { _hashKey.hashUint64(1); - } + } } return _hashKey; } diff --git a/libraries/shared/src/ShapeInfo.h b/libraries/shared/src/ShapeInfo.h index a2092c7f00..d838d7b214 100644 --- a/libraries/shared/src/ShapeInfo.h +++ b/libraries/shared/src/ShapeInfo.h @@ -47,7 +47,8 @@ enum ShapeType { SHAPE_TYPE_SIMPLE_COMPOUND, SHAPE_TYPE_STATIC_MESH, SHAPE_TYPE_ELLIPSOID, - SHAPE_TYPE_CIRCLE + SHAPE_TYPE_CIRCLE, + SHAPE_TYPE_MULTISPHERE }; class ShapeInfo { @@ -57,6 +58,8 @@ public: using PointList = QVector; using PointCollection = QVector; using TriangleIndices = QVector; + using SphereData = glm::vec4; + using SphereCollection = QVector; static QString getNameForShapeType(ShapeType type); static ShapeType getShapeTypeForName(QString string); @@ -68,7 +71,8 @@ public: void setSphere(float radius); void setPointCollection(const PointCollection& pointCollection); void setCapsuleY(float radius, float cylinderHalfHeight); - void setOffset(const glm::vec3& offset); + void setMultiSphere(const std::vector& centers, const std::vector& radiuses); + void setOffset(const glm::vec3& offset); ShapeType getType() const { return _type; } @@ -78,6 +82,7 @@ public: PointCollection& getPointCollection() { return _pointCollection; } const PointCollection& getPointCollection() const { return _pointCollection; } + const SphereCollection& getSphereCollection() const { return _sphereCollection; } TriangleIndices& getTriangleIndices() { return _triangleIndices; } const TriangleIndices& getTriangleIndices() const { return _triangleIndices; } @@ -92,6 +97,7 @@ protected: void setHalfExtents(const glm::vec3& halfExtents); QUrl _url; // url for model of convex collision hulls + SphereCollection _sphereCollection; PointCollection _pointCollection; TriangleIndices _triangleIndices; glm::vec3 _halfExtents = glm::vec3(0.0f);