---

m_vector.cpp

Go to the documentation of this file.

// OMICRON ENGINE HEADER FILE
//
// --------------------------------------------------------------------------
// Copyright (C) 2001-2002 by Bjoern Paetzel <kolrabi@gmx.de>
//
// This file is part of the Omicron Engine.
//
// The Omicron Engine is free software; you can redistribute it and/or modify
// it under the terms of the  GNU General Public License  as published by the
// Free Software Foundation;  either version  2  of the License,  or (at your
// option) any later version.
//
// The Omicron Engine  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 General Public License
// for more details.
//
// You should have  received a copy of the  GNU General Public License  along
// with The Omicron Engine;  if not,  write to the  Free Software Foundation,
// Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
//
// --------------------------------------------------------------------------
// Last modified:       $Date: 2002/12/07 19:02:06 $
// By           :       $Author: kolrabi $
// $Id: m_vector.cpp,v 1.1.1.1 2002/12/07 19:02:06 kolrabi Exp $ 

/*

  $Log: m_vector.cpp,v $
  Revision 1.1.1.1  2002/12/07 19:02:06  kolrabi
  initial release


*/

#include            "omicron/internal.h" 
#include            <math.h>

const vec3_t vec3_c::vec3x   = { 1, 0, 0 };
const vec3_t vec3_c::vec3y   = { 0, 1, 0 };
const vec3_t vec3_c::vec3z   = { 0, 0, 1 };
const vec3_t vec3_c::vec30   = { 0, 0, 0 };
 
/**************************************************************************** 
 **************************************************************************** 
 * VECTOR ROUTINES ********************************************************** 
 **************************************************************************** 
 ****************************************************************************/ 

 


/****************************************************************************
 * vector_projectonplane                projects a vector on a given plane *
 ****************************************************************************/
vec3_c vec3_c::project_on_plane
( 
    const vec3_t normal
)
{
    float   d;
    vec3_c  n;
    float   inv_denom;

    AssertReturnValue1(normal, vec3_c());

    inv_denom = 1.0f / vec3_c(normal).length_squared();

    d = dot(normal) * inv_denom;
    n = vec3_c(normal);

    return vec3_c(vec[0] - d * n[0], vec[1] - d * n[1], vec[2] - d * n[2]);
}


/****************************************************************************
 * vector_perpenticular                           get an orthogonal vector *
 ****************************************************************************/
vec3_c vec3_c::perpendicular
( 
)
{
    ulong       pos;
    ulong       i;
    float       minelem = 1.0F;
    vec3_t      tempvec;
    vec3_c      res;

    // find the smallest magnitude axially aligned vector
    for ( pos = 0, i = 0; i < 3; i++ )
    {
        if ( fabs( vec[i] ) < minelem )
        {
            pos     = i;
            minelem = (float)fabs( vec[i] );
        }
    }

    tempvec[0]    = tempvec[1] = tempvec[2] = 0.0f;
    tempvec[pos]  = 1.0f;

    // project the point onto the plane defined by src
    res = vec3_c(tempvec).project_on_plane( vec );

    // normalize the result
    return res.normalize();
}

/****************************************************************************
 * vector_rotate_vec                           rotate vector around vector *
 ****************************************************************************/
