Showing the changes to interface/src/avatar/SkeletonModel.cpp (1 of 1 file). Show all
Unified · Split
| @@ -164,7 +164,8 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { · 163 unmodified lines | ||
| 164 | 164 | |
| 165 | 165 | // rotate palm to align with its normal (normal points out of hand's palm) |
| 166 | 166 | glm::quat palmRotation; |
| 167 | − if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { | |
| 167 | + if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && | |
| 168 | + Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { | |
| 168 | 169 | getJointRotation(parentJointIndex, palmRotation, true); |
| 169 | 170 | } else { |
| 170 | 171 | getJointRotation(jointIndex, palmRotation, true); |
| @@ -280,6 +281,55 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { · 109 unmodified lines | ||
| 280 | 281 | } |
| 281 | 282 | |
| 282 | 283 | void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { |
| 284 | + // this algorithm is from sample code from sixense | |
| 285 | + const FBXGeometry& geometry = _geometry->getFBXGeometry(); | |
| 286 | + int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex; | |
| 287 | + if (elbowJointIndex == -1) { | |
| 288 | + return; | |
| 289 | + } | |
| 290 | + int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex; | |
| 291 | + glm::vec3 shoulderPosition; | |
| 292 | + if (!getJointPosition(shoulderJointIndex, shoulderPosition)) { | |
| 293 | + return; | |
| 294 | + } | |
| 295 | + // precomputed lengths | |
| 296 | + float scale = extractUniformScale(_scale); | |
| 297 | + float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale; | |
| 298 | + float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale; | |
| 299 | + | |
| 300 | + // first set wrist position | |
| 301 | + glm::vec3 wristPosition = position; | |
| 302 | + | |
| 303 | + glm::vec3 shoulderToWrist = wristPosition - shoulderPosition; | |
| 304 | + float distanceToWrist = glm::length(shoulderToWrist); | |
| 305 | + | |
| 306 | + // prevent gimbal lock | |
| 307 | + if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) { | |
| 308 | + distanceToWrist = upperArmLength + lowerArmLength - EPSILON; | |
| 309 | + shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist; | |
| 310 | + wristPosition = shoulderPosition + shoulderToWrist; | |
| 311 | + } | |
| 312 | + | |
| 313 | + // cosine of angle from upper arm to hand vector | |
| 314 | + float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / | |
| 315 | + (2 * upperArmLength * distanceToWrist); | |
| 316 | + float mid = upperArmLength * cosA; | |
| 317 | + float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA); | |
| 318 | + | |
| 319 | + // direction of the elbow | |
| 320 | + glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist | |
| 321 | + glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down | |
| 322 | + const float NORMAL_WEIGHT = 0.5f; | |
| 323 | + glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); | |
| 324 | + | |
| 325 | + if((jointIndex == geometry.rightHandJointIndex && finalNormal.y > 0.0f) || | |
| 326 | + (jointIndex == geometry.leftHandJointIndex && finalNormal.y < 0)) { | |
| 327 | + finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis) | |
| 328 | + } | |
| 283 | 329 | |
| 330 | + glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal)); | |
| 331 | + | |
| 332 | + // ik solution | |
| 333 | + glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height; | |
| 284 | 334 | } |
| 285 | 335 | |