diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 74940b44cc..b6c5c6d235 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1525,8 +1525,8 @@ void Avatar::rigReset() { void Avatar::computeMultiSphereShapes() { const Rig& rig = getSkeletonModel()->getRig(); - glm::vec3 scale = extractScale(rig.getGeometryOffsetPose()); const HFMModel& geometry = getSkeletonModel()->getHFMModel(); + glm::vec3 geometryScale = extractScale(rig.getGeometryOffsetPose()); int jointCount = rig.getJointStateCount(); _multiSphereShapes.clear(); _multiSphereShapes.reserve(jointCount); @@ -1535,9 +1535,10 @@ void Avatar::computeMultiSphereShapes() { std::vector btPoints; int lineCount = (int)shapeInfo.debugLines.size(); btPoints.reserve(lineCount); + glm::vec3 jointScale = rig.getJointPose(i).scale() / extractScale(rig.getGeometryToRigTransform()); for (int j = 0; j < lineCount; j++) { const glm::vec3 &point = shapeInfo.debugLines[j]; - auto rigPoint = scale * point; + auto rigPoint = jointScale * geometryScale * point; btVector3 btPoint = glmToBullet(rigPoint); btPoints.push_back(btPoint); } diff --git a/libraries/physics/src/MultiSphereShape.cpp b/libraries/physics/src/MultiSphereShape.cpp index 549c8042d5..5a5a9d8dab 100644 --- a/libraries/physics/src/MultiSphereShape.cpp +++ b/libraries/physics/src/MultiSphereShape.cpp @@ -17,6 +17,14 @@ void SphereRegion::translate(const glm::vec3& translation) { line.second += translation; } } + +void SphereRegion::scale(float scale) { + for (auto &line : _lines) { + line.first *= scale; + line.second *= scale; + } +} + void SphereRegion::dump(std::vector>& outLines) { for (auto &line : _lines) { outLines.push_back(line); @@ -127,7 +135,7 @@ bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& na _jointIndex = jointIndex; _name = name; _mode = getExtractionModeByName(_name); - if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) { + if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4) { return false; } std::vector points; @@ -151,7 +159,9 @@ bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& na _midPoint /= (int)points.size(); glm::vec3 dimensions = max - min; - + if (glm::length(dimensions) == 0.0f) { + return false; + } for (size_t i = 0; i < points.size(); i++) { glm::vec3 relPoint = points[i] - _midPoint; relPoints.push_back(relPoint); @@ -343,6 +353,7 @@ void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) { } void MultiSphereShape::calculateDebugLines() { + std::vector radiuses; if (_spheres.size() == 1) { auto sphere = _spheres[0]; calculateSphereLines(_debugLines, sphere._position, sphere._radius); @@ -351,41 +362,25 @@ void MultiSphereShape::calculateDebugLines() { } else if (_spheres.size() == 4) { std::vector axes; axes.resize(8); + const float AXIS_DOT_THRESHOLD = 0.3f; for (size_t i = 0; i < CORNER_SIGNS.size(); i++) { - for (size_t j = 0; j < 4; j++) { + for (size_t j = 0; j < _spheres.size(); j++) { auto axis = _spheres[j]._position - _midPoint; - glm::vec3 sign = { axis.x != 0.0f ? glm::abs(axis.x) / axis.x : 0.0f, - axis.x != 0.0f ? glm::abs(axis.y) / axis.y : 0.0f , - axis.z != 0.0f ? glm::abs(axis.z) / axis.z : 0.0f }; - bool add = false; - if (sign.x == 0.0f) { - if (sign.y == CORNER_SIGNS[i].y && sign.z == CORNER_SIGNS[i].z) { - add = true; - } - } else if (sign.y == 0.0f) { - if (sign.x == CORNER_SIGNS[i].x && sign.z == CORNER_SIGNS[i].z) { - add = true; - } - } else if (sign.z == 0.0f) { - if (sign.x == CORNER_SIGNS[i].x && sign.y == CORNER_SIGNS[i].y) { - add = true; - } - } else if (sign == CORNER_SIGNS[i]) { - add = true; - } - if (add) { + if (glm::length(axes[i]) == 0.0f && glm::length(axis) > 0.0f && glm::dot(CORNER_SIGNS[i], glm::normalize(axis)) > AXIS_DOT_THRESHOLD) { + radiuses.push_back(_spheres[j]._radius); axes[i] = axis; break; } - } + } } - calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint); + calculateChamferBox(_debugLines, radiuses, axes, _midPoint); } else if (_spheres.size() == 8) { std::vector axes; for (size_t i = 0; i < _spheres.size(); i++) { + radiuses.push_back(_spheres[i]._radius); axes.push_back(_spheres[i]._position - _midPoint); } - calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint); + calculateChamferBox(_debugLines, radiuses, axes, _midPoint); } } @@ -398,9 +393,9 @@ void MultiSphereShape::connectEdges(std::vector> } } -void MultiSphereShape::calculateChamferBox(std::vector>& outLines, const float& radius, const std::vector& axes, const glm::vec3& translation) { +void MultiSphereShape::calculateChamferBox(std::vector>& outLines, const std::vector& radiuses, const std::vector& axes, const glm::vec3& translation) { std::vector> sphereLines; - calculateSphereLines(sphereLines, glm::vec3(0.0f), radius); + calculateSphereLines(sphereLines, glm::vec3(0.0f), radiuses[0]); std::vector regions = { SphereRegion({ 1.0f, 1.0f, 1.0f }), @@ -417,6 +412,7 @@ void MultiSphereShape::calculateChamferBox(std::vector>& outLines); void extractEdges(bool reverseY = false); void translate(const glm::vec3& translation); + void scale(float scale); void dump(std::vector>& outLines); const glm::vec3& getDirection() const { return _direction; } const std::vector& getEdgesX() const { return _edgesX; } @@ -94,7 +95,7 @@ private: 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 calculateChamferBox(std::vector>& outLines, const std::vector& radiuses, 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);