diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 56bd3eb749..2c4a515736 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -555,6 +555,8 @@ Menu::Menu() { avatar.get(), SLOT(setEnableDebugDrawIKConstraints(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKChains, 0, false, avatar.get(), SLOT(setEnableDebugDrawIKChains(bool))); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderDetailedCollision, 0, false, + avatar.get(), SLOT(setEnableDebugDrawDetailedCollision(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl, Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar.get(), SLOT(updateMotionBehaviorFromMenu()), diff --git a/interface/src/Menu.h b/interface/src/Menu.h index a81ef9ac86..4e21cfa4ac 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -162,6 +162,7 @@ namespace MenuOption { const QString RenderIKTargets = "Show IK Targets"; const QString RenderIKConstraints = "Show IK Constraints"; const QString RenderIKChains = "Show IK Chains"; + const QString RenderDetailedCollision = "Show Detailed Collision"; const QString ResetAvatarSize = "Reset Avatar Size"; const QString ResetSensors = "Reset Sensors"; const QString RunningScripts = "Running Scripts..."; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 7f46a9e3bf..30c6a85470 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1060,6 +1060,10 @@ void MyAvatar::setEnableDebugDrawIKConstraints(bool isEnabled) { _enableDebugDrawIKConstraints = isEnabled; } +void MyAvatar::setEnableDebugDrawDetailedCollision(bool isEnabled) { + _enableDebugDrawDetailedCollision = isEnabled; +} + void MyAvatar::setEnableDebugDrawIKChains(bool isEnabled) { _enableDebugDrawIKChains = isEnabled; } @@ -1805,6 +1809,37 @@ void MyAvatar::postUpdate(float deltaTime) { AnimPose postUpdateRoomPose(_sensorToWorldMatrix); updateHoldActions(_prePhysicsRoomPose, postUpdateRoomPose); + + if (_enableDebugDrawDetailedCollision) { + AnimPose rigToWorldPose(glm::vec3(1.0f), getRotation() * Quaternions::Y_180, getPosition()); + const int NUM_DEBUG_COLORS = 8; + 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), + glm::vec4(1.0f, 0.65f, 0.0f, 1.0f) // Orange you glad I added this color? + }; + + 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 (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]); + } + } + } + } } void MyAvatar::preDisplaySide(RenderArgs* renderArgs) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index dc4357be52..c7fe309894 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -553,6 +553,7 @@ public slots: void setEnableDebugDrawIKTargets(bool isEnabled); void setEnableDebugDrawIKConstraints(bool isEnabled); void setEnableDebugDrawIKChains(bool isEnabled); + void setEnableDebugDrawDetailedCollision(bool isEnabled); bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); } void setEnableMeshVisible(bool isEnabled); @@ -757,6 +758,7 @@ private: bool _enableDebugDrawIKTargets { false }; bool _enableDebugDrawIKConstraints { false }; bool _enableDebugDrawIKChains { false }; + bool _enableDebugDrawDetailedCollision { false }; AudioListenerMode _audioListenerMode; glm::vec3 _customListenPosition; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 6d468c3f30..89e4368515 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -124,12 +124,26 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { } } - params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); - params.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); - params.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); - params.isTalking = head->getTimeWithoutTalking() <= 1.5f; + // pass detailed torso k-dops to rig. + int hipsJoint = _rig.indexOfJoint("Hips"); + if (hipsJoint >= 0) { + params.hipsShapeInfo = geometry.joints[hipsJoint].shapeInfo; + } + int spineJoint = _rig.indexOfJoint("Spine"); + if (spineJoint >= 0) { + params.spineShapeInfo = geometry.joints[spineJoint].shapeInfo; + } + int spine1Joint = _rig.indexOfJoint("Spine1"); + if (spine1Joint >= 0) { + params.spine1ShapeInfo = geometry.joints[spine1Joint].shapeInfo; + } + int spine2Joint = _rig.indexOfJoint("Spine2"); + if (spine2Joint >= 0) { + params.spine2ShapeInfo = geometry.joints[spine2Joint].shapeInfo; + } + _rig.updateFromControllerParameters(params, deltaTime); Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 21f98d3e01..3a31ccd25f 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1099,35 +1099,139 @@ void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPos } } -void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, - const AnimPose& leftHandPose, const AnimPose& rightHandPose, - float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { +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) +}; - // Use this capsule to represent the avatar body. - int hipsIndex = indexOfJoint("Hips"); - glm::vec3 hipsTrans; - if (hipsIndex >= 0) { - hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); +// returns true if the given point lies inside of the k-dop, specified by shapeInfo & shapePose. +// if the given point does lie within the k-dop, it also returns the amount of displacement necessary to push that point outward +// such that it lies on the surface of the kdop. +static bool findPointKDopDisplacement(const glm::vec3& point, const AnimPose& shapePose, const FBXJointShapeInfo& shapeInfo, glm::vec3& displacementOut) { + + // transform point into local space of jointShape. + glm::vec3 localPoint = shapePose.inverse().xformPoint(point); + + // Only works for 14-dop shape infos. + assert(shapeInfo.dots.size() == DOP14_COUNT); + if (shapeInfo.dots.size() != DOP14_COUNT) { + return false; } - const glm::vec3 bodyCapsuleCenter = hipsTrans - bodyCapsuleLocalOffset; - const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, bodyCapsuleHalfHeight, 0); - const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0); + glm::vec3 minDisplacement(FLT_MAX); + float minDisplacementLen = FLT_MAX; + glm::vec3 p = localPoint - shapeInfo.avgPoint; + float pLen = glm::length(p); + if (pLen > 0.0f) { + int slabCount = 0; + for (int i = 0; i < DOP14_COUNT; i++) { + float dot = glm::dot(p, DOP14_NORMALS[i]); + if (dot > 0.0f && dot < shapeInfo.dots[i]) { + slabCount++; + float distToPlane = pLen * (shapeInfo.dots[i] / dot); + float displacementLen = distToPlane - pLen; + + // keep track of the smallest displacement + if (displacementLen < minDisplacementLen) { + minDisplacementLen = displacementLen; + minDisplacement = (p / pLen) * displacementLen; + } + } + } + if (slabCount == (DOP14_COUNT / 2) && minDisplacementLen != FLT_MAX) { + // we are within the k-dop so push the point along the minimum displacement found + displacementOut = shapePose.xformVectorFast(minDisplacement); + return true; + } else { + // point is outside of kdop + return false; + } + } else { + // point is directly on top of shapeInfo.avgPoint. + // push the point out along the x axis. + displacementOut = shapePose.xformVectorFast(shapeInfo.points[0]); + return true; + } +} + +glm::vec3 Rig::deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const { + glm::vec3 position = handPosition; + glm::vec3 displacement; + int hipsJoint = indexOfJoint("Hips"); + if (hipsJoint >= 0) { + AnimPose hipsPose; + if (getAbsoluteJointPoseInRigFrame(hipsJoint, hipsPose)) { + if (findPointKDopDisplacement(position, hipsPose, hipsShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spineJoint = indexOfJoint("Spine"); + if (spineJoint >= 0) { + AnimPose spinePose; + if (getAbsoluteJointPoseInRigFrame(spineJoint, spinePose)) { + if (findPointKDopDisplacement(position, spinePose, spineShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spine1Joint = indexOfJoint("Spine1"); + if (spine1Joint >= 0) { + AnimPose spine1Pose; + if (getAbsoluteJointPoseInRigFrame(spine1Joint, spine1Pose)) { + if (findPointKDopDisplacement(position, spine1Pose, spine1ShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spine2Joint = indexOfJoint("Spine2"); + if (spine2Joint >= 0) { + AnimPose spine2Pose; + if (getAbsoluteJointPoseInRigFrame(spine2Joint, spine2Pose)) { + if (findPointKDopDisplacement(position, spine2Pose, spine2ShapeInfo, displacement)) { + position += displacement; + } + } + } + + return position; +} + +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) { - const float HAND_RADIUS = 0.05f; const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; + int hipsIndex = indexOfJoint("Hips"); + if (leftHandEnabled) { glm::vec3 handPosition = leftHandPose.trans(); glm::quat handRotation = leftHandPose.rot(); if (!hipsEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } + // prevent the hand IK targets from intersecting the torso + handPosition = deflectHandFromTorso(handPosition, hipsShapeInfo, spineShapeInfo, spine1ShapeInfo, spine2ShapeInfo); } _animVars.set("leftHandPosition", handPosition); @@ -1173,11 +1277,8 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab glm::quat handRotation = rightHandPose.rot(); if (!hipsEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } + // prevent the hand IK targets from intersecting the torso + handPosition = deflectHandFromTorso(handPosition, hipsShapeInfo, spineShapeInfo, spine1ShapeInfo, spine2ShapeInfo); } _animVars.set("rightHandPosition", handPosition); @@ -1414,7 +1515,7 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt, params.primaryControllerPoses[PrimaryControllerType_LeftHand], params.primaryControllerPoses[PrimaryControllerType_RightHand], - params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); + params.hipsShapeInfo, params.spineShapeInfo, params.spine1ShapeInfo, params.spine2ShapeInfo); updateFeet(leftFootEnabled, rightFootEnabled, params.primaryControllerPoses[PrimaryControllerType_LeftFoot], params.primaryControllerPoses[PrimaryControllerType_RightFoot]); @@ -1730,7 +1831,7 @@ void Rig::computeAvatarBoundingCapsule( const FBXJointShapeInfo& shapeInfo = geometry.joints.at(index).shapeInfo; AnimPose pose = finalPoses[index]; if (shapeInfo.points.size() > 0) { - for (int j = 0; j < shapeInfo.points.size(); ++j) { + for (size_t j = 0; j < shapeInfo.points.size(); ++j) { totalExtents.addPoint((pose * shapeInfo.points[j])); } } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 7e1504e461..ca55635250 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -75,9 +75,10 @@ public: AnimPose secondaryControllerPoses[NumSecondaryControllerTypes]; // rig space bool secondaryControllerActiveFlags[NumSecondaryControllerTypes]; bool isTalking; - float bodyCapsuleRadius; - float bodyCapsuleHalfHeight; - glm::vec3 bodyCapsuleLocalOffset; + FBXJointShapeInfo hipsShapeInfo; + FBXJointShapeInfo spineShapeInfo; + FBXJointShapeInfo spine1ShapeInfo; + FBXJointShapeInfo spine2ShapeInfo; }; struct EyeParameters { @@ -249,7 +250,8 @@ protected: void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, - float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); + const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo); void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); @@ -257,6 +259,8 @@ protected: glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; + glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) 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..6b9718fbb8 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,150 @@ 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) +}; + +typedef std::tuple Int3Tuple; +const std::tuple DOP14_PLANE_COMBINATIONS[] = { + Int3Tuple(0, 2, 4), Int3Tuple(0, 2, 5), Int3Tuple(0, 2, 6), Int3Tuple(0, 2, 7), Int3Tuple(0, 2, 8), Int3Tuple(0, 2, 9), Int3Tuple(0, 2, 10), Int3Tuple(0, 2, 11), Int3Tuple(0, 2, 12), Int3Tuple(0, 2, 13), + Int3Tuple(0, 3, 4), Int3Tuple(0, 3, 5), Int3Tuple(0, 3, 6), Int3Tuple(0, 3, 7), Int3Tuple(0, 3, 8), Int3Tuple(0, 3, 9), Int3Tuple(0, 3, 10), Int3Tuple(0, 3, 11), Int3Tuple(0, 3, 12), Int3Tuple(0, 3, 13), + Int3Tuple(0, 4, 6), Int3Tuple(0, 4, 7), Int3Tuple(0, 4, 8), Int3Tuple(0, 4, 9), Int3Tuple(0, 4, 10), Int3Tuple(0, 4, 11), Int3Tuple(0, 4, 12), Int3Tuple(0, 4, 13), + Int3Tuple(0, 5, 6), Int3Tuple(0, 5, 7), Int3Tuple(0, 5, 8), Int3Tuple(0, 5, 9), Int3Tuple(0, 5, 10), Int3Tuple(0, 5, 11), Int3Tuple(0, 5, 12), Int3Tuple(0, 5, 13), + Int3Tuple(0, 6, 8), Int3Tuple(0, 6, 9), Int3Tuple(0, 6, 10), Int3Tuple(0, 6, 11), Int3Tuple(0, 6, 12), Int3Tuple(0, 6, 13), + Int3Tuple(0, 7, 8), Int3Tuple(0, 7, 9), Int3Tuple(0, 7, 10), Int3Tuple(0, 7, 11), Int3Tuple(0, 7, 12), Int3Tuple(0, 7, 13), + Int3Tuple(0, 8, 10), Int3Tuple(0, 8, 11), Int3Tuple(0, 8, 12), Int3Tuple(0, 8, 13), Int3Tuple(0, 9, 10), + Int3Tuple(0, 9, 11), Int3Tuple(0, 9, 12), Int3Tuple(0, 9, 13), + Int3Tuple(0, 10, 12), Int3Tuple(0, 10, 13), + Int3Tuple(0, 11, 12), Int3Tuple(0, 11, 13), + Int3Tuple(1, 2, 4), Int3Tuple(1, 2, 5), Int3Tuple(1, 2, 6), Int3Tuple(1, 2, 7), Int3Tuple(1, 2, 8), Int3Tuple(1, 2, 9), Int3Tuple(1, 2, 10), Int3Tuple(1, 2, 11), Int3Tuple(1, 2, 12), Int3Tuple(1, 2, 13), + Int3Tuple(1, 3, 4), Int3Tuple(1, 3, 5), Int3Tuple(1, 3, 6), Int3Tuple(1, 3, 7), Int3Tuple(1, 3, 8), Int3Tuple(1, 3, 9), Int3Tuple(1, 3, 10), Int3Tuple(1, 3, 11), Int3Tuple(1, 3, 12), Int3Tuple(1, 3, 13), + Int3Tuple(1, 4, 6), Int3Tuple(1, 4, 7), Int3Tuple(1, 4, 8), Int3Tuple(1, 4, 9), Int3Tuple(1, 4, 10), Int3Tuple(1, 4, 11), Int3Tuple(1, 4, 12), Int3Tuple(1, 4, 13), + Int3Tuple(1, 5, 6), Int3Tuple(1, 5, 7), Int3Tuple(1, 5, 8), Int3Tuple(1, 5, 9), Int3Tuple(1, 5, 10), Int3Tuple(1, 5, 11), Int3Tuple(1, 5, 12), Int3Tuple(1, 5, 13), + Int3Tuple(1, 6, 8), Int3Tuple(1, 6, 9), Int3Tuple(1, 6, 10), Int3Tuple(1, 6, 11), Int3Tuple(1, 6, 12), Int3Tuple(1, 6, 13), + Int3Tuple(1, 7, 8), Int3Tuple(1, 7, 9), Int3Tuple(1, 7, 10), Int3Tuple(1, 7, 11), Int3Tuple(1, 7, 12), Int3Tuple(1, 7, 13), + Int3Tuple(1, 8, 10), Int3Tuple(1, 8, 11), Int3Tuple(1, 8, 12), Int3Tuple(1, 8, 13), + Int3Tuple(1, 9, 10), Int3Tuple(1, 9, 11), Int3Tuple(1, 9, 12), Int3Tuple(1, 9, 13), + Int3Tuple(1, 10, 12), Int3Tuple(1, 10, 13), + Int3Tuple(1, 11, 12), Int3Tuple(1, 11, 13), + Int3Tuple(2, 4, 6), Int3Tuple(2, 4, 7), Int3Tuple(2, 4, 8), Int3Tuple(2, 4, 9), Int3Tuple(2, 4, 10), Int3Tuple(2, 4, 11), Int3Tuple(2, 4, 12), Int3Tuple(2, 4, 13), + Int3Tuple(2, 5, 6), Int3Tuple(2, 5, 7), Int3Tuple(2, 5, 8), Int3Tuple(2, 5, 9), Int3Tuple(2, 5, 10), Int3Tuple(2, 5, 11), Int3Tuple(2, 5, 12), Int3Tuple(2, 5, 13), + Int3Tuple(2, 6, 8), Int3Tuple(2, 6, 9), Int3Tuple(2, 6, 10), Int3Tuple(2, 6, 11), Int3Tuple(2, 6, 12), Int3Tuple(2, 6, 13), + Int3Tuple(2, 7, 8), Int3Tuple(2, 7, 9), Int3Tuple(2, 7, 10), Int3Tuple(2, 7, 11), Int3Tuple(2, 7, 12), Int3Tuple(2, 7, 13), + Int3Tuple(2, 8, 10), Int3Tuple(2, 8, 11), Int3Tuple(2, 8, 12), Int3Tuple(2, 8, 13), + Int3Tuple(2, 9, 10), Int3Tuple(2, 9, 11), Int3Tuple(2, 9, 12), Int3Tuple(2, 9, 13), + Int3Tuple(2, 10, 12), Int3Tuple(2, 10, 13), + Int3Tuple(2, 11, 12), Int3Tuple(2, 11, 13), + Int3Tuple(3, 4, 6), Int3Tuple(3, 4, 7), Int3Tuple(3, 4, 8), Int3Tuple(3, 4, 9), Int3Tuple(3, 4, 10), Int3Tuple(3, 4, 11), Int3Tuple(3, 4, 12), Int3Tuple(3, 4, 13), + Int3Tuple(3, 5, 6), Int3Tuple(3, 5, 7), Int3Tuple(3, 5, 8), Int3Tuple(3, 5, 9), Int3Tuple(3, 5, 10), Int3Tuple(3, 5, 11), Int3Tuple(3, 5, 12), Int3Tuple(3, 5, 13), + Int3Tuple(3, 6, 8), Int3Tuple(3, 6, 9), Int3Tuple(3, 6, 10), Int3Tuple(3, 6, 11), Int3Tuple(3, 6, 12), Int3Tuple(3, 6, 13), + Int3Tuple(3, 7, 8), Int3Tuple(3, 7, 9), Int3Tuple(3, 7, 10), Int3Tuple(3, 7, 11), Int3Tuple(3, 7, 12), Int3Tuple(3, 7, 13), + Int3Tuple(3, 8, 10), Int3Tuple(3, 8, 11), Int3Tuple(3, 8, 12), Int3Tuple(3, 8, 13), + Int3Tuple(3, 9, 10), Int3Tuple(3, 9, 11), Int3Tuple(3, 9, 12), Int3Tuple(3, 9, 13), + Int3Tuple(3, 10, 12), Int3Tuple(3, 10, 13), + Int3Tuple(3, 11, 12), Int3Tuple(3, 11, 13), + Int3Tuple(4, 6, 8), Int3Tuple(4, 6, 9), Int3Tuple(4, 6, 10), Int3Tuple(4, 6, 11), Int3Tuple(4, 6, 12), Int3Tuple(4, 6, 13), + Int3Tuple(4, 7, 8), Int3Tuple(4, 7, 9), Int3Tuple(4, 7, 10), Int3Tuple(4, 7, 11), Int3Tuple(4, 7, 12), Int3Tuple(4, 7, 13), + Int3Tuple(4, 8, 10), Int3Tuple(4, 8, 11), Int3Tuple(4, 8, 12), Int3Tuple(4, 8, 13), + Int3Tuple(4, 9, 10), Int3Tuple(4, 9, 11), Int3Tuple(4, 9, 12), Int3Tuple(4, 9, 13), + Int3Tuple(4, 10, 12), Int3Tuple(4, 10, 13), + Int3Tuple(4, 11, 12), Int3Tuple(4, 11, 13), + Int3Tuple(5, 6, 8), Int3Tuple(5, 6, 9), Int3Tuple(5, 6, 10), Int3Tuple(5, 6, 11), Int3Tuple(5, 6, 12), Int3Tuple(5, 6, 13), + Int3Tuple(5, 7, 8), Int3Tuple(5, 7, 9), Int3Tuple(5, 7, 10), Int3Tuple(5, 7, 11), Int3Tuple(5, 7, 12), Int3Tuple(5, 7, 13), + Int3Tuple(5, 8, 10), Int3Tuple(5, 8, 11), Int3Tuple(5, 8, 12), Int3Tuple(5, 8, 13), + Int3Tuple(5, 9, 10), Int3Tuple(5, 9, 11), Int3Tuple(5, 9, 12), Int3Tuple(5, 9, 13), + Int3Tuple(5, 10, 12), Int3Tuple(5, 10, 13), + Int3Tuple(5, 11, 12), Int3Tuple(5, 11, 13), + Int3Tuple(6, 8, 10), Int3Tuple(6, 8, 11), Int3Tuple(6, 8, 12), Int3Tuple(6, 8, 13), + Int3Tuple(6, 9, 10), Int3Tuple(6, 9, 11), Int3Tuple(6, 9, 12), Int3Tuple(6, 9, 13), + Int3Tuple(6, 10, 12), Int3Tuple(6, 10, 13), + Int3Tuple(6, 11, 12), Int3Tuple(6, 11, 13), + Int3Tuple(7, 8, 10), Int3Tuple(7, 8, 11), Int3Tuple(7, 8, 12), Int3Tuple(7, 8, 13), + Int3Tuple(7, 9, 10), Int3Tuple(7, 9, 11), Int3Tuple(7, 9, 12), Int3Tuple(7, 9, 13), + Int3Tuple(7, 10, 12), Int3Tuple(7, 10, 13), + Int3Tuple(7, 11, 12), Int3Tuple(7, 11, 13), + Int3Tuple(8, 10, 12), Int3Tuple(8, 10, 13), + Int3Tuple(8, 11, 12), Int3Tuple(8, 11, 13), + Int3Tuple(9, 10, 12), Int3Tuple(9, 10, 13), + Int3Tuple(9, 11, 12), Int3Tuple(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 (size_t i = 0; i < vertices.size(); i++) { + for (size_t 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