vec3_c vec3_c::rotate_vec
( 
    float           degrees, 
    const vec3_t    dir
)
{
    matrix3_t       m;
    matrix3_t       im;
    matrix3_t       zrot;
    matrix3_t       tmpmat;
    matrix3_t       rot;
    vec3_c          vr, vup;
    vec3_t          tmp;

    AssertReturnValue1(dir, vec3_c());

    vr = vec3_c(dir).perpendicular();
    vup = vr.cross(dir);

    m[0][0] = vr[0];    m[1][0] = vr[1];   m[2][0] = vr[2];
    m[0][1] = vup[0];   m[1][1] = vup[1];  m[2][1] = vup[2];
    m[0][2] = dir[0];   m[1][2] = dir[1];  m[2][2] = dir[2];

    memcpy( im, m, sizeof( im ) );

    im[0][1] = m[1][0]; im[0][2] = m[2][0];  im[1][0] = m[0][1];
    im[1][2] = m[2][1]; im[2][0] = m[0][2];  im[2][1] = m[1][2];

    memset( zrot, 0, sizeof( zrot ) );
    zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;

    zrot[0][0] = fcos(DEG2RAD * degrees);
    zrot[0][1] = fsin(DEG2RAD * degrees);
    zrot[1][0] = -zrot[0][1];   // -sin
    zrot[1][1] =  zrot[0][0];   // cos

    matrix3_mult( m, zrot, tmpmat );
    matrix3_mult( tmpmat, im, rot );

    tmp[0] = rot[0][0] * vec[0] + rot[0][1] * vec[1] + rot[0][2] * vec[2];
    tmp[1] = rot[1][0] * vec[0] + rot[1][1] * vec[1] + rot[1][2] * vec[2];
    tmp[2] = rot[2][0] * vec[0] + rot[2][1] * vec[1] + rot[2][2] * vec[2];

    return vec3_c(tmp);
}

/****************************************************************************
 * vector_view_from_angles                      get view vectors by angles *
 ****************************************************************************/
void vec3_c::get_view_from_angles
(
    vec3_c &forward, 
    vec3_c &right, 
    vec3_c &up
) 
{
    float   angle;
    static float    sr, sp, sy, cr, cp, cy;
    // static to help MS compiler fp bugs

    // assumes z is up

    angle = vec[ANGLE_YAW] * DEG2RAD;
    sy    = fsin(angle);
    cy    = fcos(angle);
    angle = vec[ANGLE_PITCH] * DEG2RAD;
    sp    = fsin(angle);
    cp    = fcos(angle);
    angle = vec[ANGLE_ROLL] * DEG2RAD;
    sr    = fsin(angle);
    cr    = fcos(angle);

    if (forward)
        forward.set(cp*cy, cp*sy, -sp);

    if (right)
        right.set( (-1*sr*sp*cy+-1*cr*-sy), (-1*sr*sp*sy+-1*cr*cy), -1*sr*cp);
    
    if (up)
        up.set( (cr*sp*cy+-sr*-sy), (cr*sp*sy+-sr*cy), cr*cp);
}

ushort vec3_c::pack_normal()
{
    ulong nx, ny, nz = 0;
    nx = (ulong)((vec[0]*63)+63)&127;
    ny = (ulong)((vec[1]*63)+63)&127;
    
    if (vec[2]<0)
        nz = 0x8000;

    return (ushort)((nx << 7) | ny | nz);
}

vec3_c vec3_c::unpack_normal(ushort in)
{
    ulong nx, ny, nz;
    vec3_c vec;

    nx = (in>>7) & 127;
    ny = (in) & 127;
    nz = in & 0x8000;

    vec.set( ((float)nx - 63)/64.0f, ((float)ny - 63)/64.0f, 0);

    if (!vec[0] && !vec[1])
        vec.set_component(2, 1);
    else
        vec.set_component(2, (float)sqrt(1-vec.length_squared()));

    if (nz)
      vec.set_component(2, -vec[2]);

    return vec.normalize();
}
/*
float vector_intersect_bbox(const vec3_t v, const bbox_t *bbox, collision_t *out)
{
    float       t1 = 0, t2 = 0, t = 0;
    float       tnear = 0, tfar = 0;
    vec3_t      ro, rd, v2;
    ulong       a,b;
    ulong       side = 0;

    AssertReturnValue3(v,bbox,out,0);

    out->t = 1;
  
    vector_copy(bbox->pos2, out->pos);
    vector_clear(out->normal);
    
    tnear   = -10000;
    tfar    =  10000;

    vector_subtract(bbox->pos1, bbox->pos2, rd);
    vector_add(v, rd, v2);
    vector_normalize(rd,rd);
    vector_subtract(v, bbox->pos1, ro);

    for (a=0; a<3; a++)
    {
        // not moving
        if (rd[a]==0)
        {
            // and outside of bbox
            if (ro[a]<bbox->size[0][a] || ro[a]<bbox->size[0][a])
            {
                return 1;
            }
        }
        // moving
        else
        {
            t1 = (bbox->size[0][a]-ro[a])/rd[a];
            t2 = (bbox->size[1][a]-ro[a])/rd[a];

            if (t1>t2)
            {
                t = t1; t1 = t2; t2 = t;
                b = a+3;
            }
            else
                b = a;

            if (t1>tnear)
            {
                tnear   = t1;
                side    = b;
            }

            if (t2<tfar)
                tfar = t2;

            if (tnear>tfar || tfar==0)
            {
                return 1;
            }
        }
    }

    if (tnear>tfar || tfar==0)
        return 1;

    out->t = 1;
    vector_linear(v, v2, out->t, out->pos);

    vector_clear(out->normal);
    out->normal[side%3] = side>2?-1.0f:1.0f;

    return t2;
}
*/


