root/trunk/orca/math3d.c

/*============================================================================
    ORCA Interpreter
    Copyright (C) 2005-2006  Karl Robillard

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
===========================================================================*/

#ifdef OR_CONFIG_MATH3D


#include "ovalue.h"
#include "math3d.h"
#include "os.h"
#include "os_math.h"


enum eMatrixIndex
{
    k00 = 0,
    k01,
    k02,
    k03,
    k10,
    k11,
    k12,
    k13,
    k20,
    k21,
    k22,
    k23,
    k30,
    k31,
    k32,
    k33,

    kX = 12,
    kY,
    kZ
};


static float _identity[16] =
{
    1.0f, 0.0f, 0.0f, 0.0f,
    0.0f, 1.0f, 0.0f, 0.0f,
    0.0f, 0.0f, 1.0f, 0.0f,
    0.0f, 0.0f, 0.0f, 1.0f
};


void orLoadIdentity( float* mat )
{
    memCpy( mat, _identity, 16 * sizeof(float) );
}


void orLoadRotation( float* mat, const float* axis, float radians )
{
    float x, y, z;
    float c, s, t;

    x = axis[0];
    y = axis[1];
    z = axis[2];

    c = mathCosineF( radians );
    s = mathSineF( radians );
    t = 1.0f - c;

    mat[ 0 ] = t*x*x + c;
    mat[ 1 ] = t*x*y + s*z;
    mat[ 2 ] = t*x*z - s*y;
    mat[ 3 ] = 0.0f;

    mat[ 4 ] = t*x*y - s*z;
    mat[ 5 ] = t*y*y + c;
    mat[ 6 ] = t*y*z + s*x;
    mat[ 7 ] = 0.0f;

    mat[  8 ] = t*x*z + s*y;
    mat[  9 ] = t*y*z - s*x;
    mat[ 10 ] = t*z*z + c;
    mat[ 11 ] = 0.0f;

    mat[ 12 ] = 0.0f;
    mat[ 13 ] = 0.0f;
    mat[ 14 ] = 0.0f;
    mat[ 15 ] = 1.0f;
}


/*
  Result can be the same matrix as A, but not B.
*/
void orMatrixMult( const float* a, const float* b, float* result )
{
#define A(col,row)  a[(col<<2)+row]
#define R(col,row)  result[(col<<2)+row]

    int i;
    for( i = 0; i < 4; i++ )
    {
        float ai0=A(0,i), ai1=A(1,i), ai2=A(2,i), ai3=A(3,i);
        R(0,i) = ai0 * b[k00] + ai1 * b[k01] + ai2 * b[k02] + ai3 * b[k03];
        R(1,i) = ai0 * b[k10] + ai1 * b[k11] + ai2 * b[k12] + ai3 * b[k13];
        R(2,i) = ai0 * b[k20] + ai1 * b[k21] + ai2 * b[k22] + ai3 * b[k23];
        R(3,i) = ai0 * b[k30] + ai1 * b[k31] + ai2 * b[k32] + ai3 * b[k33];
    }
}


void orTransLocal( float* mat, float x, float y, float z )
{
    if( x )
    {
        mat[ kX ] += mat[ k00 ] * x;
        mat[ kY ] += mat[ k01 ] * x;
        mat[ kZ ] += mat[ k02 ] * x;
    }
    if( y )
    {
        mat[ kX ] += mat[ k10 ] * y;
        mat[ kY ] += mat[ k11 ] * y;
        mat[ kZ ] += mat[ k12 ] * y;
    }
    if( z )
    {
        mat[ kX ] += mat[ k20 ] * z;
        mat[ kY ] += mat[ k21 ] * z;
        mat[ kZ ] += mat[ k22 ] * z;
    }
}


/**
  Transpose 3x3 submatrix to negate rotation.
*/
void orMatrixTranspose( float* mat, const float* a )
{
    mat[ k00 ] = a[ k00 ];
    mat[ k01 ] = a[ k10 ];
    mat[ k02 ] = a[ k20 ];
    mat[ k03 ] = a[ k03 ];

    mat[ k10 ] = a[ k01 ];
    mat[ k11 ] = a[ k11 ];
    mat[ k12 ] = a[ k21 ];
    mat[ k13 ] = a[ k13 ];

    mat[ k20 ] = a[ k02 ];
    mat[ k21 ] = a[ k12 ];
    mat[ k22 ] = a[ k22 ];
    mat[ k23 ] = a[ k23 ];
}


