Changeset 1048


Ignore:
Timestamp:
Nov 4, 2011, 11:42:32 PM (9 years ago)
Author:
sam
Message:

math: build quaternions from rotation matrices and conversely.

Location:
trunk/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/src/matrix.cpp

    r1047 r1048  
    257257{
    258258    return rotate(angle, v.x, v.y, v.z);
     259}
     260
     261template<> mat4 mat4::rotate(quat q)
     262{
     263    mat4 ret(1.0f);
     264    float n = q.norm();
     265
     266    if (!n)
     267        return ret;
     268
     269    float s = 2.0f / n;
     270
     271    ret[0][0] = 1.0f - s * (q.y * q.y + q.z * q.z);
     272    ret[0][1] = s * (q.x * q.y - q.z * q.w);
     273    ret[0][2] = s * (q.x * q.z + q.y * q.w);
     274
     275    ret[1][0] = s * (q.x * q.y + q.z * q.w);
     276    ret[1][1] = 1.0f - s * (q.z * q.z + q.x * q.x);
     277    ret[1][2] = s * (q.y * q.z - q.x * q.w);
     278
     279    ret[2][0] = s * (q.x * q.z - q.y * q.w);
     280    ret[2][1] = s * (q.y * q.z + q.x * q.w);
     281    ret[2][2] = 1.0f - s * (q.x * q.x + q.y * q.y);
     282
     283    return ret;
     284}
     285
     286template<> quat::Quat(mat4 const &m)
     287{
     288    /* See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/christian.htm for a version with no branches */
     289    float t = m[0][0] + m[1][1] + m[2][2];
     290    if (t > 0)
     291    {
     292        w = 0.5f * sqrtf(1.0f + t);
     293        float s = 0.25f / w;
     294        x = s * (m[2][1] - m[1][2]);
     295        y = s * (m[0][2] - m[2][0]);
     296        z = s * (m[1][0] - m[0][1]);
     297    }
     298    else if (m[0][0] > m[1][1] && m[0][0] > m[2][2])
     299    {
     300        x = 0.5f * sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]);
     301        float s = 0.25f / x;
     302        y = s * (m[1][0] + m[0][1]);
     303        z = s * (m[0][2] + m[2][0]);
     304        w = s * (m[2][1] - m[1][2]);
     305    }
     306    else if (m[1][1] > m[2][2])
     307    {
     308        y = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
     309        float s = 0.25f / y;
     310        x = s * (m[1][0] + m[0][1]);
     311        z = s * (m[2][1] + m[1][2]);
     312        w = s * (m[0][2] - m[2][0]);
     313    }
     314    else
     315    {
     316        z = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
     317        float s = 0.25f / z;
     318        x = s * (m[0][2] + m[2][0]);
     319        y = s * (m[2][1] + m[1][2]);
     320        w = s * (m[1][0] - m[0][1]);
     321    }
    259322}
    260323
  • trunk/src/matrix.h

    r1047 r1048  
    2525{
    2626
     27#define VECTOR_TYPES(tname, suffix) \
     28    template <typename T> struct tname; \
     29    typedef tname<half> f16##suffix; \
     30    typedef tname<float> suffix; \
     31    typedef tname<int8_t> i8##suffix; \
     32    typedef tname<uint8_t> u8##suffix; \
     33    typedef tname<int16_t> i16##suffix; \
     34    typedef tname<uint16_t> u16##suffix; \
     35    typedef tname<int32_t> i##suffix; \
     36    typedef tname<uint32_t> u##suffix; \
     37    typedef tname<int64_t> i64##suffix; \
     38    typedef tname<uint64_t> u64##suffix;
     39
     40VECTOR_TYPES(Vec2, vec2)
     41VECTOR_TYPES(Vec3, vec3)
     42VECTOR_TYPES(Vec4, vec4)
     43VECTOR_TYPES(Quat, quat)
     44VECTOR_TYPES(Mat4, mat4)
     45
    2746#define VECTOR_OP(op) \
    2847    inline type_t operator op(type_t const &val) const \
     
    129148    inline T norm() const { return sqlen(); }
    130149
    131 #define OTHER_OPS(elems) \
     150#define OTHER_OPS(tname) \
    132151    VECTOR_OP(*) \
    133152    VECTOR_OP(/) \
     
    139158    \
    140159    template<typename U> \
    141     inline operator Vec##elems<U>() const \
    142     { \
    143         Vec##elems<U> ret; \
     160    inline operator tname<U>() const \
     161    { \
     162        tname<U> ret; \
    144163        for (size_t n = 0; n < sizeof(*this) / sizeof(T); n++) \
    145164            ret[n] = static_cast<U>((*this)[n]); \
     
    148167    \
    149168    template<typename U> \
    150     friend U dot(Vec##elems<U>, Vec##elems<U>);
     169    friend U dot(tname<U>, tname<U>);
    151170
    152171#define SWIZZLE2(e1, e2) \
     
    211230    SWIZZLE444(e1, x); SWIZZLE444(e1, y); SWIZZLE444(e1, z); SWIZZLE444(e1, w);
    212231
    213 template <typename T> struct Vec2;
    214 template <typename T> struct Vec3;
    215 template <typename T> struct Vec4;
    216 
    217232/*
    218233 * 2-element vectors
     
    228243
    229244    LINEAR_OPS()
    230     OTHER_OPS(2)
     245    OTHER_OPS(Vec2)
    231246
    232247    SWIZZLE22(x); SWIZZLE22(y);
     
    242257    union { T y; T b; T j; };
    243258};
    244 
    245 typedef Vec2<half> f16vec2;
    246 typedef Vec2<float> vec2;
    247 typedef Vec2<int8_t> i8vec2;
    248 typedef Vec2<uint8_t> u8vec2;
    249 typedef Vec2<int16_t> i16vec2;
    250 typedef Vec2<uint16_t> u16vec2;
    251 typedef Vec2<int32_t> ivec2;
    252 typedef Vec2<uint32_t> uvec2;
    253 typedef Vec2<int64_t> i64vec2;
    254 typedef Vec2<uint64_t> u64vec2;
    255259
    256260/*
     
    269273
    270274    LINEAR_OPS()
    271     OTHER_OPS(3)
     275    OTHER_OPS(Vec3)
    272276
    273277    SWIZZLE23(x); SWIZZLE23(y); SWIZZLE23(z);
     
    287291    union { T z; T c; T k; };
    288292};
    289 
    290 typedef Vec3<half> f16vec3;
    291 typedef Vec3<float> vec3;
    292 typedef Vec3<int8_t> i8vec3;
    293 typedef Vec3<uint8_t> u8vec3;
    294 typedef Vec3<int16_t> i16vec3;
    295 typedef Vec3<uint16_t> u16vec3;
    296 typedef Vec3<int32_t> ivec3;
    297 typedef Vec3<uint32_t> uvec3;
    298 typedef Vec3<int64_t> i64vec3;
    299 typedef Vec3<uint64_t> u64vec3;
    300293
    301294/*
     
    318311
    319312    LINEAR_OPS()
    320     OTHER_OPS(4)
     313    OTHER_OPS(Vec4)
    321314
    322315    SWIZZLE24(x); SWIZZLE24(y); SWIZZLE24(z); SWIZZLE24(w);
     
    335328};
    336329
    337 typedef Vec4<half> f16vec4;
    338 typedef Vec4<float> vec4;
    339 typedef Vec4<int8_t> i8vec4;
    340 typedef Vec4<uint8_t> u8vec4;
    341 typedef Vec4<int16_t> i16vec4;
    342 typedef Vec4<uint16_t> u16vec4;
    343 typedef Vec4<int32_t> ivec4;
    344 typedef Vec4<uint32_t> uvec4;
    345 typedef Vec4<int64_t> i64vec4;
    346 typedef Vec4<uint64_t> u64vec4;
    347 
    348330/*
    349331 * 4-element quaternions
     
    357339    inline Quat(T val) : x(0), y(0), z(0), w(val) { }
    358340    inline Quat(T _x, T _y, T _z, T _w) : x(_x), y(_y), z(_z), w(_w) { }
     341
     342    Quat(Mat4<T> const &m);
    359343
    360344    LINEAR_OPS()
     
    387371}
    388372
    389 typedef Quat<half> f16quat;
    390 typedef Quat<float> quat;
    391 typedef Quat<int8_t> i8quat;
    392 typedef Quat<uint8_t> u8quat;
    393 typedef Quat<int16_t> i16quat;
    394 typedef Quat<uint16_t> u16quat;
    395 typedef Quat<int32_t> iquat;
    396 typedef Quat<uint32_t> uquat;
    397 typedef Quat<int64_t> i64quat;
    398 typedef Quat<uint64_t> u64quat;
    399 
    400373/*
    401374 * Common operators for all vector types, including quaternions
     
    407380    { \
    408381        tname<U> ret; \
    409         for (unsigned int n = 0; n < sizeof(that) / sizeof(that[0]); n++) \
     382        for (size_t n = 0; n < sizeof(that) / sizeof(that[0]); n++) \
    410383            ret[n] = val op that[n]; \
    411384        return ret; \
     
    462435    static Mat4<T> rotate(T angle, T x, T y, T z);
    463436    static Mat4<T> rotate(T angle, Vec3<T> v);
     437    static Mat4<T> rotate(Quat<T> q);
    464438
    465439    static inline Mat4<T> translate(Mat4<T> mat, Vec3<T> v)
     
    551525};
    552526
    553 typedef Mat4<half> f16mat4;
    554 typedef Mat4<float> mat4;
    555 typedef Mat4<int8_t> i8mat4;
    556 typedef Mat4<uint8_t> u8mat4;
    557 typedef Mat4<int16_t> i16mat4;
    558 typedef Mat4<uint16_t> u16mat4;
    559 typedef Mat4<int32_t> imat4;
    560 typedef Mat4<uint32_t> umat4;
    561 typedef Mat4<int64_t> i64mat4;
    562 typedef Mat4<uint64_t> u64mat4;
    563 
    564527} /* namespace lol */
    565528
Note: See TracChangeset for help on using the changeset viewer.