geometry_simd.h

Go to the documentation of this file.

#ifndef LE_GEOMETRY_SIMD_H
#define LE_GEOMETRY_SIMD_H

#include "global.h"
#include "config.h"

#include "simd.h"
#include <math.h>
    
/*****************************************************************************/
struct __attribute__ ((aligned (16))) LeVertex
{
    union {
        V4SF vf;
        struct {float x, y, z, w;};
    };

    LeVertex()
    {
        V4SF vi = {0.0f, 0.0f, 0.0f, 1.0f};
        vf = vi;
    }

    LeVertex(float px, float py, float pz)
    {
        V4SF vi = {px, py, pz, 1.0f};
        vf = vi;
    }

    LeVertex(float px, float py, float pz, float pw)
    {
        V4SF vi = {px, py, pz, pw};
        vf = vi;
    }

/*****************************************************************************/
    static LeVertex spherical(float azi, float inc, float dist)
    {
        LeVertex r;
        r.x = cosf(azi) * cosf(inc) * dist;
        r.y = sinf(inc) * dist;
        r.z = -sinf(azi) * cosf(inc) * dist;
        return r;
    }

    LeVertex operator + (LeVertex v) const
    {
        LeVertex r;
        r.vf = vf + v.vf;
        return r;
    }

    LeVertex operator += (LeVertex v)
    {
        vf += v.vf;
        return *this;
    }

    LeVertex operator - (LeVertex v) const
    {
        LeVertex r;
        r.vf = vf - v.vf;
        return r;
    }

    LeVertex operator - () const
    {
        LeVertex r;
        r.vf = - vf;
        return r;
    }

    LeVertex operator -= (LeVertex v)
    {
        vf -= v.vf;
        return *this;
    }

    LeVertex operator * (LeVertex v) const
    {
        LeVertex r;
        r.vf = vf * v.vf;
        return r;
    }

    LeVertex operator / (LeVertex v) const
    {
        LeVertex r;
        r.vf = vf / v.vf;
        return r;
    }

    LeVertex operator * (float v) const
    {
        LeVertex r;
        V4SF vs = {v, v, v, v};
        r.vf = vf * vs;
        return r;
    }

    LeVertex operator / (float v) const
    {
        LeVertex r;
        V4SF vs = {v, v, v, v};
        r.vf = vf / vs;
        return r;
    }
    
    LeVertex operator *= (LeVertex v)
    {
        vf *= v.vf;
        return *this;
    }
    
    LeVertex operator /= (LeVertex v)
    {
        vf /= v.vf;
        return *this;
    }
    
    LeVertex operator *= (float v)
    {
        V4SF vs = {v, v, v, v};
        vf *= vs;
        return *this;
    }
    
    LeVertex operator /= (float v)
    {
        V4SF vs = {v, v, v, v};
        vf /= vs;
        return *this;
    }
    
    bool operator == (LeVertex v)
    {
        return x == v.x &&
               y == v.y &&
               z == v.z;
    }

    float dot(LeVertex v) const
    {
        LeVertex d;
        d.vf = vf * v.vf;
        return d.x + d.y + d.z;
    }

    LeVertex cross(LeVertex v) const
    {
        LeVertex c;
        c.x = y * v.z - v.y * z;
        c.y = -x * v.z + v.x * z;
        c.z = x * v.y - v.x * y;
        return c;
    }

    LeVertex sign() const
    {
        LeVertex c;
        c.x = copysignf(1.0f, x);
        c.y = copysignf(1.0f, y);
        c.z = copysignf(1.0f, z);
        c.w = copysignf(1.0f, w);
        return c;
    }

    LeVertex round() const
    {
        LeVertex c;
        c.x = floorf(x + 0.5f);
        c.y = floorf(y + 0.5f);
        c.z = floorf(z + 0.5f);
        c.w = floorf(w + 0.5f);
        return c;
    }

