Beispiel #1
0
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);
    }
}
Beispiel #2
0
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);
}
Beispiel #3
0
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);
}
Beispiel #4
0
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));
}
Beispiel #5
0
// 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);
}
Beispiel #6
0
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);
    }
}
Beispiel #7
0
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);
}
Beispiel #8
0
float extractUniformScale(const glm::mat4& matrix) {
    return extractUniformScale(extractScale(matrix));
}
Beispiel #9
0
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);
}
Beispiel #10
0
// 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();
}