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 | // rotate palm to align with its normal (normal points out of hand's palm) | 165 | // rotate palm to align with its normal (normal points out of hand's palm) |
| 166 | glm::quat palmRotation; | 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 | getJointRotation(parentJointIndex, palmRotation, true); | 169 | getJointRotation(parentJointIndex, palmRotation, true); |
| 169 | } else { | 170 | } else { |
| 170 | getJointRotation(jointIndex, palmRotation, true); | 171 | getJointRotation(jointIndex, palmRotation, true); |
| @@ -280,6 +281,55 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { · 109 unmodified lines | |||
| 280 | } | 281 | } |
| 281 | 282 | ||
| 282 | void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { | 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 | ||