    LeVertex floor() const
    {
        LeVertex c;
        c.x = floorf(x);
        c.y = floorf(y);
        c.z = floorf(z);
        c.w = floorf(w);
        return c;
    }

    LeVertex normalize()
    {
        float d = norm();
        if (d == 0.0f) {
            V4SF vi = {0.0f, 0.0f, 0.0f, 1.0f};
            vf = vi;
        }else{
            d = 1.0f / d;
            V4SF vs = {d, d, d, d};
            vf *= vs;
        }
        return *this;
    }

    float norm() const
    {
        return sqrtf(dot(*this));
    }
};

/*****************************************************************************/
struct __attribute__ ((aligned (16))) LeAxis
{
    LeVertex origin;        
    LeVertex axis;      
    float norm;         
    LeAxis() :
        origin(),
        axis(LeVertex(0.0f, 0.0f, 1.0f)),
        norm(1.0f)      
    {
    }

    LeAxis(const LeVertex & v1, const LeVertex & v2) :
        origin(v1)
    {
        axis = (v2 - v1).normalize();
        norm = (v2 - v1).norm();
    }
};

/*****************************************************************************/
struct __attribute__ ((aligned (16))) LePlane
{
    LeAxis xAxis;
    LeAxis yAxis;
    LeAxis zAxis;

    LePlane()
    {
        xAxis.axis = LeVertex(1.0f, 0.0f, 0.0f);
        yAxis.axis = LeVertex(0.0f, 1.0f, 0.0f);
        zAxis.axis = LeVertex(0.0f, 0.0f, 1.0f);
    }

    LePlane(const LeVertex & v1, const LeVertex & v2, const LeVertex & v3) :
        xAxis(LeAxis(v1, v2)), yAxis(LeAxis(v1, v3))
    {
        zAxis = LeAxis(v1, v1 + xAxis.axis.cross(yAxis.axis));
    }
};

/*****************************************************************************/
struct __attribute__ ((aligned (16))) LeMatrix
{
    LeVertex lines[4];

    LeMatrix()
    {
        identity();
    }

    void identity()
    {
        lines[0] = LeVertex(1.0f, 0.0f, 0.0f, 0.0f);
        lines[1] = LeVertex(0.0f, 1.0f, 0.0f, 0.0f);
        lines[2] = LeVertex(0.0f, 0.0f, 1.0f, 0.0f);
        lines[3] = LeVertex(0.0f, 0.0f, 0.0f, 1.0f);
    }

    void zero()
    {
        LeVertex zv = LeVertex(0.0f, 0.0f, 0.0f, 0.0f);
        lines[0] = zv;
        lines[1] = zv;
        lines[2] = zv;
        lines[3] = zv;
    }

    void transpose()
    {
        LeMatrix m;
        m.lines[0] = LeVertex(lines[0].x, lines[1].x, lines[2].x, lines[3].x);
        m.lines[1] = LeVertex(lines[0].x, lines[1].x, lines[2].x, lines[3].x);
        m.lines[2] = LeVertex(lines[0].x, lines[1].x, lines[2].x, lines[3].x);
        m.lines[3] = LeVertex(lines[0].x, lines[1].x, lines[2].x, lines[3].x);
        *this = m;
    }

    void translate(LeVertex d)
    {
        lines[0].w += d.x;
        lines[1].w += d.y;
        lines[2].w += d.z;
    }

    void scale(LeVertex s)
    {
        LeMatrix m;
        m.lines[0].x *= s.x;
        m.lines[1].y *= s.y;
        m.lines[2].z *= s.z;
        *this = m * *this;
    }

/*****************************************************************************/
    void rotateEulerXYZ(LeVertex a)
    {
        rotateX(a.x);
        rotateY(a.y);
        rotateZ(a.z);
    }

    void rotateEulerXZY(LeVertex a)
    {
        rotateX(a.x);
        rotateZ(a.z);
        rotateY(a.y);
    }

