#include <amsculib2/amsculib2.hpp>

namespace amscuda
{

    __host__ __device__ cuvect3f::cuvect3f()
    {
        x = 0.0; y = 0.0; z = 0.0;
        return;
    }

    __host__ __device__ cuvect3f::~cuvect3f()
    {
        x = 0.0; y = 0.0; z = 0.0;
        return;
    }

    __host__ __device__ float& cuvect3f::operator[](const int &I)
    {
        if(I==0) return x;
        if(I==1) return y;
        if(I==2) return z;
        return x;
    }

    __host__ __device__ const float& cuvect3f::operator[](const int &I) const
    {
        if(I==0) return x;
        if(I==1) return y;
        if(I==2) return z;
        return x;
    }

    __host__ __device__ cuvect3f cuvect3f::operator+(const cuvect3f &rhs)
    {
        cuvect3f ret;
        ret.x = x+rhs.x;
        ret.y = y+rhs.y;
        ret.z = z+rhs.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator-(const cuvect3f &rhs)
    {
        cuvect3f ret;
        ret.x = x-rhs.x;
        ret.y = y-rhs.y;
        ret.z = z-rhs.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator*(const float &rhs)
    {
        cuvect3f ret;
        ret.x = x*rhs;
        ret.y = y*rhs;
        ret.z = z*rhs;
        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f::operator/(const float &rhs)
    {
        cuvect3f ret;
        ret.x = x/rhs;
        ret.y = y/rhs;
        ret.z = z/rhs;
        return ret;
    }

    __host__ __device__ cuvect3f& cuvect3f::operator+=(const cuvect3f &rhs)
    {
        x = x + rhs.x;
        y = y + rhs.y;
        z = z + rhs.z;
        return *this;
    }

    __host__ __device__ cuvect3f& cuvect3f::operator-=(const cuvect3f &rhs)
    {
        x = x - rhs.x;
        y = y - rhs.y;
        z = z - rhs.z;
        return *this;
    }

    __host__ __device__ cuvect3f& cuvect3f::operator*=(const float &rhs)
    {
        x = x * rhs;
        y = y * rhs;
        z = z * rhs;
        return *this;
    }

    __host__ __device__ cuvect3f& cuvect3f::operator/=(const float &rhs)
    {
        x = x / rhs;
        y = y / rhs;
        z = z / rhs;
        return *this;
    }


    __host__ __device__ cuvect3f::cuvect3f(const float &_x, const float &_y, const float &_z)
    {
        x = _x; y = _y; z = _z;
        return;
    }


    __host__ __device__ float cuvect3f_dot(const cuvect3f &a, const cuvect3f &b)
    {
        float ret = a.x*b.x+a.y*b.y+a.z*b.z;

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_cross(const cuvect3f &a, const cuvect3f &b)
    {
        cuvect3f ret;
        ret[0] = a[1]*b[2]-a[2]*b[1];
        ret[1] = a[2]*b[0]-a[0]*b[2];
        ret[2] = a[0]*b[1]-a[1]*b[0];

        return ret;
    }

    __host__ __device__ float cuvect3f_norm(const cuvect3f &a)
    {
        float ret;
        ret = ::sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_normalize(const cuvect3f &a)
    {
        cuvect3f ret;
        float m;
        m = ::sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
        if(m>0.0)
        {
            ret.x = a.x/m; ret.y = a.y/m; ret.z = a.z/m;
        }
        else
        {
            ret.x = 0.0; ret.y = 0.0; ret.z = 0.0;
        }

        return ret;
    }

    __host__ __device__ cuvect3f cuvect3f_proj(const cuvect3f &a, const cuvect3f &b)
    {
        cuvect3f ret;
        cuvect3f bn = cuvect3f_normalize(b);
        float m = cuvect3f_dot(a,bn);
        ret = bn*m;
        return ret;
    }




__host__ __device__ cumat3f::cumat3f()
{
    m00 = 0.0f;
    m01 = 0.0f;
    m02 = 0.0f;
    m10 = 0.0f;
    m11 = 0.0f;
    m12 = 0.0f;
    m20 = 0.0f;
    m21 = 0.0f;
    m22 = 0.0f;

    return;
}

__host__ __device__ cumat3f::~cumat3f()
{
    m00 = 0.0f;
    m01 = 0.0f;
    m02 = 0.0f;
    m10 = 0.0f;
    m11 = 0.0f;
    m12 = 0.0f;
    m20 = 0.0f;
    m21 = 0.0f;
    m22 = 0.0f;
    return;
}

__host__ __device__ float& cumat3f::operator[](const int &I)
{
    if(I==0) return m00;
    if(I==1) return m10;
    if(I==2) return m20;
    if(I==3) return m01;
    if(I==4) return m11;
    if(I==5) return m21;
    if(I==6) return m02;
    if(I==7) return m12;
    if(I==8) return m22;
    
    return m00;
}

__host__ __device__ float& cumat3f::operator()(const int &I, const int &J)
{
    if(I==0 && J==0) return m00;
    if(I==1 && J==0) return m10;
    if(I==2 && J==0) return m20;
    if(I==0 && J==1) return m01;
    if(I==1 && J==1) return m11;
    if(I==2 && J==1) return m21;
    if(I==0 && J==2) return m02;
    if(I==1 && J==2) return m12;
    if(I==2 && J==2) return m22;

    return m00;
}


__host__ __device__ float& cumat3f::at(const int &I, const int &J)
{
    if(I==0 && J==0) return m00;
    if(I==1 && J==0) return m10;
    if(I==2 && J==0) return m20;
    if(I==0 && J==1) return m01;
    if(I==1 && J==1) return m11;
    if(I==2 && J==1) return m21;
    if(I==0 && J==2) return m02;
    if(I==1 && J==2) return m12;
    if(I==2 && J==2) return m22;

    return m00;
}

__host__ __device__ const float& cumat3f::operator[](const int &I) const 
{
    if(I==0) return m00;
    if(I==1) return m10;
    if(I==2) return m20;
    if(I==3) return m01;
    if(I==4) return m11;
    if(I==5) return m21;
    if(I==6) return m02;
    if(I==7) return m12;
    if(I==8) return m22;
    
    return m00;
}

__host__ __device__ const float& cumat3f::operator()(const int &I, const int &J) const 
{
    if(I==0 && J==0) return m00;
    if(I==1 && J==0) return m10;
    if(I==2 && J==0) return m20;
    if(I==0 && J==1) return m01;
    if(I==1 && J==1) return m11;
    if(I==2 && J==1) return m21;
    if(I==0 && J==2) return m02;
    if(I==1 && J==2) return m12;
    if(I==2 && J==2) return m22;

    return m00;
}


__host__ __device__ const float& cumat3f::at(const int &I, const int &J) const 
{
    if(I==0 && J==0) return m00;
    if(I==1 && J==0) return m10;
    if(I==2 && J==0) return m20;
    if(I==0 && J==1) return m01;
    if(I==1 && J==1) return m11;
    if(I==2 && J==1) return m21;
    if(I==0 && J==2) return m02;
    if(I==1 && J==2) return m12;
    if(I==2 && J==2) return m22;

    return m00;
}


__host__ __device__ cumat3f cumat3f::operator+(const cumat3f &rhs)
{
    cumat3f ret;
    ret.m00 = m00 + rhs.m00;
    ret.m10 = m10 + rhs.m10;
    ret.m20 = m20 + rhs.m20;
    ret.m01 = m01 + rhs.m01;
    ret.m11 = m11 + rhs.m11;
    ret.m21 = m21 + rhs.m21;
    ret.m02 = m02 + rhs.m02;
    ret.m12 = m12 + rhs.m12;
    ret.m22 = m22 + rhs.m22;
    

    return ret;
}

__host__ __device__ cumat3f cumat3f::operator-(const cumat3f &rhs)
{
    cumat3f ret;
    ret.m00 = m00 - rhs.m00;
    ret.m10 = m10 - rhs.m10;
    ret.m20 = m20 - rhs.m20;
    ret.m01 = m01 - rhs.m01;
    ret.m11 = m11 - rhs.m11;
    ret.m21 = m21 - rhs.m21;
    ret.m02 = m02 - rhs.m02;
    ret.m12 = m12 - rhs.m12;
    ret.m22 = m22 - rhs.m22;
    return ret;
}

__host__ __device__ cumat3f cumat3f::operator*(const float &rhs)
{
    cumat3f ret;
    ret.m00 = m00 * rhs;
    ret.m10 = m10 * rhs;
    ret.m20 = m20 * rhs;
    ret.m01 = m01 * rhs;
    ret.m11 = m11 * rhs;
    ret.m21 = m21 * rhs;
    ret.m02 = m02 * rhs;
    ret.m12 = m12 * rhs;
    ret.m22 = m22 * rhs;
    return ret;
}

__host__ __device__ cumat3f cumat3f::operator/(const float &rhs)
{
    cumat3f ret;
    ret.m00 = m00 / rhs;
    ret.m10 = m10 / rhs;
    ret.m20 = m20 / rhs;
    ret.m01 = m01 / rhs;
    ret.m11 = m11 / rhs;
    ret.m21 = m21 / rhs;
    ret.m02 = m02 / rhs;
    ret.m12 = m12 / rhs;
    ret.m22 = m22 / rhs;
    return ret;
}
        
__host__ __device__ cuvect3f cumat3f::operator*(const cuvect3f &rhs)
{
    cuvect3f ret;
    
    ret.x = m00*rhs.x + m01*rhs.y + m02*rhs.z;
    ret.y = m10*rhs.x + m11*rhs.y + m12*rhs.z;
    ret.z = m20*rhs.x + m21*rhs.y + m22*rhs.z;

    return ret;
}

__host__ __device__ cumat3f cumat3f::operator*(const cumat3f &rhs)
{
    cumat3f ret;

    ret.m00 = m00*rhs.m00 + m01*rhs.m10 + m02*rhs.m20;
    ret.m01 = m00*rhs.m01 + m01*rhs.m11 + m02*rhs.m21;
    ret.m02 = m00*rhs.m02 + m01*rhs.m12 + m02*rhs.m22;
    ret.m10 = m10*rhs.m00 + m11*rhs.m10 + m12*rhs.m20;
    ret.m11 = m10*rhs.m01 + m11*rhs.m11 + m12*rhs.m21;
    ret.m12 = m10*rhs.m02 + m11*rhs.m12 + m12*rhs.m22;
    ret.m20 = m20*rhs.m00 + m21*rhs.m10 + m22*rhs.m20;
    ret.m21 = m20*rhs.m01 + m21*rhs.m11 + m22*rhs.m21;
    ret.m22 = m20*rhs.m02 + m21*rhs.m12 + m22*rhs.m22;

    return ret;
}

__host__ __device__ float cumat3f::det()
{
    float ret = 0.0;

    ret += m00*m11*m22;
    ret += m01*m12*m20;
    ret += m02*m10*m21;
    ret -= m20*m11*m02;
    ret -= m21*m12*m00;
    ret -= m22*m10*m01;

    return ret;
}

__host__ __device__ cumat3f cumat3f::transpose()
{
    cumat3f ret;

    ret.m00 = m00;
    ret.m01 = m10;
    ret.m02 = m20;
    ret.m10 = m01;
    ret.m11 = m11;
    ret.m12 = m21;
    ret.m20 = m02;
    ret.m21 = m12;
    ret.m22 = m22;

    return ret;
}

__host__ __device__ cumat3f cumat3f::inverse()
{
    cumat3f q;
    float dt = det();
    if(dt!=0.0)
    {
        q(0,0) =  (at(1,1)*at(2,2)-at(1,2)*at(2,1))/dt;
        q(1,0) = -(at(1,0)*at(2,2)-at(1,2)*at(2,0))/dt;
        q(2,0) =  (at(1,0)*at(2,1)-at(1,1)*at(2,0))/dt;
        q(0,1) = -(at(0,1)*at(2,2)-at(0,2)*at(2,1))/dt;
        q(1,1) =  (at(0,0)*at(2,2)-at(0,2)*at(2,0))/dt;
        q(2,1) = -(at(0,0)*at(2,1)-at(0,1)*at(2,0))/dt;
        q(0,2) =  (at(0,1)*at(1,2)-at(0,2)*at(1,1))/dt;
        q(1,2) = -(at(0,0)*at(1,2)-at(0,2)*at(1,0))/dt;
        q(2,2) =  (at(0,0)*at(1,1)-at(0,1)*at(1,0))/dt;
        // q(0,0) =  (at(1,1)*at(2,2)-at(1,2)*at(2,1))/dt;
        // q(0,1) = -(at(1,0)*at(2,2)-at(1,2)*at(2,0))/dt;
        // q(0,2) =  (at(1,0)*at(2,1)-at(1,1)*at(2,0))/dt;
        // q(1,0) = -(at(0,1)*at(2,2)-at(0,2)*at(2,1))/dt;
        // q(1,1) =  (at(0,0)*at(2,2)-at(0,2)*at(2,0))/dt;
        // q(1,2) = -(at(0,0)*at(2,1)-at(0,1)*at(2,0))/dt;
        // q(2,0) =  (at(0,1)*at(1,2)-at(0,2)*at(1,1))/dt;
        // q(2,1) = -(at(0,0)*at(1,2)-at(0,2)*at(1,0))/dt;
        // q(2,2) =  (at(0,0)*at(1,1)-at(0,1)*at(1,0))/dt;
        // q = q.transpose();
    }
    else
    {
        q(0,0) = inf;
        q(0,1) = inf;
        q(0,2) = inf;
        q(1,0) = inf;
        q(1,1) = inf;
        q(1,2) = inf;
        q(2,0) = inf;
        q(2,1) = inf;
        q(2,2) = inf;
    }

    return q;
}

__host__ __device__ cumat3f operator-(const cumat3f &rhs)
{
    cumat3f ret;
    ret.m00 = -rhs.m00;
    ret.m10 = -rhs.m10;
    ret.m20 = -rhs.m20;
    ret.m01 = -rhs.m01;
    ret.m11 = -rhs.m11;
    ret.m21 = -rhs.m21;
    ret.m02 = -rhs.m02;
    ret.m12 = -rhs.m12;
    ret.m22 = -rhs.m22;

    return ret;
}

__host__ __device__ cumat3f& cumat3f::operator+=(const cumat3f &rhs)
{
    m00 += rhs.m00;
    m10 += rhs.m10;
    m20 += rhs.m20;
    m01 += rhs.m01;
    m11 += rhs.m11;
    m21 += rhs.m21;
    m02 += rhs.m02;
    m12 += rhs.m12;
    m22 += rhs.m22;

    return *this;
}

__host__ __device__ cumat3f& cumat3f::operator-=(const cumat3f &rhs)
{
    m00 -= rhs.m00;
    m10 -= rhs.m10;
    m20 -= rhs.m20;
    m01 -= rhs.m01;
    m11 -= rhs.m11;
    m21 -= rhs.m21;
    m02 -= rhs.m02;
    m12 -= rhs.m12;
    m22 -= rhs.m22;

    return *this;
}



__host__ __device__ cumat3f& cumat3f::operator/=(const float &rhs)
{
    m00 /= rhs;
    m10 /= rhs;
    m20 /= rhs;
    m01 /= rhs;
    m11 /= rhs;
    m21 /= rhs;
    m02 /= rhs;
    m12 /= rhs;
    m22 /= rhs;

    return *this;
}

__host__ __device__ cumat3f& cumat3f::operator*=(const float &rhs)
{
    m00 *= rhs;
    m10 *= rhs;
    m20 *= rhs;
    m01 *= rhs;
    m11 *= rhs;
    m21 *= rhs;
    m02 *= rhs;
    m12 *= rhs;
    m22 *= rhs;

    return *this;
}

__host__ __device__ cumat3f& cumat3f::operator*=(const cumat3f &rhs)
{
    cumat3f tmp;

    tmp.m00 = m00*rhs.m00 + m01*rhs.m10 + m02*rhs.m20;
    tmp.m01 = m00*rhs.m01 + m01*rhs.m11 + m02*rhs.m21;
    tmp.m02 = m00*rhs.m02 + m01*rhs.m12 + m02*rhs.m22;
    tmp.m10 = m10*rhs.m00 + m11*rhs.m10 + m12*rhs.m20;
    tmp.m11 = m10*rhs.m01 + m11*rhs.m11 + m12*rhs.m21;
    tmp.m12 = m10*rhs.m02 + m11*rhs.m12 + m12*rhs.m22;
    tmp.m20 = m20*rhs.m00 + m21*rhs.m10 + m22*rhs.m20;
    tmp.m21 = m20*rhs.m01 + m21*rhs.m11 + m22*rhs.m21;
    tmp.m22 = m20*rhs.m02 + m21*rhs.m12 + m22*rhs.m22;

    (*this) = tmp;

    return *this;
}

__host__ __device__ cumat3f::cumat3f(
    const float & _m00, const float & _m01, const float & _m02,
    const float & _m10, const float & _m11, const float & _m12,
    const float & _m20, const float & _m21, const float & _m22
)
{
    m00 = _m00;
    m01 = _m01;
    m02 = _m02;
    m10 = _m10;
    m11 = _m11;
    m12 = _m12;
    m20 = _m20;
    m21 = _m21;
    m22 = _m22;
}

__host__ __device__ float* cumat3f::data()
{
     //pointer to float[9] representation of matrix
    return (float*) this;
}

__host__ __device__ const float* cumat3f::data() const
{
     //pointer to float[9] representation of matrix
    return (const float*) this;
}

//transposes a 3x3 (9 element) matrix
__host__ __device__ void mat3f_transpose(float *mat3inout)
{
    int I,J;
    float matint[9];
    for(I=0;I<9;I++)
    {
        matint[I] = mat3inout[I];
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            mat3inout[I+J*3] = matint[J+I*3];
        }
    }

    return;
}

//copies src to dest
__host__ __device__ void mat3f_copy(float *mat3f_dest, const float *mat3f_src)
{
    int I;
    if(mat3f_dest==NULL || mat3f_src==NULL)
        return;

    for(I=0;I<9;I++)
        mat3f_dest[I] = mat3f_src[I];

    return;
}


__host__ __device__ float mat3f_det(float *mat3in)
{
    float ret = 0.0;

    ret = ret + mat3in[0+0*3]*mat3in[1+1*3]*mat3in[2+2*3];
    ret = ret + mat3in[0+1*3]*mat3in[1+2*3]*mat3in[2+0*3];
    ret = ret + mat3in[0+2*3]*mat3in[1+0*3]*mat3in[2+1*3];
    ret = ret - mat3in[0+0*3]*mat3in[1+2*3]*mat3in[2+1*3];
    ret = ret - mat3in[0+1*3]*mat3in[1+0*3]*mat3in[2+2*3];
    ret = ret - mat3in[0+2*3]*mat3in[1+1*3]*mat3in[2+0*3];

    return ret;
}

//inverts a 3x3 (9 element) matrix
__host__ __device__ void mat3f_inverse(float *mat3inout)
{
    int I;
    float matint[9];
    float det = mat3f_det(mat3inout);

    for(I=0;I<9;I++)
    {
        matint[I] = mat3inout[I];
    }

    mat3inout[0+0*3] =  (matint[1+1*3]*matint[2+2*3]-matint[1+2*3]*matint[2+1*3])/det;
    mat3inout[0+1*3] = -(matint[1+0*3]*matint[2+2*3]-matint[1+2*3]*matint[2+0*3])/det;
    mat3inout[0+2*3] =  (matint[1+0*3]*matint[2+1*3]-matint[1+1*3]*matint[2+0*3])/det;
    mat3inout[1+0*3] = -(matint[0+1*3]*matint[2+2*3]-matint[0+2*3]*matint[2+1*3])/det;
    mat3inout[1+1*3] =  (matint[0+0*3]*matint[2+2*3]-matint[0+2*3]*matint[2+0*3])/det;
    mat3inout[1+2*3] = -(matint[0+0*3]*matint[2+1*3]-matint[0+1*3]*matint[2+0*3])/det;
    mat3inout[2+0*3] =  (matint[0+1*3]*matint[1+2*3]-matint[0+2*3]*matint[1+1*3])/det;
    mat3inout[2+1*3] = -(matint[0+0*3]*matint[1+2*3]-matint[0+2*3]*matint[1+0*3])/det;
    mat3inout[2+2*3] =  (matint[0+0*3]*matint[1+1*3]-matint[0+1*3]*matint[1+0*3])/det;

    mat3f_transpose(mat3inout);

    return;
}

__host__ __device__ cuvect3f mat3f_mult(float *mat3in, const cuvect3f &cvin)
{
    int I,J;
    cuvect3f ret;
    for(I=0;I<3;I++)
    {
        ret[I] = 0.0;
        for(J=0;J<3;J++)
        {
            ret[I] = ret[I] + mat3in[I+3*J]*cvin[J];
        }
    }

    return ret;
}

__host__ __device__ void mat3f_mult(float *matina, float *matinb, float *matout)
{
    float wrk[9];
    int I,J,K;

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            wrk[I+3*J] = 0.0;
        }
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            for(K=0;K<3;K++)
            {
                wrk[I+3*K] = wrk[I+3*K] + matina[I+3*J]*matinb[J+3*K];
            }
        }
    }

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            matout[I+3*J] = wrk[I+3*J];
        }
    }

