示例#1
0
 Rotation3D::Rotation(const UnitQuaternion& q) {
     // quaternion to rotation conversion
     // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
     // http://en.wikipedia.org/wiki/Rotation_group_SO(3)#Quaternions_of_unit_norm
     *this << 1 - 2 * q.kY() * q.kY() - 2 * q.kZ() * q.kZ() << 2 * q.kX() * q.kY() - 2 * q.kZ() * q.kW()
           << 2 * q.kX() * q.kZ() + 2 * q.kY() * q.kW() << arma::endr
           << 2 * q.kX() * q.kY() + 2 * q.kZ() * q.kW() << 1 - 2 * q.kX() * q.kX() - 2 * q.kZ() * q.kZ()
           << 2 * q.kY() * q.kZ() - 2 * q.kX() * q.kW() << arma::endr
           << 2 * q.kX() * q.kZ() - 2 * q.kY() * q.kW() << 2 * q.kY() * q.kZ() + 2 * q.kX() * q.kW()
           << 1 - 2 * q.kX() * q.kX() - 2 * q.kY() * q.kY();
 }
    UnitQuaternion UnitQuaternion::slerp(const UnitQuaternion& p, const double& t) {
        // See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
        // Where qa = *this and qb = p

        double cosHalfTheta = kW() * p.kW() + kX() * p.kX() + kY() * p.kY() + kZ() * p.kZ();

        // If qa=qb or qa=-qb then theta = 0 and we can return qa
        if (std::abs(cosHalfTheta) >= 1.0) {
            return *this;
        }

        double halfTheta = utility::math::angle::acos_clamped(cosHalfTheta);
        double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);

        // If theta = 180 degrees then result is not fully defined
        // We could rotate around any axis normal to qa or qb
        if (std::abs(sinHalfTheta) < 0.001) {
            return *this * 0.5 + p * 0.5;
        }

        // Interpolate
        double ratioA = std::sin((1 - t) * halfTheta) / sinHalfTheta;
        double ratioB = std::sin(t * halfTheta) / sinHalfTheta;

        return *this * ratioA + p * ratioB;
    }