    void rotateEulerYZX(LeVertex a)
    {
        rotateY(a.y);
        rotateZ(a.z);
        rotateX(a.x);
    }

    void rotateEulerYXZ(LeVertex a)
    {
        rotateY(a.y);
        rotateX(a.x);
        rotateZ(a.z);
    }

    void rotateEulerZXY(LeVertex a)
    {
        rotateZ(a.z);
        rotateX(a.x);
        rotateY(a.y);
    }

    void rotateEulerZYX(LeVertex a)
    {
        rotateZ(a.z);
        rotateY(a.y);
        rotateX(a.x);
    }

    void rotateX(float a)
    {
        float c = cosf(a);
        float s = sinf(a);

        LeMatrix m;
        m.lines[1].y = c;
        m.lines[1].z = -s;
        m.lines[2].y = s;
        m.lines[2].z = c;

        *this = m * *this;
    }

    void rotateY(float a)
    {
        float c = cosf(a);
        float s = sinf(a);
        
        LeMatrix m;
        m.lines[0].x = c;
        m.lines[0].z = s;
        m.lines[2].x = -s;
        m.lines[2].z = c;

        *this = m * *this;
    }

    void rotateZ(float a)
    {
        float c = cosf(a);
        float s = sinf(a);

        LeMatrix m;
        m.lines[0].x = c;
        m.lines[0].y = -s;
        m.lines[1].x = s;
        m.lines[1].y = c;

        *this = m * *this;
    }

    void rotate(LeVertex axis, float angle)
    {
        float c = cosf(angle);
        float s = sinf(angle);
        float d = 1.0f - c;

        LeMatrix m;
        m.lines[0] = LeVertex(c + axis.x * axis.x * d, axis.x * axis.y * d - axis.z * s, axis.x * axis.z * d + axis.y * s, 0.0f);
        m.lines[1] = LeVertex(axis.y * axis.x * d + axis.z * s, c + axis.y * axis.y * d, axis.y * axis.z * d - axis.x * s, 0.0f);
        m.lines[2] = LeVertex(axis.z * axis.x * d - axis.y * s, axis.z * axis.y * d + axis.x * s, c + axis.z * axis.z * d, 0.0f);
        m.lines[3] = LeVertex(0.0f, 0.0f, 0.0f, 1.0f);

        *this = m * *this;
    }

/*****************************************************************************/
    void toEulerZYX(LeVertex & angle)
    {
        float l = sqrtf(lines[0].x * lines[0].x + lines[1].x * lines[1].x);
        if (l > 1e-6) {
            angle.x = atan2f(lines[2].y, lines[2].z);
            angle.y = atan2f(-lines[2].x, l);
            angle.z = atan2f(lines[1].x, lines[0].x);
        }
        else {
            angle.x = atan2f(-lines[1].z, lines[1].y);
            angle.y = atan2f(-lines[2].x, l);
            angle.z = 0.0f;
        }
    }

/*****************************************************************************/
    void alignBackUp(LeVertex back, LeVertex up)
    {
        back.normalize();
        up.normalize();
        LeVertex right = up.cross(back);

        LeMatrix m;
        m.lines[0] = LeVertex(right.x, up.x, back.x, 0.0f);
        m.lines[1] = LeVertex(right.y, up.y, back.y, 0.0f);
        m.lines[2] = LeVertex(right.z, up.z, back.z, 0.0f);
        m.lines[3] = LeVertex(0.0f, 0.0f, 0.0f, 1.0f);

        *this = m * *this;
    }