vec3_c vec3_c::bezier_spline
(
    const   spline_t    sp,
    float   frac
) 
{ 
    float invfrac = 1-frac;
    float b0      = invfrac * invfrac;
    float b1      = 2.0f * frac * invfrac;
    float b2      = frac * frac;

    spline_t spline;
    
    spline[0] = sp[0];
    spline[1] = sp[1];
    spline[2] = sp[2];

    return spline[0] * b0 + spline[1] * b1 + spline[2] * b2;
} 

vec3_c vec3_c::bezier_patch
(
    const   patch_t     patch,
    float   u,
    float   v
) 
{ 
    AssertReturnValue1(patch, vec3_c());

    spline_t spline;

    spline[0] = bezier_spline(patch[0], u);
    spline[1] = bezier_spline(patch[1], u);
    spline[2] = bezier_spline(patch[2], u);

    return bezier_spline(spline, v);
} 


vec3_c::vec3_c()
{
    set(0,0,0);
}

vec3_c::vec3_c(const vec3_t v)
{
    set(v[0], v[1], v[2]);
}

vec3_c::vec3_c(const vec3_c &v)
{
    set(v.vec[0], v.vec[1], v.vec[2]);
}

vec3_c::vec3_c(float x, float y, float z)
{
    set(x,y,z);
}

vec3_c::~vec3_c()
{
}

vec3_c::operator const float*()
{
    return vec;
}

// -- 

void vec3_c::set(float x, float y, float z)
{
    vec[0] = x;
    vec[1] = y;
    vec[2] = z;
}

void vec3_c::set_component(int n, float x)
{
    vec[n] = x;
}

void vec3_c::clear()
{
    set(0,0,0);
}

float vec3_c::length()
{
    return (float)sqrt(
        vec[VECT_X]*vec[VECT_X] +
        vec[VECT_Y]*vec[VECT_Y] + 
        vec[VECT_Z]*vec[VECT_Z]);       // sqrts are slow :( 
}

float vec3_c::length_squared()
{
    return 
        vec[VECT_X]*vec[VECT_X] +
        vec[VECT_Y]*vec[VECT_Y] + 
        vec[VECT_Z]*vec[VECT_Z];
}

bool vec3_c::operator ==(const vec3_t v)
{
    ulong     i; 

    AssertReturnValue1(v, false);

    for (i=0 ; ++i<3;) 
        if (fabs(vec[i]-v[i]) > EQUAL_EPSILON) 
            return false; 
 
    return true; 
}

/*vec3_c vec3_c::operator +(const vec3_t v)
{
    return vec3_c(v[0]+vec[0], v[1]+vec[1], v[2]+vec[2]);
}*/

