_AxisAngle4.h

Go to the documentation of this file.

#ifndef __VECMATH_AXISANGLE4_HPP
#define __VECMATH_AXISANGLE4_HPP

#ifndef __LANG_MATH_HPP
#include <vecmath/Math.h>
#endif

template <class Type> class _AxisAngle4;

#ifndef __VECMATH_MATRIX4_HPP
#include <vecmath/_Matrix4.h>
#endif

//#define DEBUGAXISANGLE4(d) d
#define DEBUGAXISANGLE4(d)

template <class Type> class VECMATH_EXPORT _AxisAngle4 {
public:
    static const Type EPS;// = 1e-6;
    Type x, y, z;
    Type angle;

    _AxisAngle4<Type>() {}

    _AxisAngle4<Type>(Type x, Type y = 0, Type z = 1, Type angle = 0) : x(x), y(y), z(z), angle(angle) {}

    explicit _AxisAngle4<Type>(const _AxisAngle4<double>& a) {
        set(a);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 constructor &double " << *this << endl);
    }

    _AxisAngle4<Type>(const _AxisAngle4<float>& a) {
        set(a);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 constructor &float " << *this << endl);
    }

    _AxisAngle4<Type>(const _Vector3<Type>& v, Type f) {
        set(v, f);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 constructor &float " << *this << endl);
    }

    _AxisAngle4<Type>(const Type a[]) {
        set(a);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 constructor t[4] " << *this << endl);
    }

    bool epsilonEquals(const _AxisAngle4<Type>& a, double d) const {
        DEBUGAXISANGLE4(cout << "_AxisAngle4 epsilonEquals("
                        << *this << " == " << a << " (" << d << "))" << endl);
        return (Math::epsilonEquals(x, a.x, d)
                && Math::epsilonEquals(y, a.y, d)
                && Math::epsilonEquals(z, a.z, d)
                && Math::epsilonEquals(angle, a.angle, d));
    }

    bool equals(const _AxisAngle4<Type>& a) const {
        DEBUGAXISANGLE4(cout << "_AxisAngle4 equals("
                        << *this << " == " << a << endl);
        return (x == a.x && y == a.y && z == a.z && angle == a.angle);
    }

    void get(Type a[4]) const {
        a[0] = x;
        a[1] = y;
        a[2] = z;
        a[3] = angle;
    }

    // extension
    double normSquared() const {
        return Math::sqr(x) + Math::sqr(y) + Math::sqr(z);
    }

    // extension
    double norm() const {
        return sqrt(normSquared());
    }


    void set(Type xx = 0.0, Type yy = 0.0, Type zz = 1.0, Type a = 0.0) {
        x = xx;
        y = yy;
        z = zz;
        angle = a;
    }

    void set(const _AxisAngle4<double>& a) {
        set(a.x, a.y, a.z, a.angle);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set _AxisAngle4<double> " << a << " > " << *this << endl);
    }

    void set(const _AxisAngle4<float>& a) {
        set(a.x, a.y, a.z, a.angle);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set _AxisAngle4<float> " << a << " > " << *this << endl);
    }

    void set(const _Matrix3<double>& m) {
        x = m.m21 - m.m12;
        y = m.m02 - m.m20;
        z = m.m10 - m.m01;
        Type d = sqrt(x * x + y * y + z * z);
        set(x / d, y / d, z / d,
            atan2(d / 2.0, ((m.m00 + m.m11 + m.m22) - 1.0) / 2.0));
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Matrix3d " << m << " > " << *this << endl);
    }

    void set(const _Matrix3<float>& m) {
        x = m.m21 - m.m12;
        y = m.m02 - m.m20;
        z = m.m10 - m.m01;
        Type d = sqrt(x * x + y * y + z * z);
        set(x / d, y / d, z / d,
            atan2(d / 2.0, ((m.m00 + m.m11 + m.m22) - 1.0) / 2.0));
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Matrix3f " << m << " > " << *this << endl);
    }

    void set(const _Matrix4<double>& m) {
        _Matrix3<double> mm;
        m.get(mm);
        set(mm);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Matrix4d " << m << " > " << *this << endl);
    }

    void set(const _Matrix4<float>& m) {
        _Matrix3<float> mm;
        m.get(mm);
        set(mm);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Matrix4f " << m << " > " << *this << endl);
    }

    void set(const _Quat4<double>& q) {
        Type d = sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
        if(d < EPS) {
            x = y = z = 0.0;
        } else {
            x = q.x / d;
            y = q.y / d;
            z = q.z / d;
        }
        angle = 2.0 * atan2(d, q.w);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Quat4d " << q << " > " << *this << endl);
    }

    void set(const _Quat4<float>& q) {
        Type d = sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
        if (d < EPS) {
            x = y = z = 0.0;
        } else {
            x = q.x / d;
            y = q.y / d;
            z = q.z / d;
        }
        angle = 2.0 * atan2(d, q.w);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set Quat4f " << q << " > " << *this << endl);
    }

    void set(const _Vector3<Type>& v, Type f) {
        set(v.x, v.y, v.z, f);
        DEBUGAXISANGLE4(cout << "_AxisAngle4 set _AxisAngle4<float> " << a << " > " << *this << endl);
    }

    void set(Type a[4]) {
        set(a[0], a[1], a[2], a[3]);
    }
};

#endif

---