From 4a8bbd79fe97aeef13f15120831d2d7bb3b252e8 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Sun, 30 Dec 2018 10:53:28 -0700 Subject: [PATCH 01/19] other avatars motion states --- interface/src/avatar/AvatarManager.cpp | 52 +- interface/src/avatar/AvatarManager.h | 1 + interface/src/avatar/DetailedMotionState.cpp | 207 +++++++ interface/src/avatar/DetailedMotionState.h | 95 ++++ interface/src/avatar/OtherAvatar.cpp | 38 ++ interface/src/avatar/OtherAvatar.h | 7 + .../src/avatars-renderer/Avatar.cpp | 32 +- .../src/avatars-renderer/Avatar.h | 4 + libraries/physics/src/MultiSphereShape.cpp | 527 ++++++++++++++++++ libraries/physics/src/MultiSphereShape.h | 102 ++++ libraries/physics/src/ObjectMotionState.cpp | 2 +- libraries/physics/src/ObjectMotionState.h | 3 +- 12 files changed, 1056 insertions(+), 14 deletions(-) create mode 100644 interface/src/avatar/DetailedMotionState.cpp create mode 100644 interface/src/avatar/DetailedMotionState.h create mode 100644 libraries/physics/src/MultiSphereShape.cpp create mode 100644 libraries/physics/src/MultiSphereShape.h diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 7ca18ca258..e98e082b35 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -407,21 +407,51 @@ 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) { + if (mState) { + transaction.objectsToRemove.push_back(mState); + } + } + qCDebug(animation) << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); + 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); - } else { - failedShapeBuilds.insert(avatar); + { + 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); + } + } + + { + for (int i = 0; i < avatar->getJointCount(); i++) { + avatar->addNewMotionState(avatar, i); + } + auto& detailedMotionStates = avatar->getDetailedMotionStates(); + for (auto& mState : detailedMotionStates) { + transaction.objectsToAdd.push_back(mState); + } + qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); + auto& detailedMotionStates = avatar->getDetailedMotionStates(); + for (auto& mState : detailedMotionStates) { + if (mState) { + transaction.objectsToChange.push_back(mState); + } + } + qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } } _avatarsToChangeInPhysics.swap(failedShapeBuilds); @@ -519,7 +549,7 @@ void AvatarManager::deleteAllAvatars() { avatar->die(); if (avatar != _myAvatar) { auto otherAvatar = std::static_pointer_cast(avatar); - assert(!otherAvatar->_motionState); + assert(!otherAvatar->_motionState && !otherAvatar->_motionState2); } } } diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 75dbbc7abb..0fa7f19f2b 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" diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp new file mode 100644 index 0000000000..43cd77c558 --- /dev/null +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -0,0 +1,207 @@ +// +// DetailedMotionState.cpp +// interface/src/avatar/ +// +// Created by Andrew Meadows 2015.05.14 +// Copyright 2015 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 + + +DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : + ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { + assert(_avatar); + _type = MOTIONSTATE_TYPE_DETAILED; + cacheShapeDiameter(); +} + +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() { + ShapeInfo shapeInfo; + _avatar->computeShapeInfo(shapeInfo); + return getShapeManager()->getShape(shapeInfo); +} + +// virtual +bool DetailedMotionState::isMoving() const { + return false; +} + +// virtual +void DetailedMotionState::getWorldTransform(btTransform& worldTrans) const { + worldTrans.setOrigin(glmToBullet(getObjectPosition())); + worldTrans.setRotation(glmToBullet(getObjectRotation())); + if (_body) { + _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity())); + _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); + } +} + +// virtual +void DetailedMotionState::setWorldTransform(const btTransform& worldTrans) { + const float SPRING_TIMESCALE = 0.5f; + float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE; + btVector3 currentPosition = worldTrans.getOrigin(); + btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition; + float distance = offsetToTarget.length(); + if ((1.0f - tau) * distance > _diameter) { + // the avatar body is far from its target --> slam position + btTransform newTransform; + newTransform.setOrigin(currentPosition + offsetToTarget); + newTransform.setRotation(glmToBullet(getObjectRotation())); + _body->setWorldTransform(newTransform); + _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity())); + _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); + } else { + // the avatar body is near its target --> slam velocity + btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget; + _body->setLinearVelocity(velocity); + _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); + // slam its rotation + btTransform newTransform = worldTrans; + newTransform.setRotation(glmToBullet(getObjectRotation())); + _body->setWorldTransform(newTransform); + } +} + +// 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 { + return _avatar->getJointPosition(_jointIndex); +} + +// virtual +glm::quat DetailedMotionState::getObjectRotation() const { + return _avatar->getWorldOrientation() * _avatar->getAbsoluteJointRotationInObjectFrame(_jointIndex); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectLinearVelocity() const { + return _avatar->getWorldVelocity(); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectAngularVelocity() const { + // HACK: avatars use a capusle collision shape and their angularVelocity in the local simulation is unimportant. + // Therefore, as optimization toward support for larger crowds we ignore it and return zero. + //return _avatar->getWorldAngularVelocity(); + return glm::vec3(0.0f); +} + +// virtual +glm::vec3 DetailedMotionState::getObjectGravity() const { + return _avatar->getAcceleration(); +} + +// virtual +const QUuid DetailedMotionState::getObjectID() const { + return _avatar->getSessionUUID(); +} + +QString DetailedMotionState::getName() const { + return _avatar->getName(); +} + +// virtual +QUuid DetailedMotionState::getSimulatorID() const { + return _avatar->getSessionUUID(); +} + +// virtual +void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const { + group = BULLET_COLLISION_GROUP_OTHER_AVATAR; + mask = Physics::getDefaultCollisionMask(group); +} + +// virtual +float DetailedMotionState::getMass() const { + return _avatar->computeMass(); +} + +void DetailedMotionState::cacheShapeDiameter() { + if (_shape) { + // shape is capsuleY + btVector3 aabbMin, aabbMax; + btTransform transform; + transform.setIdentity(); + _shape->getAabb(transform, aabbMin, aabbMax); + _diameter = (aabbMax - aabbMin).getX(); + } else { + _diameter = 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); + cacheShapeDiameter(); +} diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h new file mode 100644 index 0000000000..1c5f224e4a --- /dev/null +++ b/interface/src/avatar/DetailedMotionState.h @@ -0,0 +1,95 @@ +// +// DetailedMotionState.h +// interface/src/avatar/ +// +// Created by Andrew Meadows 2015.05.14 +// Copyright 2015 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(OtherAvatarPointer 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 setBoundingBox(const glm::vec3& corner, const glm::vec3& diagonal); + + void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; } + + virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override; + + virtual float getMass() const override; + + friend class AvatarManager; + friend class Avatar; + +protected: + void setRigidBody(btRigidBody* body) override; + void setShape(const btCollisionShape* shape) override; + void cacheShapeDiameter(); + + // 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; + + OtherAvatarPointer _avatar; + float _diameter { 0.0f }; + + uint32_t _dirtyFlags; + int _jointIndex { -1 }; +}; + +#endif // hifi_DetailedMotionState_h diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index c2687fd525..30793f1696 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -10,6 +10,7 @@ #include "Application.h" #include "AvatarMotionState.h" +#include "DetailedMotionState.h" static glm::u8vec3 getLoadingOrbColor(Avatar::LoadingStatus loadingStatus) { @@ -107,10 +108,47 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { int32_t bytesRead = Avatar::parseDataFromBuffer(buffer); if (_moving && _motionState) { _motionState->addDirtyFlags(Simulation::DIRTY_POSITION); + for (auto mState : _detailedMotionStates) { + if (mState) { + mState->addDirtyFlags(Simulation::DIRTY_POSITION); + } + } } return bytesRead; } +void OtherAvatar::addNewMotionState(std::shared_ptr avatar, int jointIndex) { + std::lock_guard lock(_mStateLock); + if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { + auto& data = _multiSphereShapes[jointIndex].getSpheresData(); + std::vector positions; + std::vector radiuses; + for (auto& sphere : data) { + positions.push_back(glmToBullet(sphere._position)); + radiuses.push_back(sphere._radius); + } + btCollisionShape* shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size()); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex); + motionState->setMass(computeMass()); + assert(_detailedMotionStates.size() == jointIndex); + _detailedMotionStates.push_back(motionState); + } else { + _detailedMotionStates.push_back(nullptr); + } + } +} +const std::vector& OtherAvatar::getDetailedMotionStates() { + std::lock_guard lock(_mStateLock); + return _detailedMotionStates; +} +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; } diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index 5b72815757..a337d5d299 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -20,6 +20,7 @@ class AvatarManager; class AvatarMotionState; +class DetailedMotionState; class OtherAvatar : public Avatar { public: @@ -45,14 +46,20 @@ public: bool shouldBeInPhysicsSimulation() const; bool needsPhysicsUpdate() const; + void addNewMotionState(std::shared_ptr avatar, int jointIndex); + const std::vector& getDetailedMotionStates(); + void resetDetailedMotionStates(); + friend AvatarManager; 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 }; + std::mutex _mStateLock; }; using OtherAvatarPointer = std::shared_ptr; diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index fceb146470..011247b796 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -296,7 +296,9 @@ void Avatar::setTargetScale(float targetScale) { _targetScale = newValue; _scaleChanged = usecTimestampNow(); _isAnimatingScale = true; - + for (auto& sphere : _multiSphereShapes) { + sphere.setScale(_targetScale); + } emit targetScaleChanged(targetScale); } } @@ -1540,6 +1542,7 @@ void Avatar::setModelURLFinished(bool success) { // rig is ready void Avatar::rigReady() { buildUnscaledEyeHeightCache(); + computeMultiSphereShapes(); } // rig has been reset. @@ -1547,6 +1550,33 @@ void Avatar::rigReset() { clearUnscaledEyeHeightCache(); } +void Avatar::computeMultiSphereShapes() { + const Rig& rig = getSkeletonModel()->getRig(); + auto scale = extractScale(rig.getGeometryToRigTransform()); + 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 (size_t 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(jointName, btPoints, getSensorToWorldScale())) { + multiSphereShape.calculateDebugLines(); + } + _multiSphereShapes.push_back(multiSphereShape); + } +} + // 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) { diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h index 8f70b12122..76a5a2a9d6 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h @@ -29,6 +29,7 @@ #include #include "MetaModelPayload.h" +#include "MultiSphereShape.h" namespace render { template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar); @@ -507,6 +508,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(); + const std::vector& getMultiSphereShapes() const { return _multiSphereShapes; } SkeletonModelPointer _skeletonModel; @@ -628,6 +631,7 @@ protected: static void metaBlendshapeOperator(render::ItemID renderItemID, int blendshapeNumber, const QVector& blendshapeOffsets, const QVector& blendedMeshSizes, const render::ItemIDs& subItemIDs); + std::vector _multiSphereShapes; }; #endif // hifi_Avatar_h diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp new file mode 100644 index 0000000000..f0f56d7cf8 --- /dev/null +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -0,0 +1,527 @@ +// +// 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 (pointFromSet == point); + }); + 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; + //qDebug() << "Trying to add " << (int)positions.size() << " spheres for " << jointName << " length: " << maxLength; + } + 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 (btPoint.getX() == point.x + && btPoint.getY() == point.y + && btPoint.getZ() == point.z); + }); + if (hit == uniquePoints.end()) { + uniquePoints.push_back(bulletToGLM(btPoint)); + } + } +} + +bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::vector& kdop, float scale) { + _scale = scale; + _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); + float distance = glm::length(points[i]); + if (dot > 0.0f) { + float distancePow = glm::pow(distance, 2); + float dotPow = glm::pow(dot, 2); + float radius = (dot / maxDistance) * glm::sqrt(distancePow - dotPow); + averageRadius += radius; + maxRadius = radius > maxRadius ? radius : maxRadius; + } + } + averageRadius /= (int)points.size(); + maxAverageRadius = averageRadius > maxAverageRadius ? averageRadius : maxAverageRadius; + minAverageRadius = averageRadius < minAverageRadius ? averageRadius : minAverageRadius; + spheres[j]._radius = averageRadius; + } + 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) * glm::normalize(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 percent1 = ((0.5f * PI) + angleOffset) / PI; + float percent2 = ((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), percent1, &edge1); + calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2); + } else { + calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), percent1, &edge1); + calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &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) { + if (sign.y == CORNER_SIGNS[i].y && sign.z == CORNER_SIGNS[i].z) { + add = true; + } + } else if (sign.y == 0) { + if (sign.x == CORNER_SIGNS[i].x && sign.z == CORNER_SIGNS[i].z) { + add = true; + } + } else if (sign.z == 0) { + 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 && uAxis.x < 0) || (uAxis.x == 0 && uAxis.z < 0)) { + uAxis = -uAxis; + } else if (uAxis.x < 0) { + uAxis = -uAxis; + } + if ((vAxis.z == 0 && vAxis.x < 0) || (vAxis.x == 0 && vAxis.z < 0)) { + vAxis = -vAxis; + } + else if (vAxis.x < 0) { + 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; + } +} \ 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..f8f251de42 --- /dev/null +++ b/libraries/physics/src/MultiSphereShape.h @@ -0,0 +1,102 @@ +// +// 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 "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(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); + +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); + std::vector _spheres; + std::vector> _debugLines; + CollisionShapeExtractionMode _mode; + glm::vec3 _midPoint; + float _scale { 1.0f }; +}; + +#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..4bc0389620 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -292,7 +292,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* if (!isReadyToComputeShape()) { return false; } - const btCollisionShape* newShape = computeNewShape(); + const btCollisionShape* newShape = _type != MOTIONSTATE_TYPE_DETAILED ? computeNewShape() : nullptr; if (!newShape) { qCDebug(physics) << "Warning: failed to generate new shape!"; // failed to generate new shape! --> keep old shape and remove shape-change flag diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index 74173c3f47..a1dc0f8368 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 From 21a4da4d5f5c3aeeee5f9df5198271e42dd877d8 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Thu, 3 Jan 2019 13:36:26 -0700 Subject: [PATCH 02/19] fix collisions for newly added avatars --- interface/src/avatar/AvatarManager.cpp | 13 ++++++------- interface/src/avatar/OtherAvatar.cpp | 6 +----- interface/src/avatar/OtherAvatar.h | 5 ++--- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index e98e082b35..b80d53e6c3 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -417,7 +417,7 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact avatar->resetDetailedMotionStates(); } else { - { + if (avatar->_motionState == nullptr) { ShapeInfo shapeInfo; avatar->computeShapeInfo(shapeInfo); btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); @@ -427,21 +427,20 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact avatar->_motionState = motionState; transaction.objectsToAdd.push_back(motionState); } - else { - failedShapeBuilds.insert(avatar); - } } - - { + auto& detailedMotionStates = avatar->getDetailedMotionStates(); + if (detailedMotionStates.size() == 0) { for (int i = 0; i < avatar->getJointCount(); i++) { avatar->addNewMotionState(avatar, i); } - auto& detailedMotionStates = avatar->getDetailedMotionStates(); for (auto& mState : detailedMotionStates) { transaction.objectsToAdd.push_back(mState); } qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } + if (avatar->_motionState == nullptr || detailedMotionStates.size() == 0) { + failedShapeBuilds.insert(avatar); + } } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 30793f1696..7495e5e2df 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -118,7 +118,6 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { } void OtherAvatar::addNewMotionState(std::shared_ptr avatar, int jointIndex) { - std::lock_guard lock(_mStateLock); if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { auto& data = _multiSphereShapes[jointIndex].getSpheresData(); std::vector positions; @@ -138,10 +137,7 @@ void OtherAvatar::addNewMotionState(std::shared_ptr avatar, int joi } } } -const std::vector& OtherAvatar::getDetailedMotionStates() { - std::lock_guard lock(_mStateLock); - return _detailedMotionStates; -} + void OtherAvatar::resetDetailedMotionStates() { for (size_t i = 0; i < _detailedMotionStates.size(); i++) { _detailedMotionStates[i] = nullptr; diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index a337d5d299..18b93b8aad 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -39,7 +39,7 @@ public: int parseDataFromBuffer(const QByteArray& buffer) override; - bool isInPhysicsSimulation() const { return _motionState != nullptr; } + bool isInPhysicsSimulation() const { return _motionState != nullptr && _detailedMotionStates.size() > 0; } void rebuildCollisionShape() override; void setWorkloadRegion(uint8_t region); @@ -47,7 +47,7 @@ public: bool needsPhysicsUpdate() const; void addNewMotionState(std::shared_ptr avatar, int jointIndex); - const std::vector& getDetailedMotionStates(); + const std::vector& getDetailedMotionStates() { return _detailedMotionStates; } void resetDetailedMotionStates(); friend AvatarManager; @@ -59,7 +59,6 @@ protected: std::vector _detailedMotionStates; int32_t _spaceIndex { -1 }; uint8_t _workloadRegion { workload::Region::INVALID }; - std::mutex _mStateLock; }; using OtherAvatarPointer = std::shared_ptr; From f19201fc92d15d199506f7de1cf0ad2ec739a21e Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Tue, 8 Jan 2019 14:09:13 -0700 Subject: [PATCH 03/19] Other avatar detailed collisions on motion state --- interface/src/avatar/AvatarManager.cpp | 13 ++-- interface/src/avatar/AvatarMotionState.cpp | 1 + interface/src/avatar/DetailedMotionState.cpp | 65 ++++--------------- interface/src/avatar/DetailedMotionState.h | 3 - interface/src/avatar/OtherAvatar.cpp | 27 +++++--- interface/src/avatar/OtherAvatar.h | 5 +- libraries/physics/src/ObjectMotionState.cpp | 18 +++-- libraries/shared/src/PhysicsCollisionGroups.h | 5 +- libraries/shared/src/PhysicsHelpers.cpp | 4 ++ 9 files changed, 62 insertions(+), 79 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index b80d53e6c3..44cb1300aa 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -431,12 +431,13 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact auto& detailedMotionStates = avatar->getDetailedMotionStates(); if (detailedMotionStates.size() == 0) { for (int i = 0; i < avatar->getJointCount(); i++) { - avatar->addNewMotionState(avatar, i); + auto dMotionState = avatar->createDetailedMotionStateForJoint(avatar, i); + if (dMotionState) { + detailedMotionStates.push_back(dMotionState); + transaction.objectsToAdd.push_back(dMotionState); + } } - for (auto& mState : detailedMotionStates) { - transaction.objectsToAdd.push_back(mState); - } - qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); + //qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } if (avatar->_motionState == nullptr || detailedMotionStates.size() == 0) { failedShapeBuilds.insert(avatar); @@ -450,7 +451,7 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact transaction.objectsToChange.push_back(mState); } } - qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); + //qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } } _avatarsToChangeInPhysics.swap(failedShapeBuilds); diff --git a/interface/src/avatar/AvatarMotionState.cpp b/interface/src/avatar/AvatarMotionState.cpp index ca67f634c8..27bf22b9d7 100644 --- a/interface/src/avatar/AvatarMotionState.cpp +++ b/interface/src/avatar/AvatarMotionState.cpp @@ -58,6 +58,7 @@ PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const { const btCollisionShape* AvatarMotionState::computeNewShape() { ShapeInfo shapeInfo; _avatar->computeShapeInfo(shapeInfo); + qDebug() << "Creating new Capsule Shape"; return getShapeManager()->getShape(shapeInfo); } diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index 43cd77c558..a54787b1c4 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -20,7 +20,6 @@ DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btColl ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { assert(_avatar); _type = MOTIONSTATE_TYPE_DETAILED; - cacheShapeDiameter(); } void DetailedMotionState::handleEasyChanges(uint32_t& flags) { @@ -37,6 +36,9 @@ bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngin DetailedMotionState::~DetailedMotionState() { assert(_avatar); _avatar = nullptr; + if (_shape) { + delete _shape; + } } // virtual @@ -57,9 +59,8 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const { // virtual and protected const btCollisionShape* DetailedMotionState::computeNewShape() { - ShapeInfo shapeInfo; - _avatar->computeShapeInfo(shapeInfo); - return getShapeManager()->getShape(shapeInfo); + auto shape = _avatar->createDetailedCollisionShapeForJoint(_jointIndex); + return shape; } // virtual @@ -71,37 +72,11 @@ bool DetailedMotionState::isMoving() const { void DetailedMotionState::getWorldTransform(btTransform& worldTrans) const { worldTrans.setOrigin(glmToBullet(getObjectPosition())); worldTrans.setRotation(glmToBullet(getObjectRotation())); - if (_body) { - _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity())); - _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); - } } // virtual void DetailedMotionState::setWorldTransform(const btTransform& worldTrans) { - const float SPRING_TIMESCALE = 0.5f; - float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE; - btVector3 currentPosition = worldTrans.getOrigin(); - btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition; - float distance = offsetToTarget.length(); - if ((1.0f - tau) * distance > _diameter) { - // the avatar body is far from its target --> slam position - btTransform newTransform; - newTransform.setOrigin(currentPosition + offsetToTarget); - newTransform.setRotation(glmToBullet(getObjectRotation())); - _body->setWorldTransform(newTransform); - _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity())); - _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); - } else { - // the avatar body is near its target --> slam velocity - btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget; - _body->setLinearVelocity(velocity); - _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity())); - // slam its rotation - btTransform newTransform = worldTrans; - newTransform.setRotation(glmToBullet(getObjectRotation())); - _body->setWorldTransform(newTransform); - } + _body->setWorldTransform(worldTrans); } // These pure virtual methods must be implemented for each MotionState type @@ -139,20 +114,17 @@ glm::quat DetailedMotionState::getObjectRotation() const { // virtual glm::vec3 DetailedMotionState::getObjectLinearVelocity() const { - return _avatar->getWorldVelocity(); + return glm::vec3(0.0f); } // virtual glm::vec3 DetailedMotionState::getObjectAngularVelocity() const { - // HACK: avatars use a capusle collision shape and their angularVelocity in the local simulation is unimportant. - // Therefore, as optimization toward support for larger crowds we ignore it and return zero. - //return _avatar->getWorldAngularVelocity(); return glm::vec3(0.0f); } // virtual glm::vec3 DetailedMotionState::getObjectGravity() const { - return _avatar->getAcceleration(); + return glm::vec3(0.0f); } // virtual @@ -161,7 +133,7 @@ const QUuid DetailedMotionState::getObjectID() const { } QString DetailedMotionState::getName() const { - return _avatar->getName(); + return _avatar->getName() + "_" + _jointIndex; } // virtual @@ -171,26 +143,13 @@ QUuid DetailedMotionState::getSimulatorID() const { // virtual void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const { - group = BULLET_COLLISION_GROUP_OTHER_AVATAR; + group = BULLET_COLLISION_GROUP_DETAILED_AVATAR; mask = Physics::getDefaultCollisionMask(group); } // virtual float DetailedMotionState::getMass() const { - return _avatar->computeMass(); -} - -void DetailedMotionState::cacheShapeDiameter() { - if (_shape) { - // shape is capsuleY - btVector3 aabbMin, aabbMax; - btTransform transform; - transform.setIdentity(); - _shape->getAabb(transform, aabbMin, aabbMax); - _diameter = (aabbMax - aabbMin).getX(); - } else { - _diameter = 0.0f; - } + return 0.0f; } void DetailedMotionState::setRigidBody(btRigidBody* body) { @@ -203,5 +162,5 @@ void DetailedMotionState::setRigidBody(btRigidBody* body) { void DetailedMotionState::setShape(const btCollisionShape* shape) { ObjectMotionState::setShape(shape); - cacheShapeDiameter(); } + diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h index 1c5f224e4a..4ae780de8c 100644 --- a/interface/src/avatar/DetailedMotionState.h +++ b/interface/src/avatar/DetailedMotionState.h @@ -62,8 +62,6 @@ public: virtual QString getName() const override; virtual QUuid getSimulatorID() const override; - void setBoundingBox(const glm::vec3& corner, const glm::vec3& diagonal); - void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; } virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override; @@ -76,7 +74,6 @@ public: protected: void setRigidBody(btRigidBody* body) override; void setShape(const btCollisionShape* shape) override; - void cacheShapeDiameter(); // the dtor had been made protected to force the compiler to verify that it is only // ever called by the Avatar class dtor. diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 7495e5e2df..00b89d0179 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -117,7 +117,7 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { return bytesRead; } -void OtherAvatar::addNewMotionState(std::shared_ptr avatar, int jointIndex) { +btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex) { if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { auto& data = _multiSphereShapes[jointIndex].getSpheresData(); std::vector positions; @@ -127,15 +127,19 @@ void OtherAvatar::addNewMotionState(std::shared_ptr avatar, int joi radiuses.push_back(sphere._radius); } btCollisionShape* shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size()); - if (shape) { - DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex); - motionState->setMass(computeMass()); - assert(_detailedMotionStates.size() == jointIndex); - _detailedMotionStates.push_back(motionState); - } else { - _detailedMotionStates.push_back(nullptr); - } + return shape; } + return nullptr; +} + +DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex) { + auto shape = createDetailedCollisionShapeForJoint(jointIndex); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex); + motionState->setMass(computeMass()); + return motionState; + } + return nullptr; } void OtherAvatar::resetDetailedMotionStates() { @@ -162,4 +166,9 @@ 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); + } + } } diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index 18b93b8aad..1e595eabdc 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -46,8 +46,9 @@ public: bool shouldBeInPhysicsSimulation() const; bool needsPhysicsUpdate() const; - void addNewMotionState(std::shared_ptr avatar, int jointIndex); - const std::vector& getDetailedMotionStates() { return _detailedMotionStates; } + btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex); + DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex); + std::vector& getDetailedMotionStates() { return _detailedMotionStates; } void resetDetailedMotionStates(); friend AvatarManager; diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 4bc0389620..db36c5815d 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -195,10 +195,14 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) { void ObjectMotionState::setShape(const btCollisionShape* shape) { if (_shape != shape) { if (_shape) { - getShapeManager()->releaseShape(_shape); + if (_type == MOTIONSTATE_TYPE_DETAILED) { + delete _shape; + } else { + getShapeManager()->releaseShape(_shape); + } } _shape = shape; - if (_body) { + if (_body && _type != MOTIONSTATE_TYPE_DETAILED) { updateCCDConfiguration(); } } @@ -292,7 +296,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* if (!isReadyToComputeShape()) { return false; } - const btCollisionShape* newShape = _type != MOTIONSTATE_TYPE_DETAILED ? computeNewShape() : nullptr; + const btCollisionShape* newShape = computeNewShape(); if (!newShape) { qCDebug(physics) << "Warning: failed to generate new shape!"; // failed to generate new shape! --> keep old shape and remove shape-change flag @@ -309,8 +313,12 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* if (_shape == newShape) { // 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); + if (_type == MOTIONSTATE_TYPE_DETAILED) { + delete _shape; + } else { + // and clear the reference we just created + getShapeManager()->releaseShape(_shape); + } } else { _body->setCollisionShape(const_cast(newShape)); setShape(newShape); 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; }; From 95fca826a59516e7cb045f370c27ec6fe5c88ecd Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Wed, 9 Jan 2019 17:49:19 -0700 Subject: [PATCH 04/19] Add fit AABox for avatar and implement multisphere on shapeManager --- interface/src/avatar/AvatarMotionState.cpp | 1 - interface/src/avatar/DetailedMotionState.cpp | 4 --- interface/src/avatar/MyAvatar.cpp | 10 ++++++ interface/src/avatar/MyAvatar.h | 2 ++ interface/src/avatar/OtherAvatar.cpp | 18 +++------- .../src/avatars-renderer/Avatar.cpp | 35 +++++++++++++++++-- .../src/avatars-renderer/Avatar.h | 7 +++- libraries/physics/src/MultiSphereShape.cpp | 13 +++++++ libraries/physics/src/MultiSphereShape.h | 3 ++ libraries/physics/src/ObjectMotionState.cpp | 14 ++------ libraries/physics/src/ShapeFactory.cpp | 11 ++++++ libraries/shared/src/ShapeInfo.cpp | 31 ++++++++++++++-- libraries/shared/src/ShapeInfo.h | 10 ++++-- 13 files changed, 120 insertions(+), 39 deletions(-) diff --git a/interface/src/avatar/AvatarMotionState.cpp b/interface/src/avatar/AvatarMotionState.cpp index bb51c85eb6..3fa59ea967 100644 --- a/interface/src/avatar/AvatarMotionState.cpp +++ b/interface/src/avatar/AvatarMotionState.cpp @@ -59,7 +59,6 @@ PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const { const btCollisionShape* AvatarMotionState::computeNewShape() { ShapeInfo shapeInfo; _avatar->computeShapeInfo(shapeInfo); - qDebug() << "Creating new Capsule Shape"; return getShapeManager()->getShape(shapeInfo); } diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index a54787b1c4..5d5592e9ab 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -36,9 +36,6 @@ bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngin DetailedMotionState::~DetailedMotionState() { assert(_avatar); _avatar = nullptr; - if (_shape) { - delete _shape; - } } // virtual @@ -163,4 +160,3 @@ void DetailedMotionState::setRigidBody(btRigidBody* body) { void DetailedMotionState::setShape(const btCollisionShape* shape) { ObjectMotionState::setShape(shape); } - diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e40fc7f9dd..394892735f 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -423,6 +423,16 @@ void MyAvatar::clearIKJointLimitHistory() { _skeletonModel->getRig().clearIKJointLimitHistory(); } +QVariantMap MyAvatar::getBoundingBox() { + QVariantMap bbox; + auto avatarBBox = getFitBounds(); + auto center = avatarBBox.calcCenter(); + auto dimensions = avatarBBox.getDimensions(); + bbox["center"] = vec3toVariant(center); + bbox["dimensions"] = vec3toVariant(dimensions); + return bbox; +} + void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) { assert(QThread::currentThread() == thread()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 17b71153ea..b70bcf7b30 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -311,6 +311,8 @@ public: */ Q_INVOKABLE void clearIKJointLimitHistory(); // thread-safe + Q_INVOKABLE QVariantMap getBoundingBox(); + void update(float deltaTime); virtual void postUpdate(float deltaTime, const render::ScenePointer& scene) override; void preDisplaySide(const RenderArgs* renderArgs); diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 29d3e69c2a..b365c79d8f 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -108,25 +108,15 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { int32_t bytesRead = Avatar::parseDataFromBuffer(buffer); if (_moving && _motionState) { _motionState->addDirtyFlags(Simulation::DIRTY_POSITION); - for (auto mState : _detailedMotionStates) { - if (mState) { - mState->addDirtyFlags(Simulation::DIRTY_POSITION); - } - } } return bytesRead; } btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex) { - if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { - auto& data = _multiSphereShapes[jointIndex].getSpheresData(); - std::vector positions; - std::vector radiuses; - for (auto& sphere : data) { - positions.push_back(glmToBullet(sphere._position)); - radiuses.push_back(sphere._radius); - } - btCollisionShape* shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size()); + ShapeInfo shapeInfo; + computeDetailedShapeInfo(shapeInfo, jointIndex); + if (shapeInfo.getType() != SHAPE_TYPE_NONE) { + btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); return shape; } return nullptr; diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index f68a3d7cf4..a8d5f37136 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -956,6 +956,7 @@ void Avatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { } fixupModelsInScene(scene); + updateFitBoundingBox(); } void Avatar::render(RenderArgs* renderArgs) { @@ -1670,7 +1671,7 @@ void Avatar::rigReset() { void Avatar::computeMultiSphereShapes() { const Rig& rig = getSkeletonModel()->getRig(); - auto scale = extractScale(rig.getGeometryToRigTransform()); + glm::vec3 scale = extractScale(rig.getGeometryToRigTransform()); const HFMModel& geometry = getSkeletonModel()->getHFMModel(); int jointCount = rig.getJointStateCount(); _multiSphereShapes.clear(); @@ -1688,13 +1689,28 @@ void Avatar::computeMultiSphereShapes() { } auto jointName = rig.nameOfJoint(i).toUpper(); MultiSphereShape multiSphereShape; - if (multiSphereShape.computeMultiSphereShape(jointName, btPoints, getSensorToWorldScale())) { + if (multiSphereShape.computeMultiSphereShape(jointName, btPoints)) { multiSphereShape.calculateDebugLines(); - } + multiSphereShape.setScale(getTargetScale()); +; } _multiSphereShapes.push_back(multiSphereShape); } } +void Avatar::updateFitBoundingBox() { + _fitBoundingBox = AABox(); + if (getJointCount() == _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) { @@ -1874,6 +1890,19 @@ void Avatar::computeShapeInfo(ShapeInfo& shapeInfo) { shapeInfo.setOffset(offset); } +void Avatar::computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex) { + if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { + auto& data = _multiSphereShapes[jointIndex].getSpheresData(); + std::vector positions; + std::vector radiuses; + 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 41e1090015..a63c9b835a 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h @@ -319,6 +319,7 @@ 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 @@ -397,6 +398,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); @@ -440,6 +442,8 @@ public: void accumulateGrabPositions(std::map& grabAccumulators); + const std::vector& getMultiSphereShapes() const { return _multiSphereShapes; } + signals: void targetScaleChanged(float targetScale); @@ -508,7 +512,7 @@ protected: QString _empty{}; virtual void maybeUpdateSessionDisplayNameFromTransport(const QString& sessionDisplayName) override { _sessionDisplayName = sessionDisplayName; } // don't use no-op setter! void computeMultiSphereShapes(); - const std::vector& getMultiSphereShapes() const { return _multiSphereShapes; } + void updateFitBoundingBox(); SkeletonModelPointer _skeletonModel; @@ -633,6 +637,7 @@ protected: const QVector& blendedMeshSizes, const render::ItemIDs& subItemIDs); std::vector _multiSphereShapes; + AABox _fitBoundingBox; AvatarGrabMap _avatarGrabs; }; diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index f0f56d7cf8..16094743a6 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -524,4 +524,17 @@ void MultiSphereShape::setScale(float scale) { } _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 index f8f251de42..d942d107b1 100644 --- a/libraries/physics/src/MultiSphereShape.h +++ b/libraries/physics/src/MultiSphereShape.h @@ -15,6 +15,7 @@ #include #include #include +#include #include "BulletUtil.h" @@ -78,6 +79,7 @@ public: 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); private: CollisionShapeExtractionMode getExtractionModeByName(const QString& name); @@ -97,6 +99,7 @@ private: 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 db36c5815d..0ab051fa96 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -195,11 +195,7 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) { void ObjectMotionState::setShape(const btCollisionShape* shape) { if (_shape != shape) { if (_shape) { - if (_type == MOTIONSTATE_TYPE_DETAILED) { - delete _shape; - } else { - getShapeManager()->releaseShape(_shape); - } + getShapeManager()->releaseShape(_shape); } _shape = shape; if (_body && _type != MOTIONSTATE_TYPE_DETAILED) { @@ -313,12 +309,8 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* if (_shape == newShape) { // the shape didn't actually change, so we clear the DIRTY_SHAPE flag flags &= ~Simulation::DIRTY_SHAPE; - if (_type == MOTIONSTATE_TYPE_DETAILED) { - delete _shape; - } else { - // and clear the reference we just created - getShapeManager()->releaseShape(_shape); - } + // and clear the reference we just created + getShapeManager()->releaseShape(_shape); } else { _body->setCollisionShape(const_cast(newShape)); setShape(newShape); diff --git a/libraries/physics/src/ShapeFactory.cpp b/libraries/physics/src/ShapeFactory.cpp index d7ba2f0661..e86b1da496 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(sphereData.first)); + radiuses.push_back(sphereData.second); + } + 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/shared/src/ShapeInfo.cpp b/libraries/shared/src/ShapeInfo.cpp index 3426a79782..775849ccdc 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(sphereData.first); + _hashKey.hashFloat(sphereData.second); + } + } 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..3bed7ef85e 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 = QPair; + 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); From 19701ef333fc389caa2eaa9f7f5634a1245b3b4e Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Fri, 11 Jan 2019 15:36:07 -0700 Subject: [PATCH 05/19] Fix scale and add rayTest --- interface/src/Application.cpp | 12 +- interface/src/avatar/AvatarManager.cpp | 130 +++++++----------- interface/src/avatar/AvatarManager.h | 22 +++ interface/src/avatar/DetailedMotionState.cpp | 22 ++- interface/src/avatar/DetailedMotionState.h | 11 +- interface/src/avatar/MyAvatar.cpp | 4 + interface/src/avatar/MyAvatar.h | 3 + .../src/avatar/MyCharacterController.cpp | 114 ++++++++++++++- interface/src/avatar/MyCharacterController.h | 25 ++++ interface/src/avatar/OtherAvatar.h | 4 +- .../src/avatars-renderer/Avatar.cpp | 6 +- libraries/avatars/src/AvatarData.cpp | 2 + libraries/avatars/src/AvatarData.h | 1 + libraries/physics/src/CharacterController.cpp | 7 +- libraries/physics/src/CharacterController.h | 3 + libraries/physics/src/MultiSphereShape.cpp | 1 + 16 files changed, 272 insertions(+), 95 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index d306c77cce..d2bb07501c 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -159,6 +159,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" @@ -2635,7 +2636,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 @@ -6123,7 +6131,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 b5e14fd593..b6d697fd92 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -415,7 +415,6 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact } qCDebug(animation) << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); avatar->resetDetailedMotionStates(); - } else { if (avatar->_motionState == nullptr) { ShapeInfo shapeInfo; @@ -549,7 +548,8 @@ void AvatarManager::deleteAllAvatars() { avatar->die(); if (avatar != _myAvatar) { auto otherAvatar = std::static_pointer_cast(avatar); - assert(!otherAvatar->_motionState && !otherAvatar->_motionState2); + assert(!otherAvatar->_motionState); + assert(otherAvatar->getDetailedMotionStates().size() == 0); } } } @@ -662,83 +662,21 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic 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. - - // TODO -- find a way to extract transformed avatar mesh data from the rendering engine. - - 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); - } + float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results + BoxFace face = BoxFace::UNKNOWN_FACE; + glm::vec3 surfaceNormal; + QVariantMap extraInfo; + MyCharacterController::RayAvatarResult physicsResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); + if (physicsResult._intersect) { + result.intersects = true; + result.avatarID = physicsResult._intersectWithAvatar; + result.distance = physicsResult._distance; + result.surfaceNormal = physicsResult._intersectionNormal; + result.jointIndex = physicsResult._intersectWithJoint; + result.intersection = physicsResult._intersectionPoint; + result.extraInfo = extraInfo; + result.face = face; } - - 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; - 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; - } - } - } - - if (result.intersects) { - result.intersection = ray.origin + ray.direction * result.distance; - } - return result; } @@ -836,6 +774,42 @@ ParabolaToAvatarIntersectionResult AvatarManager::findParabolaIntersectionVector return result; } +RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersection(const PickRay& ray, + const QScriptValue& jointIndexesToInclude, + const QScriptValue& jointIndexesToDiscard) { + QVector jointsToInclude; + QVector jointsToDiscard; + qVectorIntFromScriptValue(jointIndexesToInclude, jointsToInclude); + qVectorIntFromScriptValue(jointIndexesToDiscard, jointsToDiscard); + return findSelfRayIntersectionVector(ray, jointsToInclude, jointsToDiscard); +} + +RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersectionVector(const PickRay& ray, + const QVector& jointIndexesToInclude, + const QVector& jointIndexesToDiscard) { + RayToAvatarIntersectionResult result; + if (QThread::currentThread() != thread()) { + BLOCKING_INVOKE_METHOD(const_cast(this), "findSelfRayIntersectionVector", + Q_RETURN_ARG(RayToAvatarIntersectionResult, result), + Q_ARG(const PickRay&, ray), + Q_ARG(const QVector&, jointIndexesToInclude), + Q_ARG(const QVector&, jointIndexesToDiscard)); + return result; + } + glm::vec3 normDirection = glm::normalize(ray.direction); + auto jointCollisionResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(normDirection), 1.0f, jointIndexesToDiscard); + if (jointCollisionResult._intersectWithJoint > -1 && jointCollisionResult._distance > 0) { + result.intersects = true; + result.distance = jointCollisionResult._distance; + result.jointIndex = jointCollisionResult._intersectWithJoint; + result.avatarID = _myAvatar->getID(); + result.extraInfo = QVariantMap(); + result.intersection = jointCollisionResult._intersectionPoint; + result.surfaceNormal = jointCollisionResult._intersectionNormal; + } + return result; +} + // HACK float AvatarManager::getAvatarSortCoefficient(const QString& name) { if (name == "size") { diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 6717c301af..1db04b3d9e 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -165,6 +165,28 @@ public: const QVector& avatarsToInclude, const QVector& avatarsToDiscard); + /**jsdoc + * @function AvatarManager.findSelfRayIntersection + * @param {PickRay} ray + * @param {Uuid[]} [jointsToInclude=[]] + * @param {Uuid[]} [jointsToDiscard=[]] + * @returns {RayToAvatarIntersectionResult} + */ + Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersection(const PickRay& ray, + const QScriptValue& jointIndexesToInclude = QScriptValue(), + const QScriptValue& jointIndexesToDiscard = QScriptValue()); + + /**jsdoc + * @function AvatarManager.findSelfRayIntersectionVector + * @param {PickRay} ray + * @param {Uuid[]} jointsToInclude + * @param {Uuid[]} jointsToDiscard + * @returns {RayToAvatarIntersectionResult} + */ + Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersectionVector(const PickRay& ray, + const QVector& jointIndexesToInclude, + const QVector& jointIndexesToDiscard); + /**jsdoc * @function AvatarManager.getAvatarSortCoefficient * @param {string} name diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index 5d5592e9ab..8bf50b2cdc 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -2,8 +2,8 @@ // DetailedMotionState.cpp // interface/src/avatar/ // -// Created by Andrew Meadows 2015.05.14 -// Copyright 2015 High Fidelity, Inc. +// 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 @@ -14,9 +14,10 @@ #include #include #include +#include "MyAvatar.h" -DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : +DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { assert(_avatar); _type = MOTIONSTATE_TYPE_DETAILED; @@ -56,7 +57,14 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const { // virtual and protected const btCollisionShape* DetailedMotionState::computeNewShape() { - auto shape = _avatar->createDetailedCollisionShapeForJoint(_jointIndex); + btCollisionShape* shape = nullptr; + if (!_avatar->isMyAvatar()) { + OtherAvatarPointer otherAvatar = std::static_pointer_cast(_avatar); + shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex); + } else { + std::shared_ptr myAvatar = std::static_pointer_cast(_avatar); + shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex); + } return shape; } @@ -160,3 +168,9 @@ void DetailedMotionState::setRigidBody(btRigidBody* body) { void DetailedMotionState::setShape(const btCollisionShape* shape) { ObjectMotionState::setShape(shape); } + +void DetailedMotionState::forceActive() { + if (_body) { + _body->setActivationState(ACTIVE_TAG); + } +} \ No newline at end of file diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h index 4ae780de8c..a5bd777d07 100644 --- a/interface/src/avatar/DetailedMotionState.h +++ b/interface/src/avatar/DetailedMotionState.h @@ -2,8 +2,8 @@ // DetailedMotionState.h // interface/src/avatar/ // -// Created by Andrew Meadows 2015.05.14 -// Copyright 2015 High Fidelity, Inc. +// 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 @@ -21,7 +21,7 @@ class DetailedMotionState : public ObjectMotionState { public: - DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex); + DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex); virtual void handleEasyChanges(uint32_t& flags) override; virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override; @@ -67,6 +67,9 @@ public: virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override; virtual float getMass() const override; + void forceActive(); + QUuid getAvatarID() { return _avatar->getID(); } + int getJointIndex() { return _jointIndex; } friend class AvatarManager; friend class Avatar; @@ -82,7 +85,7 @@ protected: virtual bool isReadyToComputeShape() const override { return true; } virtual const btCollisionShape* computeNewShape() override; - OtherAvatarPointer _avatar; + AvatarPointer _avatar; float _diameter { 0.0f }; uint32_t _dirtyFlags; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 394892735f..3be23f2b56 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -4830,3 +4830,7 @@ void MyAvatar::releaseGrab(const QUuid& grabID) { _clientTraitsHandler->markInstancedTraitDeleted(AvatarTraits::Grab, grabID); } } + +std::shared_ptr MyAvatar::getSharedMe() { + return DependencyManager::get()->getMyAvatar(); +} diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index b70bcf7b30..01a7403c9c 100644 --- 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, @@ -1206,6 +1207,8 @@ public: */ Q_INVOKABLE void releaseGrab(const QUuid& grabID); + std::shared_ptr getSharedMe(); + public slots: /**jsdoc diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 798dbc91ed..ba019e60b6 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 @@ -44,8 +46,8 @@ void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) { void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; - if (_radius > 0.0f) { + // _pendingFlags |= PENDING_FLAG_RESET_DETAILED_SHAPES; // create RigidBody if it doesn't exist if (!_rigidBody) { btCollisionShape* shape = computeShape(); @@ -352,3 +354,113 @@ 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)); + return shape; + } + return nullptr; +} + +DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) { + auto shape = createDetailedCollisionShapeForJoint(jointIndex); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(_avatar->getSharedMe(), shape, jointIndex); + motionState->setMass(_avatar->computeMass()); + return motionState; + } + return nullptr; +} + +void MyCharacterController::resetDetailedMotionStates() { + for (size_t i = 0; i < _detailedMotionStates.size(); i++) { + _detailedMotionStates[i] = nullptr; + } + _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[i] = nullptr; + } + _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 ClosestDetailed : public btCollisionWorld::AllHitsRayResultCallback { +public: + ClosestDetailed() + : 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); + } +}; + +MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, + const QVector& jointsToExclude) const { + RayAvatarResult result; + if (_dynamicsWorld) { + btVector3 end = origin + length * direction; + ClosestDetailed rayCallback = ClosestDetailed(); + rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal; + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest; + _dynamicsWorld->rayTest(origin, end, rayCallback); + if (rayCallback.m_hitFractions.size() > 0) { + int minIndex = 0; + float hitFraction = rayCallback.m_hitFractions[0]; + for (auto i = 1; i < rayCallback.m_hitFractions.size(); i++) { + if (hitFraction > rayCallback.m_hitFractions[i]) { + hitFraction = rayCallback.m_hitFractions[i]; + minIndex = i; + } + } + auto object = rayCallback.m_collisionObjects[minIndex]; + ObjectMotionState* motionState = static_cast(object->getUserPointer()); + if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) { + DetailedMotionState* detailedMotionState = dynamic_cast(motionState); + if (detailedMotionState) { + result._intersect = true; + result._intersectWithAvatar = detailedMotionState->getAvatarID(); + result._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[minIndex]); + result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[minIndex]); + result._distance = length * hitFraction; + result._intersectWithJoint = detailedMotionState->getJointIndex(); + } + } + } + } + return result; +} \ No newline at end of file diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index fd9caface2..f5a510e5b5 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -15,9 +15,11 @@ #include //#include +#include class btCollisionShape; class MyAvatar; +class DetailedMotionState; class MyCharacterController : public CharacterController { public: @@ -42,6 +44,27 @@ public: void setDensity(btScalar density) { _density = density; } + btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); + DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex); + std::vector& getDetailedMotionStates() { return _detailedMotionStates; } + void clearDetailedMotionStates() { _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; } + void resetDetailedMotionStates(); + + void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction); + void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction); + + + struct RayAvatarResult { + bool _intersect { false }; + QUuid _intersectWithAvatar; + int _intersectWithJoint { -1 }; + float _distance { 0.0f }; + glm::vec3 _intersectionPoint; + glm::vec3 _intersectionNormal; + }; + RayAvatarResult rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, + const QVector& jointsToExclude) const; + protected: void initRayShotgun(const btCollisionWorld* world); void updateMassProperties() override; @@ -56,6 +79,8 @@ protected: btAlignedObjectArray _topPoints; btAlignedObjectArray _bottomPoints; btScalar _density { 1.0f }; + + std::vector _detailedMotionStates; }; #endif // hifi_MyCharacterController_h diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index cfe47d2f35..7b5f11c139 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -46,10 +46,11 @@ public: bool shouldBeInPhysicsSimulation() const; bool needsPhysicsUpdate() const; - btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex); + btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex); std::vector& getDetailedMotionStates() { return _detailedMotionStates; } void resetDetailedMotionStates(); + void updateCollisionGroup(bool myAvatarCollide); friend AvatarManager; @@ -64,5 +65,6 @@ protected: }; using OtherAvatarPointer = std::shared_ptr; +using AvatarPointer = std::shared_ptr; #endif // hifi_OtherAvatar_h diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index a8d5f37136..098249902b 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1671,7 +1671,7 @@ void Avatar::rigReset() { void Avatar::computeMultiSphereShapes() { const Rig& rig = getSkeletonModel()->getRig(); - glm::vec3 scale = extractScale(rig.getGeometryToRigTransform()); + glm::vec3 scale = extractScale(rig.getGeometryOffsetPose()); const HFMModel& geometry = getSkeletonModel()->getHFMModel(); int jointCount = rig.getJointStateCount(); _multiSphereShapes.clear(); @@ -1691,8 +1691,8 @@ void Avatar::computeMultiSphereShapes() { MultiSphereShape multiSphereShape; if (multiSphereShape.computeMultiSphereShape(jointName, btPoints)) { multiSphereShape.calculateDebugLines(); - multiSphereShape.setScale(getTargetScale()); -; } + multiSphereShape.setScale(_targetScale); + } _multiSphereShapes.push_back(multiSphereShape); } } diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index e72fa3a6eb..393948a467 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -2898,6 +2898,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; } @@ -2917,6 +2918,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 b42c387f61..5513abab7f 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -1614,6 +1614,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 8fd6d4eada..342a74b752 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -137,7 +137,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; } @@ -445,10 +446,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 50db2bea12..3566867f88 100644 --- 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_GROUP = 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 index 16094743a6..f361d67656 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -254,6 +254,7 @@ bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::v for (size_t i = 0; i < _spheres.size(); i++) { _spheres[i]._position += _midPoint; } + return _mode != CollisionShapeExtractionMode::None; } From 65896b3b6f6c9b35dcfb16feba1fbe6fb1d09933 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Sun, 13 Jan 2019 11:30:39 -0700 Subject: [PATCH 06/19] Fix shape LOD --- interface/src/avatar/AvatarManager.cpp | 23 +++--- interface/src/avatar/DetailedMotionState.cpp | 10 ++- interface/src/avatar/DetailedMotionState.h | 3 + interface/src/avatar/OtherAvatar.cpp | 73 +++++++++++++++++++- interface/src/avatar/OtherAvatar.h | 14 +++- 5 files changed, 103 insertions(+), 20 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index b6d697fd92..ffefcfa4a6 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -413,7 +413,7 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact transaction.objectsToRemove.push_back(mState); } } - qCDebug(animation) << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); + qDebug() << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); avatar->resetDetailedMotionStates(); } else { if (avatar->_motionState == nullptr) { @@ -427,20 +427,16 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact transaction.objectsToAdd.push_back(motionState); } } - auto& detailedMotionStates = avatar->getDetailedMotionStates(); - if (detailedMotionStates.size() == 0) { - for (int i = 0; i < avatar->getJointCount(); i++) { - auto dMotionState = avatar->createDetailedMotionStateForJoint(avatar, i); - if (dMotionState) { - detailedMotionStates.push_back(dMotionState); - transaction.objectsToAdd.push_back(dMotionState); - } + if (avatar->getDetailedMotionStates().size() == 0) { + avatar->createDetailedMotionStates(avatar); + for (auto dMotionState : avatar->getDetailedMotionStates()) { + transaction.objectsToAdd.push_back(dMotionState); + } + if (avatar->_motionState == nullptr || avatar->getDetailedMotionStates().size() == 0) { + failedShapeBuilds.insert(avatar); } - //qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); - } - if (avatar->_motionState == nullptr || detailedMotionStates.size() == 0) { - failedShapeBuilds.insert(avatar); } + qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " detailed motion states from " << avatar->getSessionUUID(); } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); @@ -450,7 +446,6 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact transaction.objectsToChange.push_back(mState); } } - //qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); } } _avatarsToChangeInPhysics.swap(failedShapeBuilds); diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index 8bf50b2cdc..6d983cdfa1 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -60,7 +60,13 @@ const btCollisionShape* DetailedMotionState::computeNewShape() { btCollisionShape* shape = nullptr; if (!_avatar->isMyAvatar()) { OtherAvatarPointer otherAvatar = std::static_pointer_cast(_avatar); - shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex); + if (otherAvatar) { + if (_isAvatarCapsule) { + shape = otherAvatar->createCapsuleCollisionShape(); + } else { + shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex); + } + } } else { std::shared_ptr myAvatar = std::static_pointer_cast(_avatar); shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex); @@ -109,7 +115,7 @@ float DetailedMotionState::getObjectAngularDamping() const { // virtual glm::vec3 DetailedMotionState::getObjectPosition() const { - return _avatar->getJointPosition(_jointIndex); + return _isAvatarCapsule ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex); } // virtual diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h index a5bd777d07..2671f9d75e 100644 --- a/interface/src/avatar/DetailedMotionState.h +++ b/interface/src/avatar/DetailedMotionState.h @@ -70,6 +70,7 @@ public: void forceActive(); QUuid getAvatarID() { return _avatar->getID(); } int getJointIndex() { return _jointIndex; } + void setIsAvatarCapsule(bool isAvatarCapsule) { _isAvatarCapsule = isAvatarCapsule; } friend class AvatarManager; friend class Avatar; @@ -90,6 +91,8 @@ protected: uint32_t _dirtyFlags; int _jointIndex { -1 }; + + bool _isAvatarCapsule { false }; }; #endif // hifi_DetailedMotionState_h diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index b365c79d8f..6fbdcf7c1f 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -122,6 +122,17 @@ btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointInd return nullptr; } +btCollisionShape* OtherAvatar::createCapsuleCollisionShape() { + ShapeInfo shapeInfo; + computeShapeInfo(shapeInfo); + shapeInfo.setOffset(glm::vec3(0.0f)); + if (shapeInfo.getType() != SHAPE_TYPE_NONE) { + btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + return shape; + } + return nullptr; +} + DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex) { auto shape = createDetailedCollisionShapeForJoint(jointIndex); if (shape) { @@ -132,6 +143,17 @@ DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ return nullptr; } +DetailedMotionState* OtherAvatar::createCapsuleMotionState(std::shared_ptr avatar) { + auto shape = createCapsuleCollisionShape(); + if (shape) { + DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, -1); + motionState->setIsAvatarCapsule(true); + motionState->setMass(computeMass()); + return motionState; + } + return nullptr; +} + void OtherAvatar::resetDetailedMotionStates() { for (size_t i = 0; i < _detailedMotionStates.size(); i++) { _detailedMotionStates[i] = nullptr; @@ -141,15 +163,42 @@ void OtherAvatar::resetDetailedMotionStates() { 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"; + } + qDebug() << "Setting workload region to " << printRegion; + computeShapeLOD(); +} + +void OtherAvatar::computeShapeLOD() { + auto newBodyLOD = (_workloadRegion < workload::Region::R3 && !isDead()) ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape; + if (newBodyLOD != _bodyLOD) { + _bodyLOD = newBodyLOD; + if (isInPhysicsSimulation()) { + qDebug() << "Changing to body LOD " << (_bodyLOD == BodyLOD::MultiSphereShapes ? "MultiSpheres" : "Capsule"); + _needsReinsertion = true; + } + } +} + +bool OtherAvatar::isInPhysicsSimulation() const { + return _motionState != nullptr && _detailedMotionStates.size() > 0; } bool OtherAvatar::shouldBeInPhysicsSimulation() const { - return (_workloadRegion < workload::Region::R3 && !isDead()); + return !(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() { @@ -175,4 +224,22 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) { _motionState->addDirtyFlags(Simulation::DIRTY_COLLISION_GROUP); } } -} \ No newline at end of file +} + +void OtherAvatar::createDetailedMotionStates(const std::shared_ptr& avatar){ + auto& detailedMotionStates = getDetailedMotionStates(); + if (_bodyLOD == BodyLOD::MultiSphereShapes) { + for (int i = 0; i < getJointCount(); i++) { + auto dMotionState = createDetailedMotionStateForJoint(avatar, i); + if (dMotionState) { + detailedMotionStates.push_back(dMotionState); + } + } + } else if (_bodyLOD == BodyLOD::CapsuleShape) { + auto dMotionState = createCapsuleMotionState(avatar); + if (dMotionState) { + detailedMotionStates.push_back(dMotionState); + } + } + _needsReinsertion = false; +} diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index 7b5f11c139..782812b2cb 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -27,6 +27,11 @@ public: explicit OtherAvatar(QThread* thread); virtual ~OtherAvatar(); + enum BodyLOD { + CapsuleShape, + MultiSphereShapes + }; + virtual void instantiableAvatar() override { }; virtual void createOrb() override; virtual void indicateLoadingStatus(LoadingStatus loadingStatus) override; @@ -39,7 +44,7 @@ public: int parseDataFromBuffer(const QByteArray& buffer) override; - bool isInPhysicsSimulation() const { return _motionState != nullptr && _detailedMotionStates.size() > 0; } + bool isInPhysicsSimulation() const; void rebuildCollisionShape() override; void setWorkloadRegion(uint8_t region); @@ -47,9 +52,14 @@ public: bool needsPhysicsUpdate() const; btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); + btCollisionShape* createCapsuleCollisionShape(); DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex); + DetailedMotionState* createCapsuleMotionState(std::shared_ptr avatar); + void createDetailedMotionStates(const std::shared_ptr& avatar); std::vector& getDetailedMotionStates() { return _detailedMotionStates; } void resetDetailedMotionStates(); + BodyLOD getBodyLOD() { return _bodyLOD; } + void computeShapeLOD(); void updateCollisionGroup(bool myAvatarCollide); @@ -62,6 +72,8 @@ protected: std::vector _detailedMotionStates; int32_t _spaceIndex { -1 }; uint8_t _workloadRegion { workload::Region::INVALID }; + BodyLOD _bodyLOD { BodyLOD::CapsuleShape }; + bool _needsReinsertion { false }; }; using OtherAvatarPointer = std::shared_ptr; From 71e7023a3e702789caee41b02fc8225714d6a57e Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Tue, 15 Jan 2019 18:07:50 -0700 Subject: [PATCH 07/19] Refactor and optimizations --- interface/src/Application.cpp | 2 +- interface/src/avatar/AvatarManager.cpp | 144 +++++++++++------- interface/src/avatar/AvatarManager.h | 22 --- interface/src/avatar/DetailedMotionState.cpp | 13 +- interface/src/avatar/DetailedMotionState.h | 12 +- interface/src/avatar/MyAvatar.cpp | 19 +-- .../src/avatar/MyCharacterController.cpp | 38 +++-- interface/src/avatar/MyCharacterController.h | 6 +- interface/src/avatar/OtherAvatar.cpp | 115 +++++++++----- interface/src/avatar/OtherAvatar.h | 13 +- .../src/avatars-renderer/Avatar.cpp | 2 +- .../src/avatars-renderer/Avatar.h | 1 + libraries/physics/src/MultiSphereShape.cpp | 6 +- libraries/physics/src/MultiSphereShape.h | 9 +- libraries/physics/src/PhysicsEngine.cpp | 3 + .../physics/src/ThreadSafeDynamicsWorld.cpp | 42 +++++ .../physics/src/ThreadSafeDynamicsWorld.h | 3 + libraries/shared/src/DebugDraw.cpp | 10 ++ libraries/shared/src/DebugDraw.h | 10 ++ 19 files changed, 293 insertions(+), 177 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index d2bb07501c..274b3131c9 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2639,7 +2639,7 @@ Application::~Application() { auto myCharacterController = getMyAvatar()->getCharacterController(); myCharacterController->clearDetailedMotionStates(); - + myCharacterController->buildPhysicsTransaction(transaction); _physicsEngine->processTransaction(transaction); myCharacterController->handleProcessedPhysicsTransaction(transaction); diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index ffefcfa4a6..80ffd32aff 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -407,6 +407,7 @@ 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) { if (mState) { @@ -415,37 +416,39 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact } qDebug() << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); avatar->resetDetailedMotionStates(); + } else { - if (avatar->_motionState == nullptr) { - 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); - } + 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); } + if (avatar->getDetailedMotionStates().size() == 0) { avatar->createDetailedMotionStates(avatar); for (auto dMotionState : avatar->getDetailedMotionStates()) { transaction.objectsToAdd.push_back(dMotionState); } - if (avatar->_motionState == nullptr || avatar->getDetailedMotionStates().size() == 0) { - failedShapeBuilds.insert(avatar); - } } + qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " detailed motion states from " << avatar->getSessionUUID(); } } 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); @@ -656,21 +659,80 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic Q_ARG(const QVector&, avatarsToDiscard)); return result; } + + glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY, + ray.direction.y != 0 ? 1.0f / ray.direction.y : INFINITY, + ray.direction.z != 0 ? 1.0f / ray.direction.z : INFINITY }; float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results BoxFace face = BoxFace::UNKNOWN_FACE; glm::vec3 surfaceNormal; QVariantMap extraInfo; - MyCharacterController::RayAvatarResult physicsResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); - if (physicsResult._intersect) { - result.intersects = true; - result.avatarID = physicsResult._intersectWithAvatar; - result.distance = physicsResult._distance; - result.surfaceNormal = physicsResult._intersectionNormal; - result.jointIndex = physicsResult._intersectWithJoint; - result.intersection = physicsResult._intersectionPoint; - result.extraInfo = extraInfo; - result.face = face; + std::vector physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); + + if (physicsResults.size() > 0) { + MyCharacterController::RayAvatarResult rayAvatarResult; + for (auto &hit : physicsResults) { + if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(hit._intersectWithAvatar)) || + (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(hit._intersectWithAvatar))) { + continue; + } + if (!hit._isBound) { + rayAvatarResult = hit; + break; + } else { + auto avatarMap = getHashCopy(); + auto avatarID = hit._intersectWithAvatar; + AvatarHash::iterator itr = avatarMap.find(avatarID); + if (itr != avatarMap.end()) { + const auto& avatar = std::static_pointer_cast(*itr); + auto &multiSpheres = avatar->getMultiSphereShapes(); + if (multiSpheres.size() > 0) { + std::vector boxHits; + for (auto 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, ray.direction, rayDirectionInv, boundDistance, face, surfaceNormal)) { + MyCharacterController::RayAvatarResult boxHit; + boxHit._distance = boundDistance; + boxHit._intersect = true; + boxHit._intersectionNormal = surfaceNormal; + boxHit._intersectionPoint = ray.origin + boundDistance * glm::normalize(ray.direction); + 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]; + 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 = rayAvatarResult._intersectionPoint; + result.extraInfo = extraInfo; + result.face = face; + } } return result; } @@ -769,42 +831,6 @@ ParabolaToAvatarIntersectionResult AvatarManager::findParabolaIntersectionVector return result; } -RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersection(const PickRay& ray, - const QScriptValue& jointIndexesToInclude, - const QScriptValue& jointIndexesToDiscard) { - QVector jointsToInclude; - QVector jointsToDiscard; - qVectorIntFromScriptValue(jointIndexesToInclude, jointsToInclude); - qVectorIntFromScriptValue(jointIndexesToDiscard, jointsToDiscard); - return findSelfRayIntersectionVector(ray, jointsToInclude, jointsToDiscard); -} - -RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersectionVector(const PickRay& ray, - const QVector& jointIndexesToInclude, - const QVector& jointIndexesToDiscard) { - RayToAvatarIntersectionResult result; - if (QThread::currentThread() != thread()) { - BLOCKING_INVOKE_METHOD(const_cast(this), "findSelfRayIntersectionVector", - Q_RETURN_ARG(RayToAvatarIntersectionResult, result), - Q_ARG(const PickRay&, ray), - Q_ARG(const QVector&, jointIndexesToInclude), - Q_ARG(const QVector&, jointIndexesToDiscard)); - return result; - } - glm::vec3 normDirection = glm::normalize(ray.direction); - auto jointCollisionResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(normDirection), 1.0f, jointIndexesToDiscard); - if (jointCollisionResult._intersectWithJoint > -1 && jointCollisionResult._distance > 0) { - result.intersects = true; - result.distance = jointCollisionResult._distance; - result.jointIndex = jointCollisionResult._intersectWithJoint; - result.avatarID = _myAvatar->getID(); - result.extraInfo = QVariantMap(); - result.intersection = jointCollisionResult._intersectionPoint; - result.surfaceNormal = jointCollisionResult._intersectionNormal; - } - return result; -} - // HACK float AvatarManager::getAvatarSortCoefficient(const QString& name) { if (name == "size") { diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 1db04b3d9e..6717c301af 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -165,28 +165,6 @@ public: const QVector& avatarsToInclude, const QVector& avatarsToDiscard); - /**jsdoc - * @function AvatarManager.findSelfRayIntersection - * @param {PickRay} ray - * @param {Uuid[]} [jointsToInclude=[]] - * @param {Uuid[]} [jointsToDiscard=[]] - * @returns {RayToAvatarIntersectionResult} - */ - Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersection(const PickRay& ray, - const QScriptValue& jointIndexesToInclude = QScriptValue(), - const QScriptValue& jointIndexesToDiscard = QScriptValue()); - - /**jsdoc - * @function AvatarManager.findSelfRayIntersectionVector - * @param {PickRay} ray - * @param {Uuid[]} jointsToInclude - * @param {Uuid[]} jointsToDiscard - * @returns {RayToAvatarIntersectionResult} - */ - Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersectionVector(const PickRay& ray, - const QVector& jointIndexesToInclude, - const QVector& jointIndexesToDiscard); - /**jsdoc * @function AvatarManager.getAvatarSortCoefficient * @param {string} name diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index 6d983cdfa1..c08272184f 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -20,6 +20,7 @@ DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { assert(_avatar); + _otherAvatar = std::static_pointer_cast(_avatar); _type = MOTIONSTATE_TYPE_DETAILED; } @@ -59,13 +60,8 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const { const btCollisionShape* DetailedMotionState::computeNewShape() { btCollisionShape* shape = nullptr; if (!_avatar->isMyAvatar()) { - OtherAvatarPointer otherAvatar = std::static_pointer_cast(_avatar); - if (otherAvatar) { - if (_isAvatarCapsule) { - shape = otherAvatar->createCapsuleCollisionShape(); - } else { - shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex); - } + if (_otherAvatar) { + shape = _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints); } } else { std::shared_ptr myAvatar = std::static_pointer_cast(_avatar); @@ -115,7 +111,8 @@ float DetailedMotionState::getObjectAngularDamping() const { // virtual glm::vec3 DetailedMotionState::getObjectPosition() const { - return _isAvatarCapsule ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex); + auto bodyLOD = _otherAvatar->getBodyLOD(); + return bodyLOD == OtherAvatar::BodyLOD::Sphere ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex); } // virtual diff --git a/interface/src/avatar/DetailedMotionState.h b/interface/src/avatar/DetailedMotionState.h index 2671f9d75e..a9b4b4bb64 100644 --- a/interface/src/avatar/DetailedMotionState.h +++ b/interface/src/avatar/DetailedMotionState.h @@ -68,9 +68,10 @@ public: virtual float getMass() const override; void forceActive(); - QUuid getAvatarID() { return _avatar->getID(); } - int getJointIndex() { return _jointIndex; } - void setIsAvatarCapsule(bool isAvatarCapsule) { _isAvatarCapsule = isAvatarCapsule; } + 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; @@ -91,8 +92,9 @@ protected: uint32_t _dirtyFlags; int _jointIndex { -1 }; - - bool _isAvatarCapsule { false }; + 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 3be23f2b56..4ae89ea2a1 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2606,15 +2606,16 @@ 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 == _multiSphereShapes.size()) { + int count = 0; + 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[count++ % NUM_DEBUG_COLORS], pose.trans(), pose.rot()); } } } diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ba019e60b6..a9c3c29051 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -429,9 +429,9 @@ public: } }; -MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, - const QVector& jointsToExclude) const { - RayAvatarResult result; +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; ClosestDetailed rayCallback = ClosestDetailed(); @@ -439,28 +439,26 @@ MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVe rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest; _dynamicsWorld->rayTest(origin, end, rayCallback); if (rayCallback.m_hitFractions.size() > 0) { - int minIndex = 0; - float hitFraction = rayCallback.m_hitFractions[0]; - for (auto i = 1; i < rayCallback.m_hitFractions.size(); i++) { - if (hitFraction > rayCallback.m_hitFractions[i]) { - hitFraction = rayCallback.m_hitFractions[i]; - minIndex = i; - } - } - auto object = rayCallback.m_collisionObjects[minIndex]; - ObjectMotionState* motionState = static_cast(object->getUserPointer()); - if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) { - DetailedMotionState* detailedMotionState = dynamic_cast(motionState); - if (detailedMotionState) { + 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[minIndex]); - result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[minIndex]); - result._distance = length * hitFraction; + 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); + foundAvatars.push_back(result); } } + std::sort(foundAvatars.begin(), foundAvatars.end(), [](const RayAvatarResult& resultA, const RayAvatarResult& resultB) { + return resultA._distance < resultB._distance; + }); } } - return result; + return foundAvatars; } \ No newline at end of file diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index f5a510e5b5..3c727d017c 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -56,14 +56,16 @@ public: struct RayAvatarResult { bool _intersect { false }; + bool _isBound { false }; QUuid _intersectWithAvatar; int _intersectWithJoint { -1 }; float _distance { 0.0f }; glm::vec3 _intersectionPoint; glm::vec3 _intersectionNormal; + std::vector _boundJoints; }; - RayAvatarResult rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, - const QVector& jointsToExclude) const; + std::vector rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length, + const QVector& jointsToExclude) const; protected: void initRayShotgun(const btCollisionWorld* world); diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 6fbdcf7c1f..c9f0d20d0b 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -112,43 +112,62 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { return bytesRead; } -btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex) { +btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector& boundJoints) { ShapeInfo shapeInfo; - computeDetailedShapeInfo(shapeInfo, jointIndex); + isBound = false; + auto jointName = jointIndex > -1 && jointIndex < _multiSphereShapes.size() ? _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) { - btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); - return shape; + return const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); } return nullptr; } -btCollisionShape* OtherAvatar::createCapsuleCollisionShape() { - ShapeInfo shapeInfo; - computeShapeInfo(shapeInfo); - shapeInfo.setOffset(glm::vec3(0.0f)); - if (shapeInfo.getType() != SHAPE_TYPE_NONE) { - btCollisionShape* shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); - return shape; - } - return nullptr; -} - -DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex) { - auto shape = createDetailedCollisionShapeForJoint(jointIndex); +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()); - return motionState; - } - return nullptr; -} - -DetailedMotionState* OtherAvatar::createCapsuleMotionState(std::shared_ptr avatar) { - auto shape = createCapsuleCollisionShape(); - if (shape) { - DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, -1); - motionState->setIsAvatarCapsule(true); - motionState->setMass(computeMass()); + motionState->setIsBound(isBound, boundJoints); return motionState; } return nullptr; @@ -178,11 +197,27 @@ void OtherAvatar::setWorkloadRegion(uint8_t region) { } void OtherAvatar::computeShapeLOD() { - auto newBodyLOD = (_workloadRegion < workload::Region::R3 && !isDead()) ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape; - if (newBodyLOD != _bodyLOD) { - _bodyLOD = newBodyLOD; + // 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()) { - qDebug() << "Changing to body LOD " << (_bodyLOD == BodyLOD::MultiSphereShapes ? "MultiSpheres" : "Capsule"); + qDebug() << "Changing to body LOD " << newLOD; _needsReinsertion = true; } } @@ -193,7 +228,7 @@ bool OtherAvatar::isInPhysicsSimulation() const { } bool OtherAvatar::shouldBeInPhysicsSimulation() const { - return !(isInPhysicsSimulation() && _needsReinsertion); + return !isDead() && !(isInPhysicsSimulation() && _needsReinsertion); } bool OtherAvatar::needsPhysicsUpdate() const { @@ -228,18 +263,18 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) { void OtherAvatar::createDetailedMotionStates(const std::shared_ptr& avatar){ auto& detailedMotionStates = getDetailedMotionStates(); - if (_bodyLOD == BodyLOD::MultiSphereShapes) { + 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 = createDetailedMotionStateForJoint(avatar, i); + auto dMotionState = createMotionState(avatar, i); if (dMotionState) { detailedMotionStates.push_back(dMotionState); } } - } else if (_bodyLOD == BodyLOD::CapsuleShape) { - auto dMotionState = createCapsuleMotionState(avatar); - if (dMotionState) { - detailedMotionStates.push_back(dMotionState); - } } _needsReinsertion = false; } diff --git a/interface/src/avatar/OtherAvatar.h b/interface/src/avatar/OtherAvatar.h index 782812b2cb..7b1b214c14 100644 --- a/interface/src/avatar/OtherAvatar.h +++ b/interface/src/avatar/OtherAvatar.h @@ -28,8 +28,9 @@ public: virtual ~OtherAvatar(); enum BodyLOD { - CapsuleShape, - MultiSphereShapes + Sphere = 0, + MultiSphereLow, // No finger joints + MultiSphereHigh // All joints }; virtual void instantiableAvatar() override { }; @@ -51,10 +52,8 @@ public: bool shouldBeInPhysicsSimulation() const; bool needsPhysicsUpdate() const; - btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); - btCollisionShape* createCapsuleCollisionShape(); - DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr avatar, int jointIndex); - DetailedMotionState* createCapsuleMotionState(std::shared_ptr avatar); + 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(); @@ -72,7 +71,7 @@ protected: std::vector _detailedMotionStates; int32_t _spaceIndex { -1 }; uint8_t _workloadRegion { workload::Region::INVALID }; - BodyLOD _bodyLOD { BodyLOD::CapsuleShape }; + BodyLOD _bodyLOD { BodyLOD::Sphere }; bool _needsReinsertion { false }; }; diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 098249902b..75f91babac 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1689,7 +1689,7 @@ void Avatar::computeMultiSphereShapes() { } auto jointName = rig.nameOfJoint(i).toUpper(); MultiSphereShape multiSphereShape; - if (multiSphereShape.computeMultiSphereShape(jointName, btPoints)) { + if (multiSphereShape.computeMultiSphereShape(i, jointName, btPoints)) { multiSphereShape.calculateDebugLines(); multiSphereShape.setScale(_targetScale); } diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h index a63c9b835a..2f14766245 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h @@ -320,6 +320,7 @@ public: 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 diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index f361d67656..30fd4b25ea 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -126,9 +126,11 @@ void MultiSphereShape::filterUniquePoints(const std::vector& kdop, st } } -bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::vector& kdop, float scale) { +bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& name, const std::vector& kdop, float scale) { _scale = scale; - _mode = getExtractionModeByName(name); + _jointIndex = jointIndex; + _name = name; + _mode = getExtractionModeByName(_name); if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) { return false; } diff --git a/libraries/physics/src/MultiSphereShape.h b/libraries/physics/src/MultiSphereShape.h index d942d107b1..e5f19ba91a 100644 --- a/libraries/physics/src/MultiSphereShape.h +++ b/libraries/physics/src/MultiSphereShape.h @@ -74,12 +74,16 @@ const std::vector CORNER_SIGNS = { class MultiSphereShape { public: MultiSphereShape() {}; - bool computeMultiSphereShape(const QString& name, const std::vector& points, float scale = 1.0f); + 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); @@ -94,6 +98,9 @@ private: 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; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index bf6e2463e5..cb2823cdf1 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -328,6 +328,9 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) { _activeStaticBodies.insert(object->getRigidBody()); } + if (object->getType() == MOTIONSTATE_TYPE_AVATAR) { + _dynamicsWorld->updateSingleAabb(object->_body); + } } // 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/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index 17a52f7cd9..816cab788a 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -172,3 +172,45 @@ void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) { } } } + +void ThreadSafeDynamicsWorld::drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, const btVector3& position2, const btVector3& color) { + int stepDegrees = 30; + 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(); + for (int i = 0; i < 360; i += stepDegrees) { + float x1 = btSin(btScalar(i)*SIMD_RADS_PER_DEG) * radius1; + float z1 = btCos(btScalar(i)*SIMD_RADS_PER_DEG) * radius1; + float x2 = btSin(btScalar(i)*SIMD_RADS_PER_DEG) * radius2; + float z2 = btCos(btScalar(i)*SIMD_RADS_PER_DEG) * 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. From 67cc5bd7f2e9d71446261ed7fd30060d4ca80f58 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Wed, 16 Jan 2019 10:52:34 -0700 Subject: [PATCH 08/19] Fix warnings and wrong shapes position for myAvatar --- interface/src/avatar/AvatarManager.cpp | 2 +- interface/src/avatar/DetailedMotionState.cpp | 19 ++++++++++++++----- interface/src/avatar/MyAvatar.cpp | 13 +------------ interface/src/avatar/MyAvatar.h | 2 -- interface/src/avatar/OtherAvatar.cpp | 7 +++++-- .../src/avatars-renderer/Avatar.cpp | 6 +++--- libraries/physics/src/PhysicsEngine.cpp | 5 +---- 7 files changed, 25 insertions(+), 29 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 80ffd32aff..53f8a0bc7f 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -689,7 +689,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic auto &multiSpheres = avatar->getMultiSphereShapes(); if (multiSpheres.size() > 0) { std::vector boxHits; - for (auto i = 0; i < hit._boundJoints.size(); i++) { + 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()) { diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index c08272184f..4d8e3d5ec7 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -20,7 +20,9 @@ DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) { assert(_avatar); - _otherAvatar = std::static_pointer_cast(_avatar); + if (!_avatar->isMyAvatar()) { + _otherAvatar = std::static_pointer_cast(_avatar); + } _type = MOTIONSTATE_TYPE_DETAILED; } @@ -60,12 +62,14 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const { const btCollisionShape* DetailedMotionState::computeNewShape() { btCollisionShape* shape = nullptr; if (!_avatar->isMyAvatar()) { - if (_otherAvatar) { + if (_otherAvatar != nullptr) { shape = _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints); } } else { std::shared_ptr myAvatar = std::static_pointer_cast(_avatar); - shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex); + if (myAvatar) { + shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex); + } } return shape; } @@ -111,8 +115,13 @@ float DetailedMotionState::getObjectAngularDamping() const { // virtual glm::vec3 DetailedMotionState::getObjectPosition() const { - auto bodyLOD = _otherAvatar->getBodyLOD(); - return bodyLOD == OtherAvatar::BodyLOD::Sphere ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex); + if (_otherAvatar != nullptr) { + auto bodyLOD = _otherAvatar->getBodyLOD(); + if (bodyLOD == OtherAvatar::BodyLOD::Sphere) { + return _avatar->getFitBounds().calcCenter(); + } + } + return _avatar->getJointPosition(_jointIndex); } // virtual diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 621fe1d479..670444baf0 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -426,16 +426,6 @@ void MyAvatar::clearIKJointLimitHistory() { _skeletonModel->getRig().clearIKJointLimitHistory(); } -QVariantMap MyAvatar::getBoundingBox() { - QVariantMap bbox; - auto avatarBBox = getFitBounds(); - auto center = avatarBBox.calcCenter(); - auto dimensions = avatarBBox.getDimensions(); - bbox["center"] = vec3toVariant(center); - bbox["dimensions"] = vec3toVariant(dimensions); - return bbox; -} - void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) { assert(QThread::currentThread() == thread()); @@ -3073,9 +3063,8 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { if (_skeletonModel && _skeletonModel->isLoaded()) { const Rig& rig = _skeletonModel->getRig(); - const HFMModel& hfmModel = _skeletonModel->getHFMModel(); int jointCount = rig.getJointStateCount(); - if (jointCount == _multiSphereShapes.size()) { + if (jointCount == (int)_multiSphereShapes.size()) { int count = 0; for (int i = 0; i < jointCount; i++) { AnimPose jointPose; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0cb5d04c02..f107a488a6 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -312,8 +312,6 @@ public: */ Q_INVOKABLE void clearIKJointLimitHistory(); // thread-safe - Q_INVOKABLE QVariantMap getBoundingBox(); - void update(float deltaTime); virtual void postUpdate(float deltaTime, const render::ScenePointer& scene) override; void preDisplaySide(const RenderArgs* renderArgs); diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 67e577ad6b..53fc826fb0 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -123,7 +123,10 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) { btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector& boundJoints) { ShapeInfo shapeInfo; isBound = false; - auto jointName = jointIndex > -1 && jointIndex < _multiSphereShapes.size() ? _multiSphereShapes[jointIndex].getJointName() : ""; + 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); @@ -241,7 +244,7 @@ bool OtherAvatar::shouldBeInPhysicsSimulation() const { bool OtherAvatar::needsPhysicsUpdate() const { constexpr uint32_t FLAGS_OF_INTEREST = Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS | Simulation::DIRTY_POSITION | Simulation::DIRTY_COLLISION_GROUP; - return (_needsReinsertion || _motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST)); + return (_needsReinsertion || (_motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST))); } void OtherAvatar::rebuildCollisionShape() { diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 09cd7d5176..f2252b2aa3 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1440,7 +1440,7 @@ void Avatar::computeMultiSphereShapes() { std::vector btPoints; int lineCount = (int)shapeInfo.debugLines.size(); btPoints.reserve(lineCount); - for (size_t j = 0; j < lineCount; j++) { + for (int j = 0; j < lineCount; j++) { const glm::vec3 &point = shapeInfo.debugLines[j]; auto rigPoint = scale * point; btVector3 btPoint = glmToBullet(rigPoint); @@ -1458,7 +1458,7 @@ void Avatar::computeMultiSphereShapes() { void Avatar::updateFitBoundingBox() { _fitBoundingBox = AABox(); - if (getJointCount() == _multiSphereShapes.size()) { + if (getJointCount() == (int)_multiSphereShapes.size()) { for (int i = 0; i < getJointCount(); i++) { auto &shape = _multiSphereShapes[i]; glm::vec3 jointPosition; @@ -1650,7 +1650,7 @@ void Avatar::computeShapeInfo(ShapeInfo& shapeInfo) { } void Avatar::computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex) { - if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) { + if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) { auto& data = _multiSphereShapes[jointIndex].getSpheresData(); std::vector positions; std::vector radiuses; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index cb2823cdf1..a571a81109 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -327,10 +327,7 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) } if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) { _activeStaticBodies.insert(object->getRigidBody()); - } - if (object->getType() == MOTIONSTATE_TYPE_AVATAR) { - _dynamicsWorld->updateSingleAabb(object->_body); - } + } } // 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) From 2158e084d43821613ec1025674ea55c63d74b905 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Thu, 17 Jan 2019 14:00:54 -0700 Subject: [PATCH 09/19] Add avatar joints filter to intersection calculation --- interface/src/avatar/AvatarManager.cpp | 121 +++++++++++------- interface/src/avatar/AvatarManager.h | 8 +- interface/src/avatar/DetailedMotionState.cpp | 2 +- .../src/avatar/MyCharacterController.cpp | 3 + interface/src/avatar/OtherAvatar.cpp | 9 +- interface/src/raypick/RayPick.cpp | 2 +- 6 files changed, 94 insertions(+), 51 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 53f8a0bc7f..f24e1e72eb 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -640,23 +640,27 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, - const QScriptValue& avatarIdsToDiscard) { + const QScriptValue& avatarIdsToDiscard, + const QScriptValue& jointIndicesToFilter) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); - - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard); + QVector jointsToFilter; + qVectorIntFromScriptValue(jointIndicesToFilter, jointsToFilter); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointsToFilter); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, - const QVector& avatarsToDiscard) { + const QVector& avatarsToDiscard, + const QVector& jointIndicesToFilter) { 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(const QVector&, jointIndicesToFilter)); return result; } @@ -672,55 +676,80 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic if (physicsResults.size() > 0) { MyCharacterController::RayAvatarResult rayAvatarResult; + AvatarPointer avatar = nullptr; for (auto &hit : physicsResults) { - if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(hit._intersectWithAvatar)) || - (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(hit._intersectWithAvatar))) { + auto avatarID = hit._intersectWithAvatar; + bool skipThisAvatar = (avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || + (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID)) && jointIndicesToFilter.size() == 0; + if (skipThisAvatar) { continue; } - if (!hit._isBound) { - rayAvatarResult = hit; - break; - } else { + if (!(_myAvatar->getSessionUUID() == avatarID)) { auto avatarMap = getHashCopy(); - auto avatarID = hit._intersectWithAvatar; AvatarHash::iterator itr = avatarMap.find(avatarID); if (itr != avatarMap.end()) { - const auto& avatar = std::static_pointer_cast(*itr); - 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, ray.direction, rayDirectionInv, boundDistance, face, surfaceNormal)) { - MyCharacterController::RayAvatarResult boxHit; - boxHit._distance = boundDistance; - boxHit._intersect = true; - boxHit._intersectionNormal = surfaceNormal; - boxHit._intersectionPoint = ray.origin + boundDistance * glm::normalize(ray.direction); - 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]; - break; + avatar = std::static_pointer_cast(*itr); + } + } else { + avatar = _myAvatar; + } + QVector jointsToDiscard; + if (avatar && jointIndicesToFilter.size() > 0) { + int jointCount = avatar->getJointCount(); + if (avatarsToInclude.size() > 0 && avatarsToInclude.contains(avatarID)) { + for (int i = 0; i < jointCount; i++) { + if (!jointIndicesToFilter.contains(i)) { + jointsToDiscard.push_back(i); } } - } + } else if (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID)) { + for (int i = 0; i < jointCount; i++) { + if (jointIndicesToFilter.contains(i)) { + jointsToDiscard.push_back(i); + } + } + } + } + if (!hit._isBound) { + if (!jointsToDiscard.contains(hit._intersectWithJoint)) { + rayAvatarResult = hit; + break; + } + } 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() && !jointsToDiscard.contains(hit._boundJoints[i])) { + float boundDistance = FLT_MAX; + BoxFace face; + glm::vec3 surfaceNormal; + auto &bbox = mSphere.getBoundingBox(); + if (bbox.findRayIntersection(ray.origin, ray.direction, rayDirectionInv, boundDistance, face, surfaceNormal)) { + MyCharacterController::RayAvatarResult boxHit; + boxHit._distance = boundDistance; + boxHit._intersect = true; + boxHit._intersectionNormal = surfaceNormal; + boxHit._intersectionPoint = ray.origin + boundDistance * glm::normalize(ray.direction); + boxHit._intersectWithAvatar = avatarID; + boxHit._intersectWithJoint = hit._intersectWithJoint; + 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]; + break; + } + } } } if (rayAvatarResult._intersect) { diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 6717c301af..93be716087 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -138,21 +138,25 @@ public: * @param {PickRay} ray * @param {Uuid[]} [avatarsToInclude=[]] * @param {Uuid[]} [avatarsToDiscard=[]] + * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), - const QScriptValue& avatarIdsToDiscard = QScriptValue()); + const QScriptValue& avatarIdsToDiscard = QScriptValue(), + const QScriptValue& jointIndicesToFilter = QScriptValue()); /**jsdoc * @function AvatarManager.findRayIntersectionVector * @param {PickRay} ray * @param {Uuid[]} avatarsToInclude * @param {Uuid[]} avatarsToDiscard + * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, - const QVector& avatarsToDiscard); + const QVector& avatarsToDiscard, + const QVector& jointIndicesToFilter); /**jsdoc * @function AvatarManager.findParabolaIntersectionVector diff --git a/interface/src/avatar/DetailedMotionState.cpp b/interface/src/avatar/DetailedMotionState.cpp index 4d8e3d5ec7..cec27108ca 100644 --- a/interface/src/avatar/DetailedMotionState.cpp +++ b/interface/src/avatar/DetailedMotionState.cpp @@ -182,7 +182,7 @@ void DetailedMotionState::setShape(const btCollisionShape* shape) { } void DetailedMotionState::forceActive() { - if (_body) { + if (_body && !_body->isActive()) { _body->setActivationState(ACTIVE_TAG); } } \ No newline at end of file diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 0141154f10..f1eb59d808 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -360,6 +360,9 @@ btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(in _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; diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 53fc826fb0..6447d8d8b5 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -114,6 +114,9 @@ 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); } @@ -166,7 +169,11 @@ btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBoun break; } if (shapeInfo.getType() != SHAPE_TYPE_NONE) { - return const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + auto shape = const_cast(ObjectMotionState::getShapeManager()->getShape(shapeInfo)); + if (shape) { + shape->setMargin(0.001f); + } + return shape; } return nullptr; } diff --git a/interface/src/raypick/RayPick.cpp b/interface/src/raypick/RayPick.cpp index 507e45b470..85ffc97450 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(), QVector()); if (avatarRes.intersects) { return std::make_shared(IntersectionType::AVATAR, avatarRes.avatarID, avatarRes.distance, avatarRes.intersection, pick, avatarRes.surfaceNormal, avatarRes.extraInfo); } else { From 007e3ac577ee174e1a2e9459af51c190ad64bb72 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Thu, 17 Jan 2019 14:53:06 -0700 Subject: [PATCH 10/19] Fix warning and wrong ID --- interface/src/avatar/AvatarManager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index f24e1e72eb..548b3d44a5 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -679,8 +679,8 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic AvatarPointer avatar = nullptr; for (auto &hit : physicsResults) { auto avatarID = hit._intersectWithAvatar; - bool skipThisAvatar = (avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || - (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID)) && jointIndicesToFilter.size() == 0; + bool skipThisAvatar = ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || + (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) && jointIndicesToFilter.size() == 0; if (skipThisAvatar) { continue; } @@ -734,7 +734,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic boxHit._intersectionNormal = surfaceNormal; boxHit._intersectionPoint = ray.origin + boundDistance * glm::normalize(ray.direction); boxHit._intersectWithAvatar = avatarID; - boxHit._intersectWithJoint = hit._intersectWithJoint; + boxHit._intersectWithJoint = mSphere.getJointIndex(); boxHits.push_back(boxHit); } } From 43244193e889e539b2a65eaab4bd9b274142769f Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Fri, 18 Jan 2019 12:46:18 -0700 Subject: [PATCH 11/19] Mesh picking against default pose with transformed ray --- interface/src/avatar/AvatarManager.cpp | 42 +++++++++-- interface/src/avatar/AvatarManager.h | 6 +- interface/src/avatar/MyAvatar.cpp | 71 ------------------ interface/src/avatar/MyAvatar.h | 50 ------------- interface/src/raypick/RayPick.cpp | 2 +- .../src/avatars-renderer/Avatar.cpp | 73 +++++++++++++++++++ .../src/avatars-renderer/Avatar.h | 51 +++++++++++++ 7 files changed, 165 insertions(+), 130 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 548b3d44a5..2c6f51cd4a 100644 --- 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 @@ -641,18 +642,20 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, const QScriptValue& avatarIdsToDiscard, - const QScriptValue& jointIndicesToFilter) { + const QScriptValue& jointIndicesToFilter, + bool pickAgainstMesh) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); QVector jointsToFilter; qVectorIntFromScriptValue(jointIndicesToFilter, jointsToFilter); - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointsToFilter); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointsToFilter, pickAgainstMesh); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QVector& jointIndicesToFilter) { + const QVector& jointIndicesToFilter, + bool pickAgainstMesh) { RayToAvatarIntersectionResult result; if (QThread::currentThread() != thread()) { BLOCKING_INVOKE_METHOD(const_cast(this), "findRayIntersectionVector", @@ -660,7 +663,8 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic Q_ARG(const PickRay&, ray), Q_ARG(const QVector&, avatarsToInclude), Q_ARG(const QVector&, avatarsToDiscard), - Q_ARG(const QVector&, jointIndicesToFilter)); + Q_ARG(const QVector&, jointIndicesToFilter), + Q_ARG(bool, pickAgainstMesh)); return result; } @@ -673,7 +677,9 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic glm::vec3 surfaceNormal; QVariantMap extraInfo; std::vector physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); - + glm::vec3 transformedRayPoint; + glm::vec3 transformedRayDirection; + if (physicsResults.size() > 0) { MyCharacterController::RayAvatarResult rayAvatarResult; AvatarPointer avatar = nullptr; @@ -752,13 +758,37 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } } } + if (rayAvatarResult._intersect) { + if (pickAgainstMesh) { + glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint); + glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + ray.direction, 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 * glm::normalize(ray.direction); + rayAvatarResult._intersectionNormal = surfaceNormal; + extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint); + } + } + result.intersects = true; result.avatarID = rayAvatarResult._intersectWithAvatar; result.distance = rayAvatarResult._distance; result.surfaceNormal = rayAvatarResult._intersectionNormal; result.jointIndex = rayAvatarResult._intersectWithJoint; - result.intersection = rayAvatarResult._intersectionPoint; + result.intersection = ray.origin + rayAvatarResult._distance * glm::normalize(ray.direction); result.extraInfo = extraInfo; result.face = face; } diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 93be716087..5bc31d74b3 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -144,7 +144,8 @@ public: Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), const QScriptValue& avatarIdsToDiscard = QScriptValue(), - const QScriptValue& jointIndicesToFilter = QScriptValue()); + const QScriptValue& jointIndicesToFilter = QScriptValue(), + bool pickAgainstMesh = false); /**jsdoc * @function AvatarManager.findRayIntersectionVector * @param {PickRay} ray @@ -156,7 +157,8 @@ public: Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QVector& jointIndicesToFilter); + const QVector& jointIndicesToFilter, + bool pickAgainstMesh); /**jsdoc * @function AvatarManager.findParabolaIntersectionVector diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 670444baf0..641b2dff00 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1122,77 +1122,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 diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f107a488a6..5bc9c14f1d 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -784,56 +784,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/raypick/RayPick.cpp b/interface/src/raypick/RayPick.cpp index 85ffc97450..7d6c0b6eef 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(), QVector()); + RayToAvatarIntersectionResult avatarRes = DependencyManager::get()->findRayIntersectionVector(pick, getIncludeItemsAs(), getIgnoreItemsAs(), QVector(), false); 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 f2252b2aa3..a34c17550f 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1283,6 +1283,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; diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h index db2908fc5d..c0c61992b1 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h @@ -223,12 +223,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; From a947c894c2379b9f8c1fe8abf0fdf741d39392af Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Wed, 23 Jan 2019 17:23:36 -0700 Subject: [PATCH 12/19] Suggested changes --- interface/src/avatar/AvatarManager.cpp | 59 ++++++++----------- interface/src/avatar/AvatarManager.h | 10 ++-- .../src/avatar/MyCharacterController.cpp | 11 +++- interface/src/avatar/MyCharacterController.h | 4 +- interface/src/avatar/OtherAvatar.cpp | 5 +- interface/src/raypick/RayPick.cpp | 2 +- libraries/physics/src/MultiSphereShape.cpp | 29 ++++----- libraries/physics/src/ShapeFactory.cpp | 4 +- libraries/shared/src/ShapeInfo.cpp | 4 +- libraries/shared/src/ShapeInfo.h | 2 +- 10 files changed, 68 insertions(+), 62 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 2c6f51cd4a..99ac922bc6 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -411,11 +411,8 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact auto& detailedMotionStates = avatar->getDetailedMotionStates(); for (auto& mState : detailedMotionStates) { - if (mState) { - transaction.objectsToRemove.push_back(mState); - } + transaction.objectsToRemove.push_back(mState); } - qDebug() << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID(); avatar->resetDetailedMotionStates(); } else { @@ -430,15 +427,12 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact } else { failedShapeBuilds.insert(avatar); } - if (avatar->getDetailedMotionStates().size() == 0) { avatar->createDetailedMotionStates(avatar); for (auto dMotionState : avatar->getDetailedMotionStates()) { transaction.objectsToAdd.push_back(dMotionState); } } - - qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " detailed motion states from " << avatar->getSessionUUID(); } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); @@ -642,19 +636,17 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, const QScriptValue& avatarIdsToDiscard, - const QScriptValue& jointIndicesToFilter, + const QStringList& jointIndicesToFilter, bool pickAgainstMesh) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); - QVector jointsToFilter; - qVectorIntFromScriptValue(jointIndicesToFilter, jointsToFilter); - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointsToFilter, pickAgainstMesh); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointIndicesToFilter, pickAgainstMesh); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QVector& jointIndicesToFilter, + const QStringList& jointIndicesToFilter, bool pickAgainstMesh) { RayToAvatarIntersectionResult result; if (QThread::currentThread() != thread()) { @@ -663,14 +655,10 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic Q_ARG(const PickRay&, ray), Q_ARG(const QVector&, avatarsToInclude), Q_ARG(const QVector&, avatarsToDiscard), - Q_ARG(const QVector&, jointIndicesToFilter), + Q_ARG(const QStringList&, jointIndicesToFilter), Q_ARG(bool, pickAgainstMesh)); return result; } - - glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY, - ray.direction.y != 0 ? 1.0f / ray.direction.y : INFINITY, - ray.direction.z != 0 ? 1.0f / ray.direction.z : INFINITY }; float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results BoxFace face = BoxFace::UNKNOWN_FACE; @@ -681,13 +669,18 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic glm::vec3 transformedRayDirection; if (physicsResults.size() > 0) { + glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY, + ray.direction.y != 0.0f ? 1.0f / ray.direction.y : INFINITY, + ray.direction.z != 0.0f ? 1.0f / ray.direction.z : INFINITY }; + MyCharacterController::RayAvatarResult rayAvatarResult; AvatarPointer avatar = nullptr; for (auto &hit : physicsResults) { auto avatarID = hit._intersectWithAvatar; - bool skipThisAvatar = ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || - (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) && jointIndicesToFilter.size() == 0; - if (skipThisAvatar) { + bool avatarIsIncluded = avatarsToInclude.contains(avatarID); + bool avatarIsDiscarded = avatarsToDiscard.contains(avatarID); + if (jointIndicesToFilter.size() == 0 && ((avatarsToInclude.size() > 0 && !avatarIsIncluded) || + (avatarsToDiscard.size() > 0 && avatarIsDiscarded))) { continue; } if (!(_myAvatar->getSessionUUID() == avatarID)) { @@ -701,16 +694,16 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } QVector jointsToDiscard; if (avatar && jointIndicesToFilter.size() > 0) { - int jointCount = avatar->getJointCount(); - if (avatarsToInclude.size() > 0 && avatarsToInclude.contains(avatarID)) { - for (int i = 0; i < jointCount; i++) { - if (!jointIndicesToFilter.contains(i)) { + auto names = avatar->getJointNames(); + if (avatarIsIncluded) { + for (int i = 0; i < names.size(); i++) { + if (!jointIndicesToFilter.contains(names[i])) { jointsToDiscard.push_back(i); } } - } else if (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID)) { - for (int i = 0; i < jointCount; i++) { - if (jointIndicesToFilter.contains(i)) { + } else if (avatarIsDiscarded) { + for (int i = 0; i < names.size(); i++) { + if (jointIndicesToFilter.contains(names[i])) { jointsToDiscard.push_back(i); } } @@ -719,7 +712,6 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic if (!hit._isBound) { if (!jointsToDiscard.contains(hit._intersectWithJoint)) { rayAvatarResult = hit; - break; } } else if (avatar) { auto &multiSpheres = avatar->getMultiSphereShapes(); @@ -753,13 +745,9 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic }); } rayAvatarResult = boxHits[0]; - break; } } } - } - - if (rayAvatarResult._intersect) { if (pickAgainstMesh) { glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint); glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + ray.direction, rayAvatarResult._intersectWithJoint); @@ -769,7 +757,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic 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; @@ -780,9 +768,14 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic rayAvatarResult._intersectionPoint = ray.origin + newDistance * glm::normalize(ray.direction); rayAvatarResult._intersectionNormal = surfaceNormal; extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint); + break; } + } else { + break; } + } + if (rayAvatarResult._intersect) { result.intersects = true; result.avatarID = rayAvatarResult._intersectWithAvatar; result.distance = rayAvatarResult._distance; diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 5bc31d74b3..4c3b06d1af 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -138,26 +138,26 @@ public: * @param {PickRay} ray * @param {Uuid[]} [avatarsToInclude=[]] * @param {Uuid[]} [avatarsToDiscard=[]] - * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] + * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), const QScriptValue& avatarIdsToDiscard = QScriptValue(), - const QScriptValue& jointIndicesToFilter = QScriptValue(), - bool pickAgainstMesh = false); + const QStringList& jointIndicesToFilter = QStringList(), + bool pickAgainstMesh = true); /**jsdoc * @function AvatarManager.findRayIntersectionVector * @param {PickRay} ray * @param {Uuid[]} avatarsToInclude * @param {Uuid[]} avatarsToDiscard - * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] + * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QVector& jointIndicesToFilter, + const QStringList& jointIndicesToFilter, bool pickAgainstMesh); /**jsdoc diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index f1eb59d808..7ff3a1e0ee 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -47,7 +47,6 @@ void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; if (_radius > 0.0f) { - // _pendingFlags |= PENDING_FLAG_RESET_DETAILED_SHAPES; // create RigidBody if it doesn't exist if (!_rigidBody) { btCollisionShape* shape = computeShape(); @@ -378,6 +377,12 @@ DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(in 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() { for (size_t i = 0; i < _detailedMotionStates.size(); i++) { _detailedMotionStates[i] = nullptr; @@ -455,6 +460,10 @@ std::vector MyCharacterController::rayTe 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); } } diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 3c727d017c..d3a811cf8f 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -47,7 +47,7 @@ public: btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex); DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex); std::vector& getDetailedMotionStates() { return _detailedMotionStates; } - void clearDetailedMotionStates() { _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; } + void clearDetailedMotionStates(); void resetDetailedMotionStates(); void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction); @@ -60,6 +60,8 @@ public: QUuid _intersectWithAvatar; int _intersectWithJoint { -1 }; float _distance { 0.0f }; + float _maxDistance { 0.0f }; + QVariantMap _extraInfo; glm::vec3 _intersectionPoint; glm::vec3 _intersectionNormal; std::vector _boundJoints; diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index 6447d8d8b5..a0c0aa7ae1 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -210,7 +210,7 @@ void OtherAvatar::setWorkloadRegion(uint8_t region) { } else { printRegion = "invalid"; } - qDebug() << "Setting workload region to " << printRegion; + qCDebug(avatars) << "Setting workload region to " << printRegion; computeShapeLOD(); } @@ -235,7 +235,7 @@ void OtherAvatar::computeShapeLOD() { if (newLOD != _bodyLOD) { _bodyLOD = newLOD; if (isInPhysicsSimulation()) { - qDebug() << "Changing to body LOD " << newLOD; + qCDebug(avatars) << "Changing to body LOD " << newLOD; _needsReinsertion = true; } } @@ -280,6 +280,7 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) { } void OtherAvatar::createDetailedMotionStates(const std::shared_ptr& avatar) { + assert(detailedMotionStates.empty()); auto& detailedMotionStates = getDetailedMotionStates(); if (_bodyLOD == BodyLOD::Sphere) { auto dMotionState = createMotionState(avatar, -1); diff --git a/interface/src/raypick/RayPick.cpp b/interface/src/raypick/RayPick.cpp index 7d6c0b6eef..b9028b3a60 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(), QVector(), false); + RayToAvatarIntersectionResult avatarRes = DependencyManager::get()->findRayIntersectionVector(pick, getIncludeItemsAs(), getIgnoreItemsAs(), QStringList(), 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/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index 30fd4b25ea..639c8c8e90 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -25,7 +25,7 @@ void SphereRegion::dump(std::vector>& outLines) 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 (pointFromSet == point); + return (glm::length(pointFromSet-point) < FLT_EPSILON); }); if (hit == pointSet.end()) { pointSet.push_back(point); @@ -107,7 +107,6 @@ CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QSt } else if (isSim || isFlow || isEye || isToe) { mode = CollisionShapeExtractionMode::None; - //qDebug() << "Trying to add " << (int)positions.size() << " spheres for " << jointName << " length: " << maxLength; } return mode; } @@ -116,9 +115,9 @@ void MultiSphereShape::filterUniquePoints(const std::vector& kdop, st 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 (btPoint.getX() == point.x - && btPoint.getY() == point.y - && btPoint.getZ() == point.z); + 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)); @@ -287,9 +286,11 @@ void MultiSphereShape::spheresFromAxes(const std::vector& points, con maxRadius = radius > maxRadius ? radius : maxRadius; } } - averageRadius /= (int)points.size(); - maxAverageRadius = averageRadius > maxAverageRadius ? averageRadius : maxAverageRadius; - minAverageRadius = averageRadius < minAverageRadius ? averageRadius : minAverageRadius; + if (points.size() > 0) { + averageRadius /= (int)points.size(); + } + maxAverageRadius = glm::max(averageRadius, maxAverageRadius); + minAverageRadius = glm::min(averageRadius, minAverageRadius); spheres[j]._radius = averageRadius; } float radiusRatio = maxRadius / maxAverageRadius; @@ -331,17 +332,17 @@ void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) { auto axis = sphere1._position - sphere2._position; float angleOffset = glm::asin((sphere1._radius - sphere2._radius) / distance); - float percent1 = ((0.5f * PI) + angleOffset) / PI; - float percent2 = ((0.5f * PI) - angleOffset) / PI; + 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), percent1, &edge1); - calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2); + 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), percent1, &edge1); - calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2); + 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); } diff --git a/libraries/physics/src/ShapeFactory.cpp b/libraries/physics/src/ShapeFactory.cpp index e86b1da496..5808a539d6 100644 --- a/libraries/physics/src/ShapeFactory.cpp +++ b/libraries/physics/src/ShapeFactory.cpp @@ -289,8 +289,8 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info) std::vector radiuses; auto sphereCollection = info.getSphereCollection(); for (auto &sphereData : sphereCollection) { - positions.push_back(glmToBullet(sphereData.first)); - radiuses.push_back(sphereData.second); + positions.push_back(glmToBullet(glm::vec3(sphereData))); + radiuses.push_back(sphereData.w); } shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size()); } diff --git a/libraries/shared/src/ShapeInfo.cpp b/libraries/shared/src/ShapeInfo.cpp index 775849ccdc..564d79bfda 100644 --- a/libraries/shared/src/ShapeInfo.cpp +++ b/libraries/shared/src/ShapeInfo.cpp @@ -276,8 +276,8 @@ const HashKey& ShapeInfo::getHash() const { _hashKey.hashUint64((uint64_t)_type); if (_type == SHAPE_TYPE_MULTISPHERE) { for (auto &sphereData : _sphereCollection) { - _hashKey.hashVec3(sphereData.first); - _hashKey.hashFloat(sphereData.second); + _hashKey.hashVec3(glm::vec3(sphereData)); + _hashKey.hashFloat(sphereData.w); } } else if (_type != SHAPE_TYPE_SIMPLE_HULL) { _hashKey.hashVec3(_halfExtents); diff --git a/libraries/shared/src/ShapeInfo.h b/libraries/shared/src/ShapeInfo.h index 3bed7ef85e..d838d7b214 100644 --- a/libraries/shared/src/ShapeInfo.h +++ b/libraries/shared/src/ShapeInfo.h @@ -58,7 +58,7 @@ public: using PointList = QVector; using PointCollection = QVector; using TriangleIndices = QVector; - using SphereData = QPair; + using SphereData = glm::vec4; using SphereCollection = QVector; static QString getNameForShapeType(ShapeType type); From 7a0f3ea7ef70dc84cebe8481493243e73652c922 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Wed, 23 Jan 2019 18:04:13 -0700 Subject: [PATCH 13/19] Fix error --- interface/src/avatar/OtherAvatar.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/OtherAvatar.cpp b/interface/src/avatar/OtherAvatar.cpp index a0c0aa7ae1..99407d29a4 100644 --- a/interface/src/avatar/OtherAvatar.cpp +++ b/interface/src/avatar/OtherAvatar.cpp @@ -280,8 +280,8 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) { } void OtherAvatar::createDetailedMotionStates(const std::shared_ptr& avatar) { - assert(detailedMotionStates.empty()); auto& detailedMotionStates = getDetailedMotionStates(); + assert(detailedMotionStates.empty()); if (_bodyLOD == BodyLOD::Sphere) { auto dMotionState = createMotionState(avatar, -1); if (dMotionState) { From df4be641eb6d56746c8158f452732cae4d7cf08e Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Fri, 25 Jan 2019 13:42:18 -0700 Subject: [PATCH 14/19] More fixes --- interface/src/avatar/AvatarManager.cpp | 54 +++++++++---------- interface/src/avatar/AvatarManager.h | 8 +-- interface/src/avatar/MyAvatar.cpp | 8 +-- interface/src/avatar/MyAvatar.h | 2 - .../src/avatar/MyCharacterController.cpp | 15 +++--- interface/src/avatar/MyCharacterController.h | 4 +- .../src/avatars-renderer/Avatar.cpp | 2 + libraries/physics/src/MultiSphereShape.cpp | 24 +++------ 8 files changed, 48 insertions(+), 69 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 99ac922bc6..6cb585e814 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -636,17 +636,17 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, const QScriptValue& avatarIdsToDiscard, - const QStringList& jointIndicesToFilter, + const QStringList& jointNamesToFilter, bool pickAgainstMesh) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointIndicesToFilter, pickAgainstMesh); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointNamesToFilter, pickAgainstMesh); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QStringList& jointIndicesToFilter, + const QStringList& jointNamesToFilter, bool pickAgainstMesh) { RayToAvatarIntersectionResult result; if (QThread::currentThread() != thread()) { @@ -655,35 +655,37 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic Q_ARG(const PickRay&, ray), Q_ARG(const QVector&, avatarsToInclude), Q_ARG(const QVector&, avatarsToDiscard), - Q_ARG(const QStringList&, jointIndicesToFilter), + Q_ARG(const QStringList&, jointNamesToFilter), Q_ARG(bool, pickAgainstMesh)); return result; } + PROFILE_RANGE(simulation_physics, __FUNCTION__); float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results - BoxFace face = BoxFace::UNKNOWN_FACE; - glm::vec3 surfaceNormal; - QVariantMap extraInfo; + std::vector physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); - glm::vec3 transformedRayPoint; - glm::vec3 transformedRayDirection; if (physicsResults.size() > 0) { glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY, - ray.direction.y != 0.0f ? 1.0f / ray.direction.y : INFINITY, - ray.direction.z != 0.0f ? 1.0f / ray.direction.z : INFINITY }; + ray.direction.y != 0.0f ? 1.0f / ray.direction.y : INFINITY, + ray.direction.z != 0.0f ? 1.0f / ray.direction.z : INFINITY }; MyCharacterController::RayAvatarResult rayAvatarResult; AvatarPointer avatar = nullptr; + + BoxFace face = BoxFace::UNKNOWN_FACE; + glm::vec3 surfaceNormal; + QVariantMap extraInfo; + for (auto &hit : physicsResults) { auto avatarID = hit._intersectWithAvatar; bool avatarIsIncluded = avatarsToInclude.contains(avatarID); bool avatarIsDiscarded = avatarsToDiscard.contains(avatarID); - if (jointIndicesToFilter.size() == 0 && ((avatarsToInclude.size() > 0 && !avatarIsIncluded) || - (avatarsToDiscard.size() > 0 && avatarIsDiscarded))) { + if (avatarIsDiscarded || (jointNamesToFilter.size() == 0 && avatarsToInclude.size() > 0 && !avatarIsIncluded)) { continue; } - if (!(_myAvatar->getSessionUUID() == avatarID)) { + + if (_myAvatar->getSessionUUID() != avatarID) { auto avatarMap = getHashCopy(); AvatarHash::iterator itr = avatarMap.find(avatarID); if (itr != avatarMap.end()) { @@ -692,25 +694,17 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } else { avatar = _myAvatar; } - QVector jointsToDiscard; - if (avatar && jointIndicesToFilter.size() > 0) { + QVector jointIndicesToDiscard; + if (avatar && jointNamesToFilter.size() > 0 && avatarIsIncluded) { auto names = avatar->getJointNames(); - if (avatarIsIncluded) { - for (int i = 0; i < names.size(); i++) { - if (!jointIndicesToFilter.contains(names[i])) { - jointsToDiscard.push_back(i); - } - } - } else if (avatarIsDiscarded) { - for (int i = 0; i < names.size(); i++) { - if (jointIndicesToFilter.contains(names[i])) { - jointsToDiscard.push_back(i); - } + for (int i = 0; i < names.size(); i++) { + if (!jointNamesToFilter.contains(names[i])) { + jointIndicesToDiscard.push_back(i); } } } if (!hit._isBound) { - if (!jointsToDiscard.contains(hit._intersectWithJoint)) { + if (!jointIndicesToDiscard.contains(hit._intersectWithJoint)) { rayAvatarResult = hit; } } else if (avatar) { @@ -720,7 +714,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic 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() && !jointsToDiscard.contains(hit._boundJoints[i])) { + if (mSphere.isValid() && jointIndicesToDiscard.contains(hit._boundJoints[i])) { float boundDistance = FLT_MAX; BoxFace face; glm::vec3 surfaceNormal; @@ -770,7 +764,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint); break; } - } else { + } else if (rayAvatarResult._intersect){ break; } } diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 4c3b06d1af..447acee248 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -138,26 +138,26 @@ public: * @param {PickRay} ray * @param {Uuid[]} [avatarsToInclude=[]] * @param {Uuid[]} [avatarsToDiscard=[]] - * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] + * @param {string[]} [jointNamesToFilter=[] - If included avatars are provided only this joints corresponding to those avatars would be included ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), const QScriptValue& avatarIdsToDiscard = QScriptValue(), - const QStringList& jointIndicesToFilter = QStringList(), + const QStringList& jointNamesToFilter = QStringList(), bool pickAgainstMesh = true); /**jsdoc * @function AvatarManager.findRayIntersectionVector * @param {PickRay} ray * @param {Uuid[]} avatarsToInclude * @param {Uuid[]} avatarsToDiscard - * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ] + * @param {string[]} [jointNamesToFilter=[] - If included avatars are provided only this joints corresponding to those avatars would be included ] * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QStringList& jointIndicesToFilter, + const QStringList& jointNamesToFilter, bool pickAgainstMesh); /**jsdoc diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 641b2dff00..abafe7dec9 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -103,7 +103,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), @@ -2994,14 +2994,13 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { const Rig& rig = _skeletonModel->getRig(); int jointCount = rig.getJointStateCount(); if (jointCount == (int)_multiSphereShapes.size()) { - int count = 0; 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[count++ % NUM_DEBUG_COLORS], pose.trans(), pose.rot()); + DebugDraw::getInstance().drawRays(debugLines, DEBUG_COLORS[i % NUM_DEBUG_COLORS], pose.trans(), pose.rot()); } } } @@ -5218,6 +5217,3 @@ void MyAvatar::releaseGrab(const QUuid& grabID) { } } -std::shared_ptr MyAvatar::getMyAvatarSharedPointer() { - return DependencyManager::get()->getMyAvatar(); -} diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 5bc9c14f1d..2b6b67ad5e 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1158,8 +1158,6 @@ public: */ Q_INVOKABLE void releaseGrab(const QUuid& grabID); - std::shared_ptr getMyAvatarSharedPointer(); - AvatarEntityMap getAvatarEntityData() const override; void setAvatarEntityData(const AvatarEntityMap& avatarEntityData) override; void updateAvatarEntity(const QUuid& entityID, const QByteArray& entityData) override; diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 7ff3a1e0ee..813bcb44ab 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -26,7 +26,7 @@ void MyCharacterController::RayShotgunResult::reset() { walkable = true; } -MyCharacterController::MyCharacterController(MyAvatar* avatar) { +MyCharacterController::MyCharacterController(std::shared_ptr avatar) { assert(avatar); _avatar = avatar; @@ -370,7 +370,7 @@ btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(in DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) { auto shape = createDetailedCollisionShapeForJoint(jointIndex); if (shape) { - DetailedMotionState* motionState = new DetailedMotionState(_avatar->getMyAvatarSharedPointer(), shape, jointIndex); + DetailedMotionState* motionState = new DetailedMotionState(_avatar, shape, jointIndex); motionState->setMass(_avatar->computeMass()); return motionState; } @@ -384,9 +384,6 @@ void MyCharacterController::clearDetailedMotionStates() { } void MyCharacterController::resetDetailedMotionStates() { - for (size_t i = 0; i < _detailedMotionStates.size(); i++) { - _detailedMotionStates[i] = nullptr; - } _detailedMotionStates.clear(); } @@ -398,7 +395,6 @@ void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction& _pendingFlags &= ~PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; for (size_t i = 0; i < _detailedMotionStates.size(); i++) { transaction.objectsToRemove.push_back(_detailedMotionStates[i]); - _detailedMotionStates[i] = nullptr; } _detailedMotionStates.clear(); } @@ -423,9 +419,9 @@ void MyCharacterController::handleProcessedPhysicsTransaction(PhysicsEngine::Tra } -class ClosestDetailed : public btCollisionWorld::AllHitsRayResultCallback { +class DetailedRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback { public: - ClosestDetailed() + 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; @@ -442,11 +438,12 @@ std::vector MyCharacterController::rayTe std::vector foundAvatars; if (_dynamicsWorld) { btVector3 end = origin + length * direction; - ClosestDetailed rayCallback = ClosestDetailed(); + 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()); diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index d3a811cf8f..0bc781c2fa 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/interface/src/avatar/MyCharacterController.h @@ -23,7 +23,7 @@ class DetailedMotionState; class MyCharacterController : public CharacterController { public: - explicit MyCharacterController(MyAvatar* avatar); + explicit MyCharacterController(std::shared_ptr avatar); ~MyCharacterController (); void setDynamicsWorld(btDynamicsWorld* world) override; @@ -77,7 +77,7 @@ private: btConvexHullShape* computeShape() const; protected: - MyAvatar* _avatar { nullptr }; + std::shared_ptr _avatar { nullptr }; // shotgun scan data btAlignedObjectArray _topPoints; diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index a34c17550f..bc708bff2d 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1727,6 +1727,8 @@ void Avatar::computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex) { 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); diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index 639c8c8e90..656fb64e39 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -48,12 +48,10 @@ void SphereRegion::extractEdges(bool reverseY) { if (vec.z == 0.0f) { insertUnique(p1, _edgesX); insertUnique(p2, _edgesX); - } - else if (vec.y == 0.0f && p1.y == yVal && p2.y == yVal) { + } else if (vec.y == 0.0f && p1.y == yVal && p2.y == yVal) { insertUnique(p1, _edgesY); insertUnique(p2, _edgesY); - } - else if (vec.x == 0.0f) { + } else if (vec.x == 0.0f) { insertUnique(p1, _edgesZ); insertUnique(p2, _edgesZ); } @@ -104,8 +102,7 @@ CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QSt mode = CollisionShapeExtractionMode::SphereCollapse; } else if (isRightHand || isLeftHand) { mode = CollisionShapeExtractionMode::SpheresXY; - } - else if (isSim || isFlow || isEye || isToe) { + } else if (isSim || isFlow || isEye || isToe) { mode = CollisionShapeExtractionMode::None; } return mode; @@ -174,17 +171,13 @@ bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& na if (xyDif < 0.5f * xyEpsilon && xzDif < 0.5f * xzEpsilon && yzDif < 0.5f * yzEpsilon) { applyMode = CollisionShapeExtractionMode::Sphere; - } - else if (xzDif < xzEpsilon) { + } else if (xzDif < xzEpsilon) { applyMode = dimensions.y > dimensions.z ? CollisionShapeExtractionMode::SpheresY : CollisionShapeExtractionMode::SpheresXZ; - } - else if (xyDif < xyEpsilon) { + } else if (xyDif < xyEpsilon) { applyMode = dimensions.z > dimensions.y ? CollisionShapeExtractionMode::SpheresZ : CollisionShapeExtractionMode::SpheresXY; - } - else if (yzDif < yzEpsilon) { + } else if (yzDif < yzEpsilon) { applyMode = dimensions.x > dimensions.y ? CollisionShapeExtractionMode::SpheresX : CollisionShapeExtractionMode::SpheresYZ; - } - else { + } else { applyMode = CollisionShapeExtractionMode::SpheresXYZ; } @@ -473,8 +466,7 @@ void MultiSphereShape::calculateSphereLines(std::vector Date: Mon, 28 Jan 2019 11:55:53 -0700 Subject: [PATCH 15/19] Fix error on exit and jointsToFilter param removed --- interface/src/avatar/AvatarManager.cpp | 57 +++++++++++--------------- interface/src/avatar/AvatarManager.h | 2 - interface/src/raypick/RayPick.cpp | 2 +- 3 files changed, 25 insertions(+), 36 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 2fdc927dff..a625faf71c 100755 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -397,31 +397,37 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact if (isInPhysics) { transaction.objectsToRemove.push_back(avatar->_motionState); avatar->_motionState = nullptr; - auto& detailedMotionStates = avatar->getDetailedMotionStates(); + qDebug() << "Deleting " << detailedMotionStates.size() << "Motion states"; 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); - } else { - failedShapeBuilds.insert(avatar); - } 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); + } + + qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " Motion states"; } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); @@ -625,17 +631,15 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID) RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude, const QScriptValue& avatarIdsToDiscard, - const QStringList& jointNamesToFilter, bool pickAgainstMesh) { QVector avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude); QVector avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard); - return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointNamesToFilter, pickAgainstMesh); + return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, pickAgainstMesh); } RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QStringList& jointNamesToFilter, bool pickAgainstMesh) { RayToAvatarIntersectionResult result; if (QThread::currentThread() != thread()) { @@ -644,10 +648,10 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic Q_ARG(const PickRay&, ray), Q_ARG(const QVector&, avatarsToInclude), Q_ARG(const QVector&, avatarsToDiscard), - Q_ARG(const QStringList&, jointNamesToFilter), Q_ARG(bool, pickAgainstMesh)); return result; } + PROFILE_RANGE(simulation_physics, __FUNCTION__); float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results @@ -668,9 +672,8 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic for (auto &hit : physicsResults) { auto avatarID = hit._intersectWithAvatar; - bool avatarIsIncluded = avatarsToInclude.contains(avatarID); - bool avatarIsDiscarded = avatarsToDiscard.contains(avatarID); - if (avatarIsDiscarded || (jointNamesToFilter.size() == 0 && avatarsToInclude.size() > 0 && !avatarIsIncluded)) { + if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) || + (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) { continue; } @@ -683,19 +686,8 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } else { avatar = _myAvatar; } - QVector jointIndicesToDiscard; - if (avatar && jointNamesToFilter.size() > 0 && avatarIsIncluded) { - auto names = avatar->getJointNames(); - for (int i = 0; i < names.size(); i++) { - if (!jointNamesToFilter.contains(names[i])) { - jointIndicesToDiscard.push_back(i); - } - } - } if (!hit._isBound) { - if (!jointIndicesToDiscard.contains(hit._intersectWithJoint)) { - rayAvatarResult = hit; - } + rayAvatarResult = hit; } else if (avatar) { auto &multiSpheres = avatar->getMultiSphereShapes(); if (multiSpheres.size() > 0) { @@ -703,7 +695,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic 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() && jointIndicesToDiscard.contains(hit._boundJoints[i])) { + if (mSphere.isValid()) { float boundDistance = FLT_MAX; BoxFace face; glm::vec3 surfaceNormal; @@ -757,7 +749,6 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic break; } } - if (rayAvatarResult._intersect) { result.intersects = true; result.avatarID = rayAvatarResult._intersectWithAvatar; diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index 3c0fa0282c..dcec3d1945 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -143,7 +143,6 @@ public: Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, const QScriptValue& avatarIdsToInclude = QScriptValue(), const QScriptValue& avatarIdsToDiscard = QScriptValue(), - const QStringList& jointNamesToFilter = QStringList(), bool pickAgainstMesh = true); /**jsdoc * @function AvatarManager.findRayIntersectionVector @@ -156,7 +155,6 @@ public: Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, const QVector& avatarsToInclude, const QVector& avatarsToDiscard, - const QStringList& jointNamesToFilter, bool pickAgainstMesh); /**jsdoc diff --git a/interface/src/raypick/RayPick.cpp b/interface/src/raypick/RayPick.cpp index b9028b3a60..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(), QStringList(), true); + 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 { From 2a1f1a485581c5194251462c18f0b9e017d680d3 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 28 Jan 2019 12:55:36 -0700 Subject: [PATCH 16/19] Remove debug logging --- interface/src/avatar/AvatarManager.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index a625faf71c..8d053a4d66 100755 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -398,7 +398,6 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact transaction.objectsToRemove.push_back(avatar->_motionState); avatar->_motionState = nullptr; auto& detailedMotionStates = avatar->getDetailedMotionStates(); - qDebug() << "Deleting " << detailedMotionStates.size() << "Motion states"; for (auto& mState : detailedMotionStates) { transaction.objectsToRemove.push_back(mState); } @@ -426,8 +425,6 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact } else { failedShapeBuilds.insert(avatar); } - - qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " Motion states"; } } else if (isInPhysics) { transaction.objectsToChange.push_back(avatar->_motionState); From 1b75f569b35ebaccbabd3e748e8add601b607f03 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 28 Jan 2019 15:02:10 -0700 Subject: [PATCH 17/19] fix jsdoc --- interface/src/avatar/AvatarManager.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/AvatarManager.h b/interface/src/avatar/AvatarManager.h index dcec3d1945..50d9e80e8b 100644 --- a/interface/src/avatar/AvatarManager.h +++ b/interface/src/avatar/AvatarManager.h @@ -137,7 +137,7 @@ public: * @param {PickRay} ray * @param {Uuid[]} [avatarsToInclude=[]] * @param {Uuid[]} [avatarsToDiscard=[]] - * @param {string[]} [jointNamesToFilter=[] - If included avatars are provided only this joints corresponding to those avatars would be included ] + * @param {boolean} pickAgainstMesh * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray, @@ -149,7 +149,7 @@ public: * @param {PickRay} ray * @param {Uuid[]} avatarsToInclude * @param {Uuid[]} avatarsToDiscard - * @param {string[]} [jointNamesToFilter=[] - If included avatars are provided only this joints corresponding to those avatars would be included ] + * @param {boolean} pickAgainstMesh * @returns {RayToAvatarIntersectionResult} */ Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray, From fced9e2814849d6e4daeb7b5080657bb913207c5 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 28 Jan 2019 17:43:51 -0700 Subject: [PATCH 18/19] Review fixes --- interface/src/avatar/AvatarManager.cpp | 21 +++++++++---------- libraries/physics/src/MultiSphereShape.cpp | 19 ++++++++++------- .../physics/src/ThreadSafeDynamicsWorld.cpp | 13 ++++++------ 3 files changed, 28 insertions(+), 25 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 8d053a4d66..d883aafe2c 100755 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -652,13 +652,12 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic PROFILE_RANGE(simulation_physics, __FUNCTION__); float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results - - std::vector physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector()); - + 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 = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY, - ray.direction.y != 0.0f ? 1.0f / ray.direction.y : INFINITY, - ray.direction.z != 0.0f ? 1.0f / ray.direction.z : INFINITY }; + 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 }; MyCharacterController::RayAvatarResult rayAvatarResult; AvatarPointer avatar = nullptr; @@ -697,12 +696,12 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic BoxFace face; glm::vec3 surfaceNormal; auto &bbox = mSphere.getBoundingBox(); - if (bbox.findRayIntersection(ray.origin, ray.direction, rayDirectionInv, boundDistance, face, surfaceNormal)) { + 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 * glm::normalize(ray.direction); + boxHit._intersectionPoint = ray.origin + boundDistance * rayDirection; boxHit._intersectWithAvatar = avatarID; boxHit._intersectWithJoint = mSphere.getJointIndex(); boxHits.push_back(boxHit); @@ -722,7 +721,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } if (pickAgainstMesh) { glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint); - glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + ray.direction, rayAvatarResult._intersectWithJoint); + glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + rayDirection, rayAvatarResult._intersectWithJoint); auto avatarOrientation = avatar->getWorldOrientation(); auto avatarPosition = avatar->getWorldPosition(); @@ -737,7 +736,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic 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 * glm::normalize(ray.direction); + rayAvatarResult._intersectionPoint = ray.origin + newDistance * rayDirection; rayAvatarResult._intersectionNormal = surfaceNormal; extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint); break; @@ -752,7 +751,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic result.distance = rayAvatarResult._distance; result.surfaceNormal = rayAvatarResult._intersectionNormal; result.jointIndex = rayAvatarResult._intersectWithJoint; - result.intersection = ray.origin + rayAvatarResult._distance * glm::normalize(ray.direction); + result.intersection = ray.origin + rayAvatarResult._distance * rayDirection; result.extraInfo = extraInfo; result.face = face; } diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index 656fb64e39..98fb6dd34d 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -286,6 +286,9 @@ void MultiSphereShape::spheresFromAxes(const std::vector& points, con 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; @@ -299,7 +302,7 @@ void MultiSphereShape::spheresFromAxes(const std::vector& points, con } spheres[j]._radius = radius; if (distance - radius > 0.0f) { - spheres[j]._position = (distance - radius) * glm::normalize(axis); + spheres[j]._position = ((distance - radius) / distance) * axis; } else { spheres[j]._position = glm::vec3(0.0f); } @@ -356,15 +359,15 @@ void MultiSphereShape::calculateDebugLines() { 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) { + 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) { + } 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) { + } else if (sign.z == 0.0f) { if (sign.x == CORNER_SIGNS[i].x && sign.y == CORNER_SIGNS[i].y) { add = true; } @@ -459,14 +462,14 @@ void MultiSphereShape::calculateSphereLines(std::vector Date: Mon, 28 Jan 2019 18:39:16 -0700 Subject: [PATCH 19/19] Optimization --- libraries/physics/src/MultiSphereShape.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index 98fb6dd34d..549c8042d5 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -270,9 +270,8 @@ void MultiSphereShape::spheresFromAxes(const std::vector& points, con glm::vec3 axisDir = glm::normalize(axis); for (size_t i = 0; i < points.size(); i++) { float dot = glm::dot(points[i], axisDir); - float distance = glm::length(points[i]); if (dot > 0.0f) { - float distancePow = glm::pow(distance, 2); + float distancePow = glm::distance2(Vectors::ZERO, points[i]); float dotPow = glm::pow(dot, 2); float radius = (dot / maxDistance) * glm::sqrt(distancePow - dotPow); averageRadius += radius;