    return;
}

__host__ __device__ void mat3f_hodgedual(const cuvect3f &vecin, float *matout)
{
    matout[0 + 0*3] = 0.0f;
    matout[1 + 0*3] = -vecin[2];
    matout[2 + 0*3] = vecin[1];

    matout[0 + 1*3] = vecin[2];
    matout[1 + 1*3] = 0.0f;
    matout[2 + 1*3] = -vecin[0];

    matout[0 + 2*3] = -vecin[1];
    matout[1 + 2*3] = vecin[0];
    matout[2 + 2*3] = 0.0f;
    return;
}

__host__ __device__ void mat3f_hodgedual(float *matin, cuvect3f &vecout)
{
    vecout[0] = 0.5*(matin[1 + 2*3] - matin[2 + 1*3]);
    vecout[1] = 0.5*(matin[2 + 0*3] - matin[0 + 2*3]);
    vecout[2] = 0.5*(matin[0 + 1*3] - matin[1 + 0*3]);
    
    return;
}

//returns direction cosine rotation matrix from axis and angle
__host__ __device__ void mat3f_rot_from_axisangle(cuvect3f axis, float angle, float *matout)
{
    int I;
    float H[9];
    float Hsq[9];
    float II[9];

    for(I=0;I<9;I++) II[I] = 0.0;
    II[0+0*3] = 1.0;
    II[1+1*3] = 1.0;
    II[2+2*3] = 1.0;

    axis = cuvect3f_normalize(axis);
    
    mat3f_hodgedual(axis,H);
    mat3f_mult(H,H,Hsq);

    for(I=0;I<9;I++)
    {
        matout[I] = (II[I] + Hsq[I]) + H[I]*sinf(angle) - Hsq[I]*cosf(angle);
    }

    return;
}