vec3_c operator +(const vec3_t vec, vec3_c v)
{
    return vec3_c(v[0]+vec[0], v[1]+vec[1], v[2]+vec[2]);
}

vec3_c vec3_c::operator -(const vec3_t v)
{
    return vec3_c(vec[0]-v[0], vec[1]-v[1], vec[2]-v[2]);
}

vec3_c vec3_c::operator *(const float f)
{
    return vec3_c(f*vec[0], f*vec[1], f*vec[2]);
}

vec3_c vec3_c::operator *(const vec3_t v)
{
    return vec3_c(vec[0]*v[0], vec[1]*v[1], vec[2]*v[2]);
}

/**************************************************************************** 
 * vector_cross                                    calculate cross product * 
 ****************************************************************************/ 
vec3_c vec3_c::cross 
( 
    const vec3_t v
) 
{ 
    AssertReturnValue1(v, vec3_c(0,0,0));

    return vec3_c(  vec[VECT_Y]*v[VECT_Z] - vec[VECT_Z]*v[VECT_Y],
                    vec[VECT_Z]*v[VECT_X] - vec[VECT_X]*v[VECT_Z],
                    vec[VECT_X]*v[VECT_Y] - vec[VECT_Y]*v[VECT_X]); 
} // vector_cross 
 
/**************************************************************************** 
 * vector_dot                                        calculate dot product * 
 ****************************************************************************/
float vec3_c::dot
( 
    const vec3_t v
) 
{ 
    AssertReturnValue1(v, 0);

    return  v[VECT_X]*vec[VECT_X] + 
            v[VECT_Y]*vec[VECT_Y] + 
            v[VECT_Z]*vec[VECT_Z]; 
} // vector_dot 

vec3_c vec3_c::operator =(const vec3_t v)
{
    vec[0] = v[0];
    vec[1] = v[1];
    vec[2] = v[2];

    return *this;
}

vec3_c vec3_c::normalize()
{
    return *this * (1/length());
}

vec3_c vec3_c::project()
{
    double    matModel[16], matProj[16]; 
    int       viewport[4]; 
    GLdouble  o[3]; 

    // slooooooooooow..
 
    glGetDoublev( GL_MODELVIEW_MATRIX, matModel ); 
    glGetDoublev( GL_PROJECTION_MATRIX, matProj ); 
    glGetIntegerv( GL_VIEWPORT, viewport); 
    gluProject( vec[0], vec[1], vec[2], matModel, matProj, viewport, &o[0], &o[1], &o[2] ); 

    return vec3_c((float)o[0],(float)o[1],(float)o[2]);
}

vec3_c vec3_c::project_ex 
( 
        ulong   w,                  // dimensions of the screen
        ulong   h 
) 
{ 
    double      matModel[16], matProj[16]; 
    int         viewport[4]; 
    GLdouble    o[3]; 
 
    glGetDoublev( GL_MODELVIEW_MATRIX, matModel ); 
    glGetDoublev( GL_PROJECTION_MATRIX, matProj ); 
    viewport[0] = viewport[1] = 0; 
    viewport[2] = (int)w; 
    viewport[3] = (int)h; 
    gluProject( vec[0], vec[1], vec[2], matModel, matProj, viewport, &o[0], &o[1], &o[2] ); 
 
    return vec3_c((float)o[0],(float)o[1],(float)o[2]);
} 

vec3_c vec3_c::unproject() 
{ 
    double  matModel[16], matProj[16]; 
    int     viewport[4]; 
    GLdouble o[3]; 

    glGetDoublev( GL_MODELVIEW_MATRIX, matModel ); 
    glGetDoublev( GL_PROJECTION_MATRIX, matProj ); 
    glGetIntegerv( GL_VIEWPORT, viewport); 
    gluUnProject( vec[0], vec[1], vec[2], matModel, matProj, viewport, &o[0], &o[1], &o[2] ); 
 
    return vec3_c((float)o[0],(float)o[1],(float)o[2]);
} 
 

