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();
}