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 | 71 | } else if (leftPalmIndex == rightPalmIndex) { |
| 72 | 72 | // right hand only |
| 73 | − applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); | |
| 73 | + applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); | |
| 74 | 74 | restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY); |
| 75 | 75 | |
| 76 | 76 | } else { |
| @@ -132,29 +132,6 @@ bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) { · 55 unmodified lines | ||
| 132 | 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 | 135 | void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position) { |
| 159 | 136 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { |
| 160 | 137 | return; |
| @@ -178,52 +155,6 @@ void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec · 17 unmodified lines | ||
| 178 | 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 | 158 | void SkeletonModel::applyPalmDataInModelFrame(int jointIndex, PalmData& palm) { |
| 228 | 159 | if (jointIndex == -1 || jointIndex >= _jointStates.size()) { |
| 229 | 160 | return; |
| @@ -377,70 +308,6 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { · 147 unmodified lines | ||
| 377 | 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 | 311 | void SkeletonModel::setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { |
| 445 | 312 | // this algorithm is from sample code from sixense |
| 446 | 313 | const FBXGeometry& geometry = _geometry->getFBXGeometry(); |