/**************************************************************************** 
 * vector_unproject_ex                   unprojects screen to world coords *
 ****************************************************************************/ 
vec3_c vec3_c::unproject_ex(ulong w, ulong h) 
{ 
    double  matModel[16], matProj[16]; 
    int     viewport[4]; 
    GLdouble o[3]; 

    glGetDoublev( GL_MODELVIEW_MATRIX, matModel ); 
    glGetDoublev( GL_PROJECTION_MATRIX, matProj ); 
    viewport[0] = viewport[1] = 0; 
    viewport[2] = (int)w; 
    viewport[3] = (int)h; 
    gluUnProject( vec[0], vec[1], vec[2], matModel, matProj, viewport, &o[0], &o[1], &o[2] ); 
 
    return vec3_c((float)o[0],(float)o[1],(float)o[2]);
} 

/****************************************************************************
 * vector_rotate                                    rotates a vector (ypr) *
 ****************************************************************************/
vec3_c vec3_c::rotate
( 
    const vec3_t angles
)
{ 
    float rad; 
    float radsin; 
    float radcos; 
 
    vec3_t tmp,tmp2; 

    AssertReturnValue1(angles, vec3_c());
 
    rad     = (float)(-angles[ANGLE_ROLL] * DEG2RAD); 

    radsin  = (float)(sin(rad)); radcos = (float)(cos(rad)); 
 
    tmp2[0] = vec[0]*radcos-vec[2]*radsin;
    tmp2[1] = vec[1];
    tmp2[2] = vec[0]*radsin+vec[2]*radcos;
 
    rad     = (float)(-angles[ANGLE_PITCH] * DEG2RAD); 
  
    radsin  = (float)(sin(rad)); radcos = (float)(cos(rad)); 
 
    tmp[0]  = tmp2[0]; 
    tmp[2]  = tmp2[2]*radcos-vec[1]*radsin; 
    tmp[1]  = tmp2[2]*radsin+vec[1]*radcos; 
 
    rad     = (float)(-angles[ANGLE_YAW] * DEG2RAD); 
  
    radsin  = (float)(sin(rad)); radcos = (float)(cos(rad)); 
 
    return vec3_c(  tmp[0]*radcos-tmp[1]*radsin,
                    tmp[0]*radsin+tmp[1]*radcos,
                    tmp[2] );
} 

/****************************************************************************
 * vector_linear                              interpolation of two vectors *
 ****************************************************************************/
vec3_c vec3_c::linear
(
    const vec3_t p1, 
    const vec3_t p2, 
    float frac
) 
{ 
    AssertReturnValue2(p1,p2, vec3_c());

    vec3_c vec;

    for (ulong i=0; i<3; i++) 
        vec.set_component(i, _interpolate_float_linear(p1[i], p2[i], frac));

    return vec;
} 

/****************************************************************************
 * vector_hermite                     hermite interpolation of two vectors *
 ****************************************************************************/
vec3_c vec3_c::hermite
(
    const vec3_t p1, 
    const vec3_t t1, 
    const vec3_t p2, 
    const vec3_t t2, 
    float   frac
) 
{ 
    AssertReturnValue4(p1,p2,t1,t2, vec3_c());

    vec3_c vec;

    float tsquared  = frac*frac; 
    float tsquared3 = tsquared*3; 
    float tcubed    = frac*tsquared; 
    float tcubed2   = tcubed*2; 
    float t         = frac; 
 
    float a = (tcubed2-tsquared3+1); 
    float b = (tcubed-(2*tsquared)+t); 
    float c = (tcubed-tsquared); 
    float d = (-tcubed2+tsquared3); 
 
    for (ulong i=0; i<3; i++) 
    { 
        float   pa = p1[i]; 
        float   pb = p2[i]; 
        float   ma = t1[i]; 
        float   mb = t2[i]; 
 
        vec.set_component(i, a*pa+b*ma+c*mb+d*pb);
    } 

    return vec;
} 

---