__host__ void test_cudavectf_logic1()
{
    //3 dim vector and matrix functional tests on host side

    printf("3 dim vector and matrix functional tests on host side\n");
    
    cuvect3f a,b,c;
    float ma[9],mb[9],mc[9];

    int I,J;

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            ma[I+3*J] = ((float) rand())/((float) RAND_MAX);
            mb[I+3*J] = ma[I+3*J];
        }
    }

    mat3f_inverse(mb);
    mat3f_mult(ma,mb,mc);

    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("ma[%d,%d] = %1.3f\n",I,J,ma[I+3*J]);
        }
    }
    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("mb[%d,%d] = %1.3f\n",I,J,mb[I+3*J]);
        }
    }
    for(I=0;I<3;I++)
    {
        for(J=0;J<3;J++)
        {
            printf("mc[%d,%d] = %1.3f\n",I,J,mc[I+3*J]);
        }
    }

    a = cuvect3f(1,1,1);
    b = mat3f_mult(ma,a);
    b = mat3f_mult(mb,b);
    
    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f\n",I,a[I],I,b[I]);
    }

    a = cuvect3f(1,0,1);
    b = cuvect3f(0,1,-1);
    c = a+b;

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    c = c/2.0;

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    c = cuvect3f_cross(a,b);

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }

    printf("c dot a = %1.3f, c dot b = %1.3f\n",cuvect3f_dot(c,a),cuvect3f_dot(c,b));

    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3f_norm(a),cuvect3f_norm(b),cuvect3f_norm(c));
    a = cuvect3f_normalize(a);
    b = cuvect3f_normalize(b);
    c = cuvect3f_normalize(c);

    for(I=0;I<3;I++)
    {
        printf("a[%d] = %1.3f, b[%d] = %1.3f, c[%d] = %1.3f\n",I,a[I],I,b[I],I,c[I]);
    }
    printf("c dot a = %1.3f, c dot b = %1.3f\n",cuvect3f_dot(c,a),cuvect3f_dot(c,b));
    printf("norm(a)=%1.3f, norm(b)=%1.3f, norm(c)=%1.3f\n",cuvect3f_norm(a),cuvect3f_norm(b),cuvect3f_norm(c));

    return;
}

