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.