From ae98974d792345e91cbf7c3515499aae4ab7be4a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 10 Aug 2017 15:52:51 -0700 Subject: [PATCH] improve 14-dop generation for avatar joint collision Also added debug rendering functionality. --- interface/src/avatar/MyAvatar.cpp | 31 ++++++ libraries/fbx/src/FBXReader.cpp | 8 +- libraries/fbx/src/FBXReader.h | 5 +- libraries/shared/src/GeometryUtil.cpp | 147 ++++++++++++++++++++++++++ libraries/shared/src/GeometryUtil.h | 7 ++ 5 files changed, 195 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 36295e87b8..cc2ad686a4 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1805,6 +1805,37 @@ void MyAvatar::postUpdate(float deltaTime) { AnimPose postUpdateRoomPose(_sensorToWorldMatrix); updateHoldActions(_prePhysicsRoomPose, postUpdateRoomPose); + + bool _enableDebugDrawDetailedCollision = true; + if (_enableDebugDrawDetailedCollision) { + AnimPose rigToWorldPose(glm::vec3(1.0f), Quaternions::Y_180 * getRotation(), getPosition()); + const int NUM_DEBUG_COLORS = 7; + const glm::vec4 DEBUG_COLORS[NUM_DEBUG_COLORS] = { + glm::vec4(1.0f, 1.0f, 1.0f, 1.0f), + glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), + glm::vec4(0.0f, 1.0f, 0.0f, 1.0f), + glm::vec4(0.25f, 0.25f, 1.0f, 1.0f), + glm::vec4(1.0f, 1.0f, 0.0f, 1.0f), + glm::vec4(0.25f, 1.0f, 1.0f, 1.0f), + glm::vec4(1.0f, 0.25f, 1.0f, 1.0f), + }; + + if (_skeletonModel && _skeletonModel->isLoaded()) { + const Rig& rig = _skeletonModel->getRig(); + const FBXGeometry& geometry = _skeletonModel->getFBXGeometry(); + for (int i = 0; i < rig.getJointStateCount(); i++) { + AnimPose jointPose; + rig.getAbsoluteJointPoseInRigFrame(i, jointPose); + const FBXJointShapeInfo& shapeInfo = geometry.joints[i].shapeInfo; + const AnimPose pose = rigToWorldPose * jointPose; + for (int 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]); + } + } + } + } } void MyAvatar::preDisplaySide(RenderArgs* renderArgs) { diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp index 6d4f586c52..cd313dbd05 100644 --- a/libraries/fbx/src/FBXReader.cpp +++ b/libraries/fbx/src/FBXReader.cpp @@ -1682,8 +1682,8 @@ FBXGeometry* FBXReader::extractFBXGeometry(const QVariantHash& mapping, const QS int newIndex = it.value(); // remember vertices with at least 1/4 weight - const float EXPANSION_WEIGHT_THRESHOLD = 0.99f; - if (weight > EXPANSION_WEIGHT_THRESHOLD) { + const float EXPANSION_WEIGHT_THRESHOLD = 0.25f; + if (weight >= EXPANSION_WEIGHT_THRESHOLD) { // transform to joint-frame and save for later const glm::mat4 vertexTransform = meshToJoint * glm::translate(extracted.mesh.vertices.at(newIndex)); points.push_back(extractTranslation(vertexTransform) * clusterScale); @@ -1788,6 +1788,7 @@ FBXGeometry* FBXReader::extractFBXGeometry(const QVariantHash& mapping, const QS avgPoint += points[j]; } avgPoint /= (float)points.size(); + joint.shapeInfo.avgPoint = avgPoint; // compute a k-Dop bounding volume for (uint32_t j = 0; j < cardinalDirections.size(); ++j) { @@ -1803,8 +1804,11 @@ FBXGeometry* FBXReader::extractFBXGeometry(const QVariantHash& mapping, const QS } } joint.shapeInfo.points.push_back(avgPoint + maxDot * cardinalDirections[j]); + joint.shapeInfo.dots.push_back(maxDot); joint.shapeInfo.points.push_back(avgPoint + minDot * cardinalDirections[j]); + joint.shapeInfo.dots.push_back(-minDot); } + generateBoundryLinesForDop14(joint.shapeInfo.dots, joint.shapeInfo.avgPoint, joint.shapeInfo.debugLines); } } geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString()); diff --git a/libraries/fbx/src/FBXReader.h b/libraries/fbx/src/FBXReader.h index f73088e7a1..170bbbf366 100644 --- a/libraries/fbx/src/FBXReader.h +++ b/libraries/fbx/src/FBXReader.h @@ -56,7 +56,10 @@ public: struct FBXJointShapeInfo { // same units and frame as FBXJoint.translation - QVector points; + glm::vec3 avgPoint; + std::vector dots; + std::vector points; + std::vector debugLines; }; /// A single joint (transformation node) extracted from an FBX document. diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 4ae907eb3b..0b889e4c24 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -17,6 +17,7 @@ #include #include "NumericalConstants.h" +#include "glmHelpers.h" glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { // compute the projection of the point vector onto the segment vector @@ -657,3 +658,149 @@ bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& p planeNormalOut = glm::normalize(dir); return true; } + +bool findIntersectionOfThreePlanes(const glm::vec4& planeA, const glm::vec4& planeB, const glm::vec4& planeC, glm::vec3& intersectionPointOut) { + glm::vec3 normalA(planeA); + glm::vec3 normalB(planeB); + glm::vec3 normalC(planeC); + glm::vec3 u = glm::cross(normalB, normalC); + float denom = glm::dot(normalA, u); + if (fabsf(denom) < EPSILON) { + return false; // planes do not intersect in a point. + } else { + intersectionPointOut = (planeA.w * u + glm::cross(normalA, planeC.w * normalB - planeB.w * normalC)) / denom; + return true; + } +} + +const float INV_SQRT_3 = 1.0f / sqrtf(3.0f); +const int DOP14_COUNT = 14; +const glm::vec3 DOP14_NORMALS[DOP14_COUNT] = { + Vectors::UNIT_X, + -Vectors::UNIT_X, + Vectors::UNIT_Y, + -Vectors::UNIT_Y, + Vectors::UNIT_Z, + -Vectors::UNIT_Z, + glm::vec3(INV_SQRT_3, INV_SQRT_3, INV_SQRT_3), + -glm::vec3(INV_SQRT_3, INV_SQRT_3, INV_SQRT_3), + glm::vec3(INV_SQRT_3, -INV_SQRT_3, INV_SQRT_3), + -glm::vec3(INV_SQRT_3, -INV_SQRT_3, INV_SQRT_3), + glm::vec3(INV_SQRT_3, INV_SQRT_3, -INV_SQRT_3), + -glm::vec3(INV_SQRT_3, INV_SQRT_3, -INV_SQRT_3), + glm::vec3(INV_SQRT_3, -INV_SQRT_3, -INV_SQRT_3), + -glm::vec3(INV_SQRT_3, -INV_SQRT_3, -INV_SQRT_3) +}; + +const std::tuple DOP14_PLANE_COMBINATIONS[] = { + {0,2,4}, {0,2,5}, {0,2,6}, {0,2,7}, {0,2,8}, {0,2,9}, {0,2,10}, {0,2,11}, {0,2,12}, {0,2,13}, + {0,3,4}, {0,3,5}, {0,3,6}, {0,3,7}, {0,3,8}, {0,3,9}, {0,3,10}, {0,3,11}, {0,3,12}, {0,3,13}, + {0,4,6}, {0,4,7}, {0,4,8}, {0,4,9}, {0,4,10}, {0,4,11}, {0,4,12}, {0,4,13}, + {0,5,6}, {0,5,7}, {0,5,8}, {0,5,9}, {0,5,10}, {0,5,11}, {0,5,12}, {0,5,13}, + {0,6,8}, {0,6,9}, {0,6,10}, {0,6,11}, {0,6,12}, {0,6,13}, + {0,7,8}, {0,7,9}, {0,7,10}, {0,7,11}, {0,7,12}, {0,7,13}, + {0,8,10}, {0,8,11}, {0,8,12}, {0,8,13}, {0,9,10}, + {0,9,11}, {0,9,12}, {0,9,13}, + {0,10,12}, {0,10,13}, + {0,11,12}, {0,11,13}, + {1,2,4}, {1,2,5}, {1,2,6}, {1,2,7}, {1,2,8}, {1,2,9}, {1,2,10}, {1,2,11}, {1,2,12}, {1,2,13}, + {1,3,4}, {1,3,5}, {1,3,6}, {1,3,7}, {1,3,8}, {1,3,9}, {1,3,10}, {1,3,11}, {1,3,12}, {1,3,13}, + {1,4,6}, {1,4,7}, {1,4,8}, {1,4,9}, {1,4,10}, {1,4,11}, {1,4,12}, {1,4,13}, + {1,5,6}, {1,5,7}, {1,5,8}, {1,5,9}, {1,5,10}, {1,5,11}, {1,5,12}, {1,5,13}, + {1,6,8}, {1,6,9}, {1,6,10}, {1,6,11}, {1,6,12}, {1,6,13}, + {1,7,8}, {1,7,9}, {1,7,10}, {1,7,11}, {1,7,12}, {1,7,13}, + {1,8,10}, {1,8,11}, {1,8,12}, {1,8,13}, + {1,9,10}, {1,9,11}, {1,9,12}, {1,9,13}, + {1,10,12}, {1,10,13}, + {1,11,12}, {1,11,13}, + {2,4,6}, {2,4,7}, {2,4,8}, {2,4,9}, {2,4,10}, {2,4,11}, {2,4,12}, {2,4,13}, + {2,5,6}, {2,5,7}, {2,5,8}, {2,5,9}, {2,5,10}, {2,5,11}, {2,5,12}, {2,5,13}, + {2,6,8}, {2,6,9}, {2,6,10}, {2,6,11}, {2,6,12}, {2,6,13}, + {2,7,8}, {2,7,9}, {2,7,10}, {2,7,11}, {2,7,12}, {2,7,13}, + {2,8,10}, {2,8,11}, {2,8,12}, {2,8,13}, + {2,9,10}, {2,9,11}, {2,9,12}, {2,9,13}, + {2,10,12}, {2,10,13}, + {2,11,12}, {2,11,13}, + {3,4,6}, {3,4,7}, {3,4,8}, {3,4,9}, {3,4,10}, {3,4,11}, {3,4,12}, {3,4,13}, + {3,5,6}, {3,5,7}, {3,5,8}, {3,5,9}, {3,5,10}, {3,5,11}, {3,5,12}, {3,5,13}, + {3,6,8}, {3,6,9}, {3,6,10}, {3,6,11}, {3,6,12}, {3,6,13}, + {3,7,8}, {3,7,9}, {3,7,10}, {3,7,11}, {3,7,12}, {3,7,13}, + {3,8,10}, {3,8,11}, {3,8,12}, {3,8,13}, + {3,9,10}, {3,9,11}, {3,9,12}, {3,9,13}, + {3,10,12}, {3,10,13}, + {3,11,12}, {3,11,13}, + {4,6,8}, {4,6,9}, {4,6,10}, {4,6,11}, {4,6,12}, {4,6,13}, + {4,7,8}, {4,7,9}, {4,7,10}, {4,7,11}, {4,7,12}, {4,7,13}, + {4,8,10}, {4,8,11}, {4,8,12}, {4,8,13}, + {4,9,10}, {4,9,11}, {4,9,12}, {4,9,13}, + {4,10,12}, {4,10,13}, + {4,11,12}, {4,11,13}, + {5,6,8}, {5,6,9}, {5,6,10}, {5,6,11}, {5,6,12}, {5,6,13}, + {5,7,8}, {5,7,9}, {5,7,10}, {5,7,11}, {5,7,12}, {5,7,13}, + {5,8,10}, {5,8,11}, {5,8,12}, {5,8,13}, + {5,9,10}, {5,9,11}, {5,9,12}, {5,9,13}, + {5,10,12}, {5,10,13}, + {5,11,12}, {5,11,13}, + {6,8,10}, {6,8,11}, {6,8,12}, {6,8,13}, + {6,9,10}, {6,9,11}, {6,9,12}, {6,9,13}, + {6,10,12}, {6,10,13}, + {6,11,12}, {6,11,13}, + {7,8,10}, {7,8,11}, {7,8,12}, {7,8,13}, + {7,9,10}, {7,9,11}, {7,9,12}, {7,9,13}, + {7,10,12}, {7,10,13}, + {7,11,12}, {7,11,13}, + {8,10,12}, {8,10,13}, + {8,11,12}, {8,11,13}, + {9,10,12}, {9,10,13}, + {9,11,12}, {9,11,13} +}; + +void generateBoundryLinesForDop14(const std::vector& dots, const glm::vec3& center, std::vector& linesOut) { + if (dots.size() != DOP14_COUNT) { + return; + } + + // iterate over all purmutations of non-parallel planes. + // find all the vertices that lie on the surface of the k-dop + std::vector vertices; + for (auto& tuple : DOP14_PLANE_COMBINATIONS) { + int i = std::get<0>(tuple); + int j = std::get<1>(tuple); + int k = std::get<2>(tuple); + glm::vec4 planeA(DOP14_NORMALS[i], dots[i]); + glm::vec4 planeB(DOP14_NORMALS[j], dots[j]); + glm::vec4 planeC(DOP14_NORMALS[k], dots[k]); + glm::vec3 intersectionPoint; + const float IN_FRONT_MARGIN = 0.01f; + if (findIntersectionOfThreePlanes(planeA, planeB, planeC, intersectionPoint)) { + bool inFront = false; + for (int p = 0; p < DOP14_COUNT; p++) { + if (glm::dot(DOP14_NORMALS[p], intersectionPoint) > dots[p] + IN_FRONT_MARGIN) { + inFront = true; + } + } + if (!inFront) { + vertices.push_back(intersectionPoint); + } + } + } + + // build a set of lines between these vertices, that also lie on the surface of the k-dop. + for (int i = 0; i < vertices.size(); i++) { + for (int j = i; j < vertices.size(); j++) { + glm::vec3 midPoint = (vertices[i] + vertices[j]) * 0.5f; + int onSurfaceCount = 0; + const float SURFACE_MARGIN = 0.01f; + for (int p = 0; p < DOP14_COUNT; p++) { + float d = glm::dot(DOP14_NORMALS[p], midPoint); + if (d > dots[p] - SURFACE_MARGIN && d < dots[p] + SURFACE_MARGIN) { + onSurfaceCount++; + } + } + if (onSurfaceCount > 1) { + linesOut.push_back(vertices[i] + center); + linesOut.push_back(vertices[j] + center); + } + } + } +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index a5ee67748b..eb9424d938 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -13,6 +13,7 @@ #define hifi_GeometryUtil_h #include +#include glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end); @@ -166,4 +167,10 @@ private: // given a set of points, compute a best fit plane that passes as close as possible through all the points. bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& planeNormalOut, glm::vec3& pointOnPlaneOut); +// plane equation is specified by ax + by + cz + d = 0. +// the coefficents are passed in as a vec4. (a, b, c, d) +bool findIntersectionOfThreePlanes(const glm::vec4& planeA, const glm::vec4& planeB, const glm::vec4& planeC, glm::vec3& intersectionPointOut); + +void generateBoundryLinesForDop14(const std::vector& dots, const glm::vec3& center, std::vector& linesOut); + #endif // hifi_GeometryUtil_h