Showing the changes to interface/src/avatar/SkeletonModel.cpp (1 of 4 files). Show all
Unified · Split
| @@ -70,7 +70,7 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) { · 69 unmodified lines | |||
| 70 | 70 | ||
| 71 | } else if (leftPalmIndex == rightPalmIndex) { | 71 | } else if (leftPalmIndex == rightPalmIndex) { |
| 72 | // right hand only | 72 | // right hand only |
| 73 | applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); | 73 | applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); |
| 74 | restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY); | 74 | restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY); |
| 75 | 75 | ||
| 76 | } else { | 76 | } else { |
| @@ -132,29 +132,6 @@ bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) { · 55 unmodified lines | |||
| 132 | return firstIndex.value < secondIndex.value; | 132 | return firstIndex.value < secondIndex.value; |
| 133 | } | 133 | } |
| 134 | 134 | ||
| 135 | void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) { | ||
| 136 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { | ||
| 137 | return; | ||
| 138 | } | ||
| 139 | setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); | ||
| 140 | |||
| 141 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); | ||
| 142 | glm::vec3 handPosition, elbowPosition; | ||
| 143 | getJointPosition(jointIndex, handPosition); | ||
| 144 | getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition); | ||
| 145 | glm::vec3 forearmVector = handPosition - elbowPosition; | ||
| 146 | float forearmLength = glm::length(forearmVector); | ||
| 147 | if (forearmLength < EPSILON) { | ||
| 148 | return; | ||
| 149 | } | ||
| 150 | JointState& state = _jointStates[jointIndex]; | ||
| 151 | glm::quat handRotation = state.getJointRotation(); | ||
| 152 | |||
| 153 | // align hand with forearm | ||
| 154 | float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; | ||
| 155 | state.applyRotationDelta(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); | ||
| 156 | } | ||
| 157 | |||
| 158 | void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position) { | 135 | void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position) { |
| 159 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { | 136 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { |
| 160 | return; | 137 | return; |
| @@ -178,52 +155,6 @@ void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec · 17 unmodified lines | |||
| 178 | state.applyRotationDeltaInModelFrame(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); | 155 | state.applyRotationDeltaInModelFrame(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); |
| 179 | } | 156 | } |
| 180 | 157 | ||
| 181 | void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { | ||
| 182 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { | ||
| 183 | return; | ||
| 184 | } | ||
| 185 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); | ||
| 186 | float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; | ||
| 187 | int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; | ||
| 188 | if (parentJointIndex == -1) { | ||
| 189 | return; | ||
| 190 | } | ||
| 191 | |||
| 192 | // rotate palm to align with its normal (normal points out of hand's palm) | ||
| 193 | glm::quat palmRotation; | ||
| 194 | if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && | ||
| 195 | Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { | ||
| 196 | JointState parentState = _jointStates[parentJointIndex]; | ||
| 197 | palmRotation = parentState.getJointRotation(); | ||
| 198 | } else { | ||
| 199 | JointState state = _jointStates[jointIndex]; | ||
| 200 | palmRotation = state.getJointRotation(); | ||
| 201 | } | ||
| 202 | palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation; | ||
| 203 | |||
| 204 | // rotate palm to align with finger direction | ||
| 205 | glm::vec3 direction = palm.getFingerDirection(); | ||
| 206 | palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation; | ||
| 207 | |||
| 208 | // set hand position, rotation | ||
| 209 | if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { | ||
| 210 | setHandPosition(jointIndex, palm.getPosition(), palmRotation); | ||
| 211 | |||
| 212 | } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { | ||
| 213 | glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f); | ||
| 214 | setJointPosition(parentJointIndex, palm.getPosition() + forearmVector * | ||
| 215 | geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale), | ||
| 216 | glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); | ||
| 217 | JointState& parentState = _jointStates[parentJointIndex]; | ||
| 218 | parentState.setRotation(palmRotation, PALM_PRIORITY); | ||
| 219 | // slam parent-relative rotation to identity | ||
| 220 | _jointStates[jointIndex]._rotation = glm::quat(); | ||
| 221 | } else { | ||
| 222 | setJointPosition(jointIndex, palm.getPosition(), palmRotation, | ||
| 223 | true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); | ||
| 224 | } | ||
| 225 | } | ||
| 226 | |||
| 227 | void SkeletonModel::applyPalmDataInModelFrame(int jointIndex, PalmData& palm) { | 158 | void SkeletonModel::applyPalmDataInModelFrame(int jointIndex, PalmData& palm) { |
| 228 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { | 159 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { |
| 229 | return; | 160 | return; |
| @@ -377,70 +308,6 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { · 147 unmodified lines | |||
| 377 | glLineWidth(1.0f); | 308 | glLineWidth(1.0f); |
| 378 | } | 309 | } |
| 379 | 310 | ||
| 380 | void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { | ||
| 381 | // this algorithm is from sample code from sixense | ||
| 382 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); | ||
| 383 | int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex; | ||
| 384 | if (elbowJointIndex == -1) { | ||
| 385 | return; | ||
| 386 | } | ||
| 387 | int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex; | ||
| 388 | glm::vec3 shoulderPosition; | ||
| 389 | if (!getJointPosition(shoulderJointIndex, shoulderPosition)) { | ||
| 390 | return; | ||
| 391 | } | ||
| 392 | // precomputed lengths | ||
| 393 | float scale = extractUniformScale(_scale); | ||
| 394 | float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale; | ||
| 395 | float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale; | ||
| 396 | |||
| 397 | // first set wrist position | ||
| 398 | glm::vec3 wristPosition = position; | ||
| 399 | |||
| 400 | glm::vec3 shoulderToWrist = wristPosition - shoulderPosition; | ||
| 401 | float distanceToWrist = glm::length(shoulderToWrist); | ||
| 402 | |||
| 403 | // prevent gimbal lock | ||
| 404 | if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) { | ||
| 405 | distanceToWrist = upperArmLength + lowerArmLength - EPSILON; | ||
| 406 | shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist; | ||
| 407 | wristPosition = shoulderPosition + shoulderToWrist; | ||
| 408 | } | ||
| 409 | |||
| 410 | // cosine of angle from upper arm to hand vector | ||
| 411 | float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / | ||
| 412 | (2 * upperArmLength * distanceToWrist); | ||
| 413 | float mid = upperArmLength * cosA; | ||
| 414 | float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA); | ||
| 415 | |||
| 416 | // direction of the elbow | ||
| 417 | glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist | ||
| 418 | glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down | ||
| 419 | const float NORMAL_WEIGHT = 0.5f; | ||
| 420 | glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); | ||
| 421 | |||
| 422 | bool rightHand = (jointIndex == geometry.rightHandJointIndex); | ||
| 423 | if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) { | ||
| 424 | finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis) | ||
| 425 | } | ||
| 426 | |||
| 427 | glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal)); | ||
| 428 | |||
| 429 | // ik solution | ||
| 430 | glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height; | ||
| 431 | glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f); | ||
| 432 | glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition); | ||
| 433 | |||
| 434 | JointState& shoulderState = _jointStates[shoulderJointIndex]; | ||
| 435 | shoulderState.setRotation(shoulderRotation, PALM_PRIORITY); | ||
| 436 | |||
| 437 | JointState& elbowState = _jointStates[elbowJointIndex]; | ||
| 438 | elbowState.setRotation(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY); | ||
| 439 | |||
| 440 | JointState& handState = _jointStates[jointIndex]; | ||
| 441 | handState.setRotation(rotation, PALM_PRIORITY); | ||
| 442 | } | ||
| 443 | |||
| 444 | void SkeletonModel::setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { | 311 | void SkeletonModel::setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { |
| 445 | // this algorithm is from sample code from sixense | 312 | // this algorithm is from sample code from sixense |
| 446 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); | 313 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); |