void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { if (jointIndex == -1 || jointIndex >= _rig->getJointStateCount()) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; if (parentJointIndex == -1) { return; } // rotate palm to align with its normal (normal points out of hand's palm) glm::quat inverseRotation = glm::inverse(_rotation); glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation); glm::vec3 palmNormal = inverseRotation * palm.getNormal(); glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection(); glm::quat palmRotation = rotationBetween(geometry.palmDirection, palmNormal); palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation; if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { _rig->setHandPosition(jointIndex, palmPosition, palmRotation, extractUniformScale(_scale), PALM_PRIORITY); } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale); glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f); setJointPosition(parentJointIndex, palmPosition + forearm, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); _rig->setJointRotationInBindFrame(parentJointIndex, palmRotation, PALM_PRIORITY); // lock hand to forearm by slamming its rotation (in parent-frame) to identity _rig->setJointRotationInConstrainedFrame(jointIndex, glm::quat(), PALM_PRIORITY); } else { inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY); } }
void SkeletonModel::renderJointConstraints(gpu::Batch& batch, int jointIndex) { if (jointIndex == -1 || jointIndex >= _jointStates.size()) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); const float BASE_DIRECTION_SIZE = 0.3f; float directionSize = BASE_DIRECTION_SIZE * extractUniformScale(_scale); batch._glLineWidth(3.0f); do { const FBXJoint& joint = geometry.joints.at(jointIndex); const JointState& jointState = _jointStates.at(jointIndex); glm::vec3 position = _rotation * jointState.getPosition() + _translation; glm::quat parentRotation = (joint.parentIndex == -1) ? _rotation : _rotation * _jointStates.at(joint.parentIndex).getRotation(); float fanScale = directionSize * 0.75f; Transform transform = Transform(); transform.setTranslation(position); transform.setRotation(parentRotation); transform.setScale(fanScale); batch.setModelTransform(transform); const int AXIS_COUNT = 3; auto geometryCache = DependencyManager::get<GeometryCache>(); for (int i = 0; i < AXIS_COUNT; i++) { if (joint.rotationMin[i] <= -PI + EPSILON && joint.rotationMax[i] >= PI - EPSILON) { continue; // unconstrained } glm::vec3 axis; axis[i] = 1.0f; glm::vec3 otherAxis; if (i == 0) { otherAxis.y = 1.0f; } else { otherAxis.x = 1.0f; } glm::vec4 color(otherAxis.r, otherAxis.g, otherAxis.b, 0.75f); QVector<glm::vec3> points; points << glm::vec3(0.0f, 0.0f, 0.0f); const int FAN_SEGMENTS = 16; for (int j = 0; j < FAN_SEGMENTS; j++) { glm::vec3 rotated = glm::angleAxis(glm::mix(joint.rotationMin[i], joint.rotationMax[i], (float)j / (FAN_SEGMENTS - 1)), axis) * otherAxis; points << rotated; } // TODO: this is really inefficient constantly recreating these vertices buffers. It would be // better if the skeleton model cached these buffers for each of the joints they are rendering geometryCache->updateVertices(_triangleFanID, points, color); geometryCache->renderVertices(batch, gpu::TRIANGLE_FAN, _triangleFanID); } renderOrientationDirections(jointIndex, position, _rotation * jointState.getRotation(), directionSize); jointIndex = joint.parentIndex; } while (jointIndex != -1 && geometry.joints.at(jointIndex).isFree); }
void SkeletonModel::renderJointConstraints(int jointIndex) { if (jointIndex == -1) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); const float BASE_DIRECTION_SIZE = 300.0f; float directionSize = BASE_DIRECTION_SIZE * extractUniformScale(_scale); glLineWidth(3.0f); do { const FBXJoint& joint = geometry.joints.at(jointIndex); const JointState& jointState = _jointStates.at(jointIndex); glm::vec3 position = extractTranslation(jointState.transform) + _translation; glPushMatrix(); glTranslatef(position.x, position.y, position.z); glm::quat parentRotation = (joint.parentIndex == -1) ? _rotation : _jointStates.at(joint.parentIndex).combinedRotation; glm::vec3 rotationAxis = glm::axis(parentRotation); glRotatef(glm::degrees(glm::angle(parentRotation)), rotationAxis.x, rotationAxis.y, rotationAxis.z); float fanScale = directionSize * 0.75f; glScalef(fanScale, fanScale, fanScale); const int AXIS_COUNT = 3; for (int i = 0; i < AXIS_COUNT; i++) { if (joint.rotationMin[i] <= -PI + EPSILON && joint.rotationMax[i] >= PI - EPSILON) { continue; // unconstrained } glm::vec3 axis; axis[i] = 1.0f; glm::vec3 otherAxis; if (i == 0) { otherAxis.y = 1.0f; } else { otherAxis.x = 1.0f; } glColor4f(otherAxis.r, otherAxis.g, otherAxis.b, 0.75f); glBegin(GL_TRIANGLE_FAN); glVertex3f(0.0f, 0.0f, 0.0f); const int FAN_SEGMENTS = 16; for (int j = 0; j < FAN_SEGMENTS; j++) { glm::vec3 rotated = glm::angleAxis(glm::mix(joint.rotationMin[i], joint.rotationMax[i], (float)j / (FAN_SEGMENTS - 1)), axis) * otherAxis; glVertex3f(rotated.x, rotated.y, rotated.z); } glEnd(); } glPopMatrix(); renderOrientationDirections(position, jointState.combinedRotation, directionSize); jointIndex = joint.parentIndex; } while (jointIndex != -1 && geometry.joints.at(jointIndex).isFree); glLineWidth(1.0f); }
void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices, const QVector<int>& fingertipJointIndices, PalmData& palm) { if (jointIndex == -1) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; if (parentJointIndex == -1) { return; } // rotate forearm to align with palm direction glm::quat palmRotation; getJointRotation(parentJointIndex, palmRotation, true); applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false); getJointRotation(parentJointIndex, palmRotation, true); // sort the finger indices by raw x, get the average direction QVector<IndexValue> fingerIndices; glm::vec3 direction; for (size_t i = 0; i < palm.getNumFingers(); i++) { glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition(); float length = glm::length(fingerVector); if (length > EPSILON) { direction += fingerVector / length; } fingerVector = glm::inverse(palmRotation) * fingerVector * -sign; IndexValue indexValue = { (int)i, atan2f(fingerVector.z, fingerVector.x) }; fingerIndices.append(indexValue); } qSort(fingerIndices.begin(), fingerIndices.end()); // rotate forearm according to average finger direction float directionLength = glm::length(direction); const unsigned int MIN_ROTATION_FINGERS = 3; if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) { applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false); getJointRotation(parentJointIndex, palmRotation, true); } // let wrist inherit forearm rotation _jointStates[jointIndex].rotation = glm::quat(); // set elbow position from wrist position glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f); setJointPosition(parentJointIndex, palm.getPosition() + forearmVector * geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale)); }
// virtual void SkeletonModel::buildShapes() { if (_geometry == NULL || _jointStates.isEmpty()) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (geometry.joints.isEmpty() || geometry.rootJointIndex == -1) { // rootJointIndex == -1 if the avatar model has no skeleton return; } float uniformScale = extractUniformScale(_scale); const int numStates = _jointStates.size(); for (int i = 0; i < numStates; i++) { JointState& state = _jointStates[i]; const FBXJoint& joint = state.getFBXJoint(); float radius = uniformScale * joint.boneRadius; float halfHeight = 0.5f * uniformScale * joint.distanceToParent; Shape::Type type = joint.shapeType; int parentIndex = joint.parentIndex; if (parentIndex == -1 || radius < EPSILON) { type = INVALID_SHAPE; } else if (type == CAPSULE_SHAPE && halfHeight < EPSILON) { // this shape is forced to be a sphere type = SPHERE_SHAPE; } Shape* shape = NULL; if (type == SPHERE_SHAPE) { shape = new SphereShape(radius); shape->setEntity(this); } else if (type == CAPSULE_SHAPE) { assert(parentIndex != -1); shape = new CapsuleShape(radius, halfHeight); shape->setEntity(this); } if (shape && parentIndex != -1) { // always disable collisions between joint and its parent disableCollisions(i, parentIndex); } _shapes.push_back(shape); } // This method moves the shapes to their default positions in Model frame. computeBoundingShape(geometry); }
void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { if (jointIndex == -1) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; if (parentJointIndex == -1) { return; } // rotate palm to align with its normal (normal points out of hand's palm) glm::quat palmRotation; if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { getJointRotation(parentJointIndex, palmRotation, true); } else { getJointRotation(jointIndex, palmRotation, true); } palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation; // rotate palm to align with finger direction glm::vec3 direction = palm.getFingerDirection(); palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation; // set hand position, rotation if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { setHandPosition(jointIndex, palm.getPosition(), palmRotation); } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f); setJointPosition(parentJointIndex, palm.getPosition() + forearmVector * geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale)); setJointRotation(parentJointIndex, palmRotation, true); _jointStates[jointIndex].rotation = glm::quat(); } else { setJointPosition(jointIndex, palm.getPosition(), palmRotation, true); } }
void SkeletonModel::stretchArm(int jointIndex, const glm::vec3& position) { // find out where the hand is pointing glm::quat handRotation; getJointRotation(jointIndex, handRotation, true); const FBXGeometry& geometry = _geometry->getFBXGeometry(); glm::vec3 forwardVector(jointIndex == geometry.rightHandJointIndex ? -1.0f : 1.0f, 0.0f, 0.0f); glm::vec3 handVector = handRotation * forwardVector; // align elbow with hand const FBXJoint& joint = geometry.joints.at(jointIndex); if (joint.parentIndex == -1) { return; } glm::quat elbowRotation; getJointRotation(joint.parentIndex, elbowRotation, true); applyRotationDelta(joint.parentIndex, rotationBetween(elbowRotation * forwardVector, handVector), false); // set position according to normal length float scale = extractUniformScale(_scale); glm::vec3 handPosition = position - _translation; glm::vec3 elbowPosition = handPosition - handVector * joint.distanceToParent * scale; // set shoulder orientation to point to elbow const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex); if (parentJoint.parentIndex == -1) { return; } glm::quat shoulderRotation; getJointRotation(parentJoint.parentIndex, shoulderRotation, true); applyRotationDelta(parentJoint.parentIndex, rotationBetween(shoulderRotation * forwardVector, elbowPosition - extractTranslation(_jointStates.at(parentJoint.parentIndex).transform)), false); // update the shoulder state updateJointState(parentJoint.parentIndex); // adjust the elbow's local translation setJointTranslation(joint.parentIndex, elbowPosition); }
float extractUniformScale(const glm::mat4& matrix) { return extractUniformScale(extractScale(matrix)); }
void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { // this algorithm is from sample code from sixense const FBXGeometry& geometry = _geometry->getFBXGeometry(); int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex; if (elbowJointIndex == -1) { return; } int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex; glm::vec3 shoulderPosition; if (!getJointPosition(shoulderJointIndex, shoulderPosition)) { return; } // precomputed lengths float scale = extractUniformScale(_scale); float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale; float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale; // first set wrist position glm::vec3 wristPosition = position; glm::vec3 shoulderToWrist = wristPosition - shoulderPosition; float distanceToWrist = glm::length(shoulderToWrist); // prevent gimbal lock if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) { distanceToWrist = upperArmLength + lowerArmLength - EPSILON; shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist; wristPosition = shoulderPosition + shoulderToWrist; } // cosine of angle from upper arm to hand vector float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / (2 * upperArmLength * distanceToWrist); float mid = upperArmLength * cosA; float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA); // direction of the elbow glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down const float NORMAL_WEIGHT = 0.5f; glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); bool rightHand = (jointIndex == geometry.rightHandJointIndex); if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) { finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis) } glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal)); // ik solution glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height; glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f); glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition); JointState& shoulderState = _jointStates[shoulderJointIndex]; shoulderState.setRotationInBindFrame(shoulderRotation, PALM_PRIORITY); JointState& elbowState = _jointStates[elbowJointIndex]; elbowState.setRotationInBindFrame(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY); JointState& handState = _jointStates[jointIndex]; handState.setRotationInBindFrame(rotation, PALM_PRIORITY); }
// virtual void SkeletonModel::buildShapes() { if (_geometry == NULL || _jointStates.isEmpty()) { return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (geometry.joints.isEmpty()) { return; } if (!_ragdoll) { _ragdoll = new SkeletonRagdoll(this); } _ragdoll->setRootIndex(geometry.rootJointIndex); _ragdoll->initPoints(); QVector<VerletPoint>& points = _ragdoll->getPoints(); float massScale = _ragdoll->getMassScale(); float uniformScale = extractUniformScale(_scale); const int numStates = _jointStates.size(); float totalMass = 0.0f; for (int i = 0; i < numStates; i++) { JointState& state = _jointStates[i]; const FBXJoint& joint = state.getFBXJoint(); float radius = uniformScale * joint.boneRadius; float halfHeight = 0.5f * uniformScale * joint.distanceToParent; Shape::Type type = joint.shapeType; int parentIndex = joint.parentIndex; if (parentIndex == -1 || radius < EPSILON) { type = SHAPE_TYPE_UNKNOWN; } else if (type == SHAPE_TYPE_CAPSULE && halfHeight < EPSILON) { // this shape is forced to be a sphere type = SHAPE_TYPE_SPHERE; } Shape* shape = NULL; if (type == SHAPE_TYPE_SPHERE) { shape = new VerletSphereShape(radius, &(points[i])); shape->setEntity(this); float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); points[i].setMass(mass); totalMass += mass; } else if (type == SHAPE_TYPE_CAPSULE) { assert(parentIndex != -1); shape = new VerletCapsuleShape(radius, &(points[parentIndex]), &(points[i])); shape->setEntity(this); float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); points[i].setMass(mass); totalMass += mass; } if (shape && parentIndex != -1) { // always disable collisions between joint and its parent disableCollisions(i, parentIndex); } _shapes.push_back(shape); } // set the mass of the root if (numStates > 0) { points[_ragdoll->getRootIndex()].setMass(totalMass); } // This method moves the shapes to their default positions in Model frame. computeBoundingShape(geometry); // While the shapes are in their default position we disable collisions between // joints that are currently colliding. disableCurrentSelfCollisions(); _ragdoll->buildConstraints(); // ... then move shapes back to current joint positions _ragdoll->slamPointPositions(); _ragdoll->enforceConstraints(); }