_Quat4.cc

Go to the documentation of this file.

#include <vecmath/_Quat4.h>

template<> const double _Quat4<double>::EPS = 1.0e-06;
template<> const double _Quat4<double>::EPS1 = 1.0e-30;
template<> const float _Quat4<float>::EPS = 1.0e-06;
template<> const float _Quat4<float>::EPS1 = 1.0e-30;

template <class Type>
void _Quat4<Type>::setByMatrix(double m00, double m01, double m02,
                               double m10, double m11, double m12,
                               double m20, double m21, double m22)
{
    double d = (1.0 + m00 + m11 + m22) / 4;
    if (fabs(d) >= EPS1) {
        _Tuple4<Type>::w = sqrt(d);
        d = 0.25 / _Tuple4<Type>::w;
        _Tuple4<Type>::x = (m21 - m12) * d;
        _Tuple4<Type>::y = (m02 - m20) * d;
        _Tuple4<Type>::z = (m10 - m01) * d;
        return;
    }
    _Tuple4<Type>::w = 0.0;
    d = -0.5 * (m11 + m22);
    if (fabs(d) >= EPS1) {
        _Tuple4<Type>::x = sqrt(d);
        d = 1.0 / (2 * _Tuple4<Type>::x);
        _Tuple4<Type>::y = m10 * d;
        _Tuple4<Type>::z = m20 * d;
        return;
    }
    _Tuple4<Type>::x = 0.0;
    d = (1.0 - m22) / 2;
    if (fabs(d) >= EPS1) {
        _Tuple4<Type>::y = sqrt(d);
        _Tuple4<Type>::z = m21 / (2.0 * _Tuple4<Type>::y);
    } else {
        _Tuple4<Type>::y = 0.0;
        _Tuple4<Type>::z = 1.0;
    }
}

template <class Type>
void _Quat4<Type>::setByAxis(double ax, double ay, double az, double aa)
{
    double d1 = sqrt(ax * ax + ay * ay + az * az);
    if(d1 > EPS) {
        Type d = sin(aa / 2);
        _Tuple4<Type>::set(ax / d1 * d, ay / d1 * d, az / d1 * d, cos(aa / 2));
    } else
        _Tuple4<Type>::set(0.0, 0.0, 0.0, 0.0);
}

//
// theta = phi + k * PI
//
//                     sin(phi - alpha * theta)       sin(alpha * theta)
// SLERP(A, B, k, a) = ------------------------ * A + ------------------ * B
//                            sin(phi)                      sin(phi)
//
//
template <class Type>
void _Quat4<Type>::interpolate(const _Quat4<Type>& q, const _Quat4<Type>& q1, Type d)
{
    Type x, y, z, w;

    Type d1 = q.x * q1.x + q.y * q1.y + q.z * q1.z + q.w * q1.w;

    /* ???????????? */
     if (d1 < 0.0) {
         // modificated passed object - changed - using local values!!
         x = -q.x;
         y = -q.y;
         z = -q.z;
         w = -q.w;
     } else {
         x = q.x;
         y = q.y;
         z = q.z;
         w = q.w;
     }

     if(1.0 + d1 > EPS) {
         Type d2;
         Type d4;
         if(1.0 - d1 > EPS) {
             Type d6 = acos(d1);
             Type d7 = sin(d6);
             d2 = sin((1.0 - d) * d6) / d7;
             d4 = sin(d * d6) / d7;
         } else {
             d2 = 1.0 - d;
             d4 = d;
         }
         _Tuple4<Type>::set(d2 * x + d4 * q1.x,
                            d2 * y + d4 * q1.y,
                            d2 * z + d4 * q1.z,
                            d2 * w + d4 * q1.w);
     } else {
         Type d9 = x;
         Type d8 = -y;
         Type d11 = z;
         Type d10 = -w;
         Type d3 = sin((1.0 - d) * Math::PI/2.0);
         Type d5 = sin(d * Math::PI/2.0);
         _Tuple4<Type>::set(d3 * x + d5 * d8,
                            d3 * y + d5 * d9,
                            d3 * z + d5 * d10,
                            d3 * w + d5 * d11);
     }
}

template <class Type>
void _Quat4<Type>::mul(const _Quat4<Type>& q, const _Quat4<Type>& q1)
{
    Type d = (q.w * q1.x + q1.w * q.x + q.y * q1.z) - q.z * q1.y;
    Type d1 = ((q.w * q1.y + q1.w * q.y) - q.x * q1.z) + q.z * q1.x;
    Type d2 = q.w * q1.w - q.x * q1.x - q.y * q1.y - q.z * q1.z;

    _Tuple4<Type>::z = (q.w * q1.z + q1.w * q.z + q.x * q1.y) - q.y * q1.x;
    _Tuple4<Type>::x = d;
    _Tuple4<Type>::y = d1;
    _Tuple4<Type>::w = d2;
}

template class _Quat4<double>;
template class _Quat4<float>;

---