////  AnimPose.cpp////  Created by Anthony J. Thibault on 10/14/15.//  Copyright (c) 2015 High Fidelity, Inc. All rights reserved.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "AnimPose.h"#include <GLMHelpers.h>#include <algorithm>#include "AnimUtil.h" const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f),                                             glm::quat(),                                             glm::vec3(0.0f)); AnimPose::AnimPose(const glm::mat4& mat) {    glm::mat3 m(mat);    _scale = glm::vec3(glm::length(m[0]), glm::length(m[1]), glm::length(m[2]));    float det = glm::determinant(m);     glm::mat3 tmp;    if (det < 0.0f) {        _scale *= -1.0f;    }     // quat_cast doesn't work so well with scaled matrices, so cancel out scale.    // also, as a side effect, multiply mirrored matrices by -1 to get the right rotation out.    tmp[0] = m[0] * (1.0f / _scale[0]);    tmp[1] = m[1] * (1.0f / _scale[1]);    tmp[2] = m[2] * (1.0f / _scale[2]);    _rot = glm::quat_cast(tmp);     // normalize quat if necessary    float lengthSquared = glm::length2(_rot);    if (glm::abs(lengthSquared - 1.0f) > EPSILON) {        float oneOverLength = 1.0f / sqrtf(lengthSquared);        _rot = glm::quat(_rot.w * oneOverLength, _rot.x * oneOverLength, _rot.y * oneOverLength, _rot.z * oneOverLength);    }     _trans = extractTranslation(mat);} glm::vec3 AnimPose::operator*(const glm::vec3& rhs) const {    return _trans + (_rot * (_scale * rhs));} glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const {    return *this * rhs;} // really slow, but accurate for transforms with non-uniform scaleglm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {    glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f);    glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f);    glm::vec3 zAxis = _rot * glm::vec3(0.0f, 0.0f, _scale.z);    glm::mat3 mat(xAxis, yAxis, zAxis);    glm::mat3 transInvMat = glm::inverse(glm::transpose(mat));    return transInvMat * rhs;} // faster, but does not handle non-uniform scale correctly.glm::vec3 AnimPose::xformVectorFast(const glm::vec3& rhs) const {    return _rot * (_scale * rhs);} AnimPose AnimPose::operator*(const AnimPose& rhs) const {    glm::mat4 result;    glm_mat4u_mul(*this, rhs, result);    return AnimPose(result);} AnimPose AnimPose::inverse() const {    return AnimPose(glm::inverse(static_cast<glm::mat4>(*this)));} // mirror about x-axis without applying negative scale.AnimPose AnimPose::mirror() const {    return AnimPose(_scale, glm::quat(_rot.w, _rot.x, -_rot.y, -_rot.z), glm::vec3(-_trans.x, _trans.y, _trans.z));} AnimPose::operator glm::mat4() const {    glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f);    glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f);    glm::vec3 zAxis = _rot * glm::vec3(0.0f, 0.0f, _scale.z);    return glm::mat4(glm::vec4(xAxis, 0.0f), glm::vec4(yAxis, 0.0f),        glm::vec4(zAxis, 0.0f), glm::vec4(_trans, 1.0f));} void AnimPose::blend(const AnimPose& srcPose, float alpha) {    _scale = lerp(srcPose._scale, _scale, alpha);    _rot = safeLerp(srcPose._rot, _rot, alpha);    _trans = lerp(srcPose._trans, _trans, alpha);}