    void alignBackRight(LeVertex back, LeVertex right)
    {
        back.normalize();
        right.normalize();
        LeVertex up = back.cross(right);

        LeMatrix m;
        m.lines[0] = LeVertex(right.x, up.x, back.x, 0.0f);
        m.lines[1] = LeVertex(right.y, up.y, back.y, 0.0f);
        m.lines[2] = LeVertex(right.z, up.z, back.z, 0.0f);
        m.lines[3] = LeVertex(0.0f, 0.0f, 0.0f, 1.0f);

        *this = m * *this;
    }

/*****************************************************************************/
    LeMatrix inverse3x3()
    {
        float d = lines[0].x*(lines[1].y*lines[2].z-lines[2].y*lines[1].z)
                - lines[0].y*(lines[1].x*lines[2].z-lines[1].z*lines[2].x)
                + lines[0].z*(lines[1].x*lines[2].y-lines[1].y*lines[2].x);

        LeMatrix m;
        if (d == 0.0f) {m.zero(); return m;}
        d = 1.0f / d;

        m.lines[0].x =  (lines[1].y * lines[2].z - lines[2].y * lines[1].z) * d;
        m.lines[0].y = -(lines[0].y * lines[2].z - lines[2].y * lines[0].z) * d;
        m.lines[0].z =  (lines[0].y * lines[1].z - lines[1].y * lines[0].z) * d;

        m.lines[1].x = -(lines[1].x * lines[2].z - lines[2].x * lines[1].z) * d;
        m.lines[1].y =  (lines[0].x * lines[2].z - lines[2].x * lines[0].z) * d;
        m.lines[1].z = -(lines[0].x * lines[1].z - lines[1].x * lines[0].z) * d;

        m.lines[2].x =  (lines[1].x * lines[2].y - lines[2].x * lines[1].y) * d;
        m.lines[2].y = -(lines[0].x * lines[2].y - lines[2].x * lines[0].y) * d;
        m.lines[2].z =  (lines[0].x * lines[1].y - lines[1].x * lines[0].y) * d;

        return m;
    }

    LeVertex operator * (LeVertex v) const
    {
        v.w = 1.0f;
        LeVertex l1 = lines[0] * v;
        LeVertex l2 = lines[1] * v;
        LeVertex l3 = lines[2] * v;

        return LeVertex(l1.x + l1.y + l1.z + l1.w,
                      l2.x + l2.y + l2.z + l2.w,
                      l3.x + l3.y + l3.z + l3.w);
    }

    LeMatrix operator + (LeMatrix m) const
    {
        LeMatrix r;
        r.lines[0] = lines[0] + m.lines[0];
        r.lines[1] = lines[1] + m.lines[1];
        r.lines[2] = lines[2] + m.lines[2];
        r.lines[3] = lines[3] + m.lines[3];

        return r;
    }

    LeMatrix operator * (LeMatrix m) const
    {
        LeMatrix r;
        for (int i = 0; i < 4; i++) {
            LeVertex vx = LeVertex(lines[i].x, lines[i].x, lines[i].x, lines[i].x);
            LeVertex vy = LeVertex(lines[i].y, lines[i].y, lines[i].y, lines[i].y);
            LeVertex vz = LeVertex(lines[i].z, lines[i].z, lines[i].z, lines[i].z);
            LeVertex vw = LeVertex(lines[i].w, lines[i].w, lines[i].w, lines[i].w);
            r.lines[i] = vx * m.lines[0] + vy * m.lines[1] + vz * m.lines[2] + vw * m.lines[3];
        }

        return r;
    }
};

/*****************************************************************************/
namespace LePrimitives {
    const LeVertex up    = LeVertex(0.0f, 1.0f, 0.0f);
    const LeVertex down  = LeVertex(0.0f, -1.0f, 0.0f);
    const LeVertex front = LeVertex(0.0f, 0.0f, -1.0f);
    const LeVertex back  = LeVertex(0.0f, 0.0f, 1.0f);
    const LeVertex left  = LeVertex(-1.0f, 0.0f, 0.0f);
    const LeVertex right = LeVertex(1.0f, 0.0f, 0.0f);
    const LeVertex zero  = LeVertex(0.0f, 0.0f, 0.0f);
}

#endif  //LE_GEOMETRY_SIMD_H