/**
  Negates rotation and translation.
*/
void orMatrixInverse( float* mat, const float* a )
{
    orMatrixTranspose( mat, a );

    // Negate translation.

    mat[ kX ] = - (a[kX] * a[k00]) - (a[kY] * a[k01]) - (a[kZ] * a[k02]);
    mat[ kY ] = - (a[kX] * a[k10]) - (a[kY] * a[k11]) - (a[kZ] * a[k12]);
    mat[ kZ ] = - (a[kX] * a[k20]) - (a[kY] * a[k21]) - (a[kZ] * a[k22]);
    mat[ k33 ] = a[ k33 ];
}


/*--------------------------------------------------------------------------*/


/**
  Returns the distance from vecA to vecB.
*/
float orDistance( const float* vecA, const float* vecB )
{
    float dx, dy, dz;

    dx = vecB[0] - vecA[0];
    dy = vecB[1] - vecA[1];
    dz = vecB[2] - vecA[2];

    return mathSqrtF( dx * dx + dy * dy + dz * dz );
}


/**
  Applies the entire matrix to this point.
*/
void orTransform( float* pnt, const float* mat )
{
    float ox, oy, oz;

    ox = pnt[0];
    oy = pnt[1];
    oz = pnt[2];

    pnt[0] = mat[ k00 ] * ox + mat[ k10 ] * oy + mat[ k20 ] * oz + mat[ k30 ];
    pnt[1] = mat[ k01 ] * ox + mat[ k11 ] * oy + mat[ k21 ] * oz + mat[ k31 ];
    pnt[2] = mat[ k02 ] * ox + mat[ k12 ] * oy + mat[ k22 ] * oz + mat[ k32 ];
}


/**
  Applies the upper 3x3 portion of the matrix to this point.
*/
void orTransform3x3( float* pnt, const float* mat )
{
    float ox, oy, oz;

    ox = pnt[0];
    oy = pnt[1];
    oz = pnt[2];

    pnt[0] = mat[ k00 ] * ox + mat[ k10 ] * oy + mat[ k20 ] * oz;
    pnt[1] = mat[ k01 ] * ox + mat[ k11 ] * oy + mat[ k21 ] * oz;
    pnt[2] = mat[ k02 ] * ox + mat[ k12 ] * oy + mat[ k22 ] * oz;
}


/**
  Result can be the same as either a or b.
*/
void orReflect( const float* a, const float* b, float* result )
{
    float dot2 = orDot(a, b) * 2.0f;

    result[0] = a[0] - (b[0] * dot2);
    result[1] = a[1] - (b[1] * dot2);
    result[2] = a[2] - (b[2] * dot2);
}


void orNormalize( float* vec )
{
    float x, y, z;
    float len;

    x = vec[0];
    y = vec[1];
    z = vec[2];

    len = mathSqrtF( x * x + y * y + z * z );
    if( len )
    {
        len = 1.0f / len;
        vec[0] = x * len;
        vec[1] = y * len;
        vec[2] = z * len;
    }
}


/*--------------------------------------------------------------------------*/


/**
  Returns zero if invalid values were found.
*/
int orLoadVectorBlock( float* vec, int count, OValue* blkV )
{
#define VEND    if(vec == vend) break

    OBlock* blk = orBLOCK(blkV);
    OValue* it  = blk->values + blkV->series.it;
    OValue* end = blk->values + blk->used;
    float* vend = vec + count;
    int ok = 1;

    while( it != end )
    {
        if( it->type == OT_INTEGER )
        {
            *vec++ = (float) orInt(it);
            VEND;
        }
        else if( it->type == OT_DECIMAL )
        {
            *vec++ = (float) orDecimal(it);
            VEND;
        }
#if 0
        else if( it->type == OT_VEC3 )
        {
            *vec++ = it->vec3.x;
            VEND;
            *vec++ = it->vec3.y;
            VEND;
            *vec++ = it->vec3.z;
            VEND;
        }
#endif
        else
        {
            ok = 0;
        }

        ++it;
    }

    return ok;
}


OR_NATIVE_PUB( orDotNative )
{
    float* a = &a1->vec3.x;
    float* b = &(a1 + 1)->vec3.x;

    orSetTF( a1, OT_DECIMAL );
    orDecimal(a1) = (double) orDot(a, b);
}


OR_NATIVE_PUB( orCrossNative )
{
    float* a = &a1->vec3.x;
    float* b = &(a1 + 1)->vec3.x;
    float res[3];

    orCross( a, b, res );
    memCpy( &a1->vec3, res, 3 * sizeof(float) );
}


OR_NATIVE_PUB( orNormalizeNative )
{
    orNormalize( &a1->vec3.x );
}


#endif

/* EOF */