__host__ __device__ cumat3f hodge_dual(const cuvect3f &vin)
{
    cumat3f ret;

    ret.m00 = 0.0f;
    ret.m11 = 0.0f;
    ret.m22 = 0.0f;

    ret.m01 = vin.z;
    ret.m12 = vin.x;
    ret.m20 = vin.y;

    ret.m10 = -vin.z;
    ret.m21 = -vin.x;
    ret.m02 = -vin.y;

    return ret;
}

__host__ __device__ cuvect3f hodge_dual(const cumat3f &min)
{
    cuvect3f ret;

    ret.x = 0.5f*(min.m12 - min.m21);
    ret.y = 0.5f*(min.m20 - min.m02);
    ret.z = 0.5f*(min.m01 - min.m10);

    return ret;
}

__host__ __device__ const cumat3f cumat3f_eye()
{
    cumat3f ret;
    ret.m00 = 1.0f;
    ret.m11 = 1.0f;
    ret.m22 = 1.0f;
    
    return ret;
}

__host__ __device__ const cumat3f cumat3f_zeros()
{
    cumat3f ret;
    return ret;
}

__host__ __device__ cumat3f rotmat_from_axisangle(const cuvect3f &axis, const float &angle)
{
    cumat3f ret = cumat3f_zeros();
    cumat3f H;
    cuvect3f _naxis;

    _naxis = cuvect3f_normalize(axis);
    H = hodge_dual(_naxis);

    ret += H*sinf(angle);
    H = H*H;
    ret -= H*cosf(angle);
    ret += (H + cumat3f_eye());

    return ret;
}



};