Logo Search packages:      
Sourcecode: vdrift version File versions

quat.cpp

#include "quat.h"

#define DELTA 0.00001

void QUATERNION::LoadMultIdent()
{
      w = 1.0f;
      x = y = z = 0.0f;
      cachematvalid = false;
}

QUATERNION::QUATERNION()
{
      LoadMultIdent();
      cachematvalid = false;
}

float QUATERNION::Norm()
{
      return sqrt(w*w+x*x+y*y+z*z);
}

void QUATERNION::Normalize()
{
      cachematvalid = false;
      
      float len = Norm();
      w = w / len;
      x = x / len;
      y = y / len;
      z = z / len;
}

void QUATERNION::GetMat(float * destmat)
{
      if (!cachematvalid)
      {
            Normalize();
            //NOTE:  We're assuming the quaternion is normalized here!
            
            float xx = x*x;
            float xy = x*y;
            float xz = x*z;
            float xw = x*w;
            
            float yy = y*y;
            float yz = y*z;
            float yw = y*w;
            
            float zz = z*z;
            float zw = z*w;
            
            destmat[0] = 1.0f - 2.0f*(yy+zz);
            destmat[1] = 2.0f*(xy+zw);
            destmat[2] = 2.0f*(xz-yw);
            destmat[3] = 0;
            
            destmat[4] = 2.0f*(xy-zw);
            destmat[5] = 1.0f-2.0f*(xx+zz);
            destmat[6] = 2.0f*(yz+xw);
            destmat[7] = 0;
            
            destmat[8] = 2.0f*(xz+yw);
            destmat[9] = 2.0f*(yz-xw);
            destmat[10] = 1.0f-2.0f*(xx+yy);
            destmat[11] = 0;
            
            destmat[12] = 0;
            destmat[13] = 0;
            destmat[14] = 0;
            destmat[15] = 1;
            
            //build cached matrix
            for (int i = 0; i < 16; i++)
            {
                  cachemat[i] = destmat[i];
            }
            cachematvalid = true;
      }
      else
      {
            //send back the cached matrix
            for (int i = 0; i < 16; i++)
            {
                  destmat[i] = cachemat[i];
            }
      }
      
      /*xx      = X * X;
    xy      = X * Y;
    xz      = X * Z;
    xw      = X * W;

    yy      = Y * Y;
    yz      = Y * Z;
    yw      = Y * W;

    zz      = Z * Z;
    zw      = Z * W;

    mat[0]  = 1 - 2 * ( yy + zz );
    mat[1]  =     2 * ( xy - zw );
    mat[2]  =     2 * ( xz + yw );

    mat[4]  =     2 * ( xy + zw );
    mat[5]  = 1 - 2 * ( xx + zz );
    mat[6]  =     2 * ( yz - xw );

    mat[8]  =     2 * ( xz - yw );
    mat[9]  =     2 * ( yz + xw );
    mat[10] = 1 - 2 * ( xx + yy );

    mat[3]  = mat[7] = mat[11 = mat[12] = mat[13] = mat[14] = 0;
    mat[15] = 1;*/
}

void QUATERNION::GetMat(GLdouble * destmat)
{
      if (!cachematvalid)
      {
            Normalize();
            //NOTE:  We're assuming the quaternion is normalized here!
            
            GLdouble xx = x*x;
            GLdouble xy = x*y;
            GLdouble xz = x*z;
            GLdouble xw = x*w;
            
            GLdouble yy = y*y;
            GLdouble yz = y*z;
            GLdouble yw = y*w;
            
            GLdouble zz = z*z;
            GLdouble zw = z*w;
            
            destmat[0] = 1.0f - 2.0f*(yy+zz);
            destmat[1] = 2.0f*(xy+zw);
            destmat[2] = 2.0f*(xz-yw);
            destmat[3] = 0;
            
            destmat[4] = 2.0f*(xy-zw);
            destmat[5] = 1.0f-2.0f*(xx+zz);
            destmat[6] = 2.0f*(yz+xw);
            destmat[7] = 0;
            
            destmat[8] = 2.0f*(xz+yw);
            destmat[9] = 2.0f*(yz-xw);
            destmat[10] = 1.0f-2.0f*(xx+yy);
            destmat[11] = 0;
            
            destmat[12] = 0;
            destmat[13] = 0;
            destmat[14] = 0;
            destmat[15] = 1;
            
            //build cached matrix
            for (int i = 0; i < 16; i++)
            {
                  cachemat[i] = destmat[i];
            }
            cachematvalid = true;
      }
      else
      {
            //send back the cached matrix
            for (int i = 0; i < 16; i++)
            {
                  destmat[i] = cachemat[i];
            }
      }
}

void QUATERNION::GetAxisAngle(float * aangle)
{
      //aangle is (angle of rotation, x, y, z)
      
      /*float scale = x*x+y*y+z*z;
      if (scale < 0.00001)
      {
            aangle[0] = 0;
            aangle[1] = 1;
            aangle[2] = 0;
            aangle[3] = 0;
      }
      else
      {
            aangle[0] = 2*acos(w);
            aangle[1] = x/scale;
            aangle[2] = y/scale;
            aangle[3] = z/scale;
      }*/
      
      
      /*float scale = x*x+y*y+z*z;
      
      float cos_angle  = w;
    aangle[0]      = acos( cos_angle ) * 2;
    float sin_angle  = sqrt( 1.0 - cos_angle * cos_angle );


    if ( fabs( sin_angle ) < 0.00005 )
      scale = 1.0f;

    aangle[1] = x/scale;
    aangle[2] = y/scale;
      aangle[3] = z/scale;*/
      
    GLfloat len;
    GLfloat tx, ty, tz;

      // cache variables
      tx = x;
      ty = y;
      tz = z;

      len = tx * tx + ty * ty + tz * tz;

    if (len > DELTA) 
      {
            aangle[1] = tx * (1.0f / len);
            aangle[2] = ty * (1.0f / len);
            aangle[3] = tz * (1.0f / len);
          aangle[0] = (GLfloat)(2.0 * acos(w));
    }

    else {
            aangle[1] = 0.0;
            aangle[2] = 0.0;
            aangle[3] = 1.0;
          aangle[0] = 0.0;
    }
}



void QUATERNION::Multiply(QUATERNION quat2)
{
      cachematvalid = false;
      
      //slow multiply
      float w2 = quat2.w;
      float x2 = quat2.x;
      float y2 = quat2.y;
      float z2 = quat2.z;
      w = w*w2 - x*x2 - y*y2 - z*z2;
      x = w*x2 + x*w2 + y*z2 - z*y2;
      y = w*y2 + y*w2 + z*x2 - x*z2;
      z = w*z2 + z*w2 + x*y2 - y*x2;
      
      /*//FAST multiply code
      float a = w;
      float b = x;
      float c = y;
      float d = z;
      float w2 = quat2.w;
      float x2 = quat2.x;
      float y2 = quat2.y;
      float z2 = quat2.z;
      
      float tmp_00, tmp_01, tmp_02, tmp_03, tmp_04, tmp_05, tmp_06, tmp_07, tmp_08, tmp_09;
      
      tmp_00 = (d - c) * (z2 - w2);
      tmp_01 = (a + b) * (x2 + y2);
      tmp_02 = (a - b) * (z2 + w2);
      tmp_03 = (c + d) * (x2 - y2);
      tmp_04 = (d - b) * (y2 - z2);
      tmp_05 = (d + b) * (y2 + z2);
      tmp_06 = (a + c) * (x2 - w2);
      tmp_07 = (a - c) * (x2 + w2);
      tmp_08 = tmp_05 + tmp_06 + tmp_07;
      tmp_09 = 0.5 * (tmp_04 + tmp_08);
      
      w = tmp_00 + tmp_09 - tmp_05;
      x = tmp_01 + tmp_09 - tmp_08;
      y = tmp_02 + tmp_09 - tmp_07;
      z = tmp_03 + tmp_09 - tmp_06;*/
}

void QUATERNION::PostMultiply(QUATERNION quat2)
{
      cachematvalid = false;
      
      //slow multiply
      float w1 = quat2.w;
      float x1 = quat2.x;
      float y1 = quat2.y;
      float z1 = quat2.z;
      float w2 = w;
      float x2 = x;
      float y2 = y;
      float z2 = z;
      w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
      x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
      y = w1*y2 + y1*w2 + z1*x2 - x1*z2;
      z = w1*z2 + z1*w2 + x1*y2 - y1*x2;
      
      //FAST multiply code
      /*float a = quat2.w;
      float b = quat2.x;
      float c = quat2.y;
      float d = quat2.z;
      
      float tmp_00, tmp_01, tmp_02, tmp_03, tmp_04, tmp_05, tmp_06, tmp_07, tmp_08, tmp_09;
      
      tmp_00 = (d - c) * (z - w);
      tmp_01 = (a + b) * (x + y);
      tmp_02 = (a - b) * (z + w);
      tmp_03 = (c + d) * (x - y);
      tmp_04 = (d - b) * (y - z);
      tmp_05 = (d + b) * (y + z);
      tmp_06 = (a + c) * (x - w);
      tmp_07 = (a - c) * (x + w);
      tmp_08 = tmp_05 + tmp_06 + tmp_07;
      tmp_09 = 0.5 * (tmp_04 + tmp_08);
      
      w = tmp_00 + tmp_09 - tmp_05;
      x = tmp_01 + tmp_09 - tmp_08;
      y = tmp_02 + tmp_09 - tmp_07;
      z = tmp_03 + tmp_09 - tmp_06;*/
}

QUATERNION QUATERNION::operator* (const QUATERNION &quat2 )
{
      //below line stays true since we're not actually modifying our current quat
      //cachematvalid = false;
      
      QUATERNION qout;
      
      //slow multiply
      float w2 = quat2.w;
      float x2 = quat2.x;
      float y2 = quat2.y;
      float z2 = quat2.z;
      qout.w = w*w2 - x*x2 - y*y2 - z*z2;
      qout.x = w*x2 + x*w2 + y*z2 - z*y2;
      qout.y = w*y2 + y*w2 + z*x2 - x*z2;
      qout.z = w*z2 + z*w2 + x*y2 - y*x2;
            
      return qout;
}

//i believe a is in radians
void QUATERNION::SetAxisAngle(float a, float ax, float ay, float az)
{
      cachematvalid = false;
      
      //normalize axis
      float temp = ax*ax + ay*ay + az*az;
    float dist = (GLfloat)(1.0 / sqrt(temp));

    ax *= dist;
    ay *= dist;
    az *= dist;
      
      /*if (ax < 0.00001 && ay < 0.00001 && az < 0.00001)
      {
            LoadMultIdent();
      }
      else*/
      {
            float sina2 = sin(a/2);
            
            w   =   cos(a/2);
            x   =   ax * sina2;
            y   =   ay * sina2;
            z   =   az * sina2;
      }
      
      
      /*GLfloat temp, dist;

      // normalize
      temp = ax*ax + ay*ay + az*az;

    dist = (GLfloat)(1.0 / sqrt(temp));

    ax *= dist;
    ay *= dist;
    az *= dist;

      x = ax;
      y = ay;
      z = az;

      w = (GLfloat)cos(a / 2.0f);*/
      
}

void QUATERNION::SetEuler(float a, float b, float c)
{
      cachematvalid = false;
      
      QUATERNION qout;
      
    QUATERNION Qx, Qy, Qz;
      Qx.w = cos(a/2); Qx.x = sin(a/2); Qx.y = Qx.z = 0.0f;
    Qy.w = cos(b/2); Qy.x = Qy.z = 0.0f; Qy.y = sin(b/2);
    Qz.w = cos(c/2); Qz.x = Qz.y = 0.0f; Qz.z = sin(c/2);

      qout = Qx * Qy * Qz;
      
      *this = qout;
}

QUATERNION QUATERNION::ReturnConjugate()
{
      QUATERNION qtemp;
      qtemp.w = w;
      qtemp.x = -x;
      qtemp.y = -y;
      qtemp.z = -z;
      return qtemp;
}

void QUATERNION::LookAt(float eyeX, 
               float eyeY, 
               float eyeZ, 
               float centerX, 
               float centerY, 
               float centerZ, 
               float upX, 
               float upY, 
               float upZ)
{
      float tempmat[16];
      
      glPushMatrix();
      glLoadIdentity();
      gluLookAt(eyeX, eyeY, eyeZ, centerX, centerY, centerZ, upX, upY, upZ);
      glGetFloatv(GL_MODELVIEW_MATRIX , tempmat);
      glPopMatrix();
      
      SetMat(tempmat);
}

void QUATERNION::SetMat(float * mat)
{
      float S;
      
      float T = 1 + mat[0] + mat[5] + mat[10];
      if ( T > 0.00000001 )
      {
            S = sqrt(T) * 2;
            x = ( mat[9] - mat[6] ) / S;
            y = ( mat[2] - mat[8] ) / S;
            z = ( mat[4] - mat[1] ) / S;
            w = 0.25 * S;
      }
      else
      {
            if ( mat[0] > mat[5] && mat[0] > mat[10] )  {       // Column 0: 
                  S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
                  x = 0.25 * S;
                  y = (mat[4] + mat[1] ) / S;
                  z = (mat[2] + mat[8] ) / S;
                  w = (mat[9] - mat[6] ) / S;
            } else if ( mat[5] > mat[10] ) {                    // Column 1: 
                  S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
                  x = (mat[4] + mat[1] ) / S;
                  y = 0.25 * S;
                  z = (mat[9] + mat[6] ) / S;
                  w = (mat[2] - mat[8] ) / S;
            } else {                                            // Column 2:
                  S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
                  x = (mat[2] + mat[8] ) / S;
                  y = (mat[9] + mat[6] ) / S;
                  z = 0.25 * S;
                  w = (mat[4] - mat[1] ) / S;
            }
      }
}

/*void QUATERNION::SetMat(float * m1)
{
      w = sqrt(1.0 + m1[0] + m1[5] + m1[10]) / 2.0;
      double w4 = (4.0 * w);
      x = (m1[9] - m1[6]) / w4 ;
      y = (m1[2] - m1[8]) / w4 ;
      z = (m1[4] - m1[1]) / w4 ;
}*/

VERTEX QUATERNION::RotateVec(VERTEX vec)
{
      QUATERNION dirconj;
      
      QUATERNION dir;
      dir.w = w;
      dir.x = x;
      dir.y = y;
      dir.z = z;

      dirconj = dir.ReturnConjugate();
      QUATERNION qtemp;
      qtemp.w = 0;
      qtemp.x = vec.x;
      qtemp.y = vec.y;
      qtemp.z = vec.z;
      QUATERNION qout;
      qout = dir * qtemp * dirconj;
      
      VERTEX vout;
      
      vout.x += qout.x;
      vout.y += qout.y;
      vout.z += qout.z;
      
      return vout;
}

float QUATERNION::GetAngleBetween(QUATERNION quat2)
{
      //establish a forward vector
      VERTEX forward;
      forward.x = 0;
      forward.y = 0;
      forward.z = 1;
      
      //create vectors for quats
      VERTEX vec1, vec2;
      vec1 = RotateVec(forward);
      vec2 = quat2.RotateVec(forward);
      
      //return the angle between the vectors
      return acos(vec1.dot(vec2));
}

VERTEX QUATERNION::GetEuler()
{
      VERTEX vout;
      QUATERNION q1 = *this;
      
      float sqw = q1.w*q1.w;
    float sqx = q1.x*q1.x;
    float sqy = q1.y*q1.y;
    float sqz = q1.z*q1.z;
      
    vout.z = atan(2.0 * (q1.x*q1.y + q1.z*q1.w)/(sqx - sqy - sqz + sqw));
    vout.x = atan(2.0 * (q1.y*q1.z + q1.x*q1.w)/(-sqx - sqy + sqz + sqw));
    vout.y = asin(-2.0 * (q1.x*q1.z - q1.y*q1.w));
      
      return vout;
}

QUATERNION QUATERNION::Slerp(QUATERNION quat2, float t)
{
      QUATERNION qout, p1, p2;
      float theta = GetAngleBetween(quat2);
      
      //qout = (*this).MultScalar(sin((1.0f-t)*theta)).Add(quat2.MultScalar(sin(t*theta))).MultScalar(1.0f/sin(theta));
      
      p1 = (*this).MultScalar((sin((1.0f-t)*theta))/sin(theta));
      p2 = quat2.MultScalar(sin(theta*t)/sinh(theta));
      qout = p1.Add(p2);
      
      
      return qout;
}

QUATERNION QUATERNION::Add(QUATERNION quat2)
{
      QUATERNION qout;
      
      qout.w = w + quat2.w;
      qout.x = x + quat2.x;
      qout.y = y + quat2.y;
      qout.z = z + quat2.z;
      
      return qout;
}

QUATERNION QUATERNION::MultScalar(float scalar)
{
      QUATERNION qout;
      
      qout.w = w * scalar;
      qout.x = x * scalar;
      qout.y = y * scalar;
      qout.z = z * scalar;
      
      return qout;
}

QUATERNION QUATERNION::QuatSlerp(QUATERNION quat2, float t)
{
      float           to1[4];
      float        omega, cosom, sinom, scale0, scale1;


      // calc cosine
      cosom = x * quat2.x + y * quat2.y + z * quat2.z
                     + w * quat2.w;


      // adjust signs (if necessary)
      if ( cosom <0.0 ){ cosom = -cosom; to1[0] = - quat2.x;
      to1[1] = - quat2.y;
      to1[2] = - quat2.z;
      to1[3] = - quat2.w;
      } else  {
      to1[0] = quat2.x;
      to1[1] = quat2.y;
      to1[2] = quat2.z;
      to1[3] = quat2.w;
      }


      // calculate coefficients


   if ( (1.0 - cosom) > DELTA ) {
                  // standard case (slerp)
                  omega = acos(cosom);
                  sinom = sin(omega);
                  scale0 = sin((1.0 - t) * omega) / sinom;
                  scale1 = sin(t * omega) / sinom;


      } else {        
// "from" and "to" quaternions are very close 
      //  ... so we can do a linear interpolation
                  scale0 = 1.0 - t;
                  scale1 = t;
      }
      // calculate final values
      QUATERNION qout;
      qout.x = scale0 * x + scale1 * to1[0];
      qout.y = scale0 * y + scale1 * to1[1];
      qout.z = scale0 * z + scale1 * to1[2];
      qout.w = scale0 * w + scale1 * to1[3];
      return qout;
}

//i was getting angry because the quats couldn't do this, so i added it....
void QUATERNION::Rotate(float a, float ax, float ay, float az)
{
      //NOTE: ax, ay, az is assumed to be a UNIT VECTOR!!
      
      QUATERNION qtemp;
      qtemp.SetAxisAngle(a, ax, ay, az);
      PostMultiply(qtemp);
}

void QUATERNION::DebugPrint()
{
      cout << x << "," << y << "," << z << "," << w << endl;
}

VERTEX VERTEX::operator+ (VERTEX v)
{
      VERTEX result;
      
      result.x = x + v.x;
      result.y = y + v.y;
      result.z = z + v.z;
      
      return result;
}

VERTEX VERTEX::operator- (VERTEX v)
{
      VERTEX result;
      
      result.x = x - v.x;
      result.y = y - v.y;
      result.z = z - v.z;
      
      return result;
}

void VERTEX::Set(float nx, float ny, float nz)
{
      x = nx;
      y = ny;
      z = nz;
}

float VERTEX::len()
{
      //optimization!
      if (x == 0 && y == 0 && z == 0)
            return 0;
      
      float sl = x*x+y*y+z*z;
      
      if (sl < 0)
      {
            ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
            err << x << endl << y << endl << z << endl << sl << endl;
            err.close();
            assert(0);
      }
      else if (sl >=0 )
      {     
            return sqrt(sl);
      }
      else
      {
            //last ditch effort to fix the problem
            if (!(x < 0 || x >= 0))
                  x = 1;
            if (!(y < 0 || y >= 0))
                  y = 1;
            if (!(z < 0 || z >= 0))
                  z = 1;
            sl = x*x+y*y+z*z;
            
            if (!(sl >= 0))
            {
                  ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
                  err << x << endl << y << endl << z << endl << sl << endl;
                  err.close();
                  assert(0);
                  return 0;
            }
            else
                  return sqrt(sl);
      }
}

VERTEX VERTEX::cross(VERTEX b)
{
      VERTEX result;

      result.x = y*b.z - z*b.y;
      //result.y = x*b.z - z*b.x;
      result.y = z*b.x - x*b.z;
      result.z = x*b.y - y*b.x;

      return result;
}

VERTEX VERTEX::normalize()
{
      VERTEX new_vec;
      float vec_len = len();
      
      if (vec_len == 0.0f)
      {
            new_vec.x = 0.0f;
            new_vec.y = 0.0f;
            new_vec.z = 0.0f;
            return new_vec;
      }
      
      float inv_vec_len = 1.0f/vec_len;

      new_vec.x = x*inv_vec_len;
      new_vec.y = y*inv_vec_len;
      new_vec.z = z*inv_vec_len;

      return new_vec;
}

VERTEX::VERTEX()
{
      x = y = z = 0.0f;
}

void VERTEX::Scale(float scalar)
{
      x = x*scalar;
      y = y*scalar;
      z = z*scalar;
}

VERTEX VERTEX::ScaleR(float scalar)
{
      VERTEX result;
      
      result.x = x*scalar;
      result.y = y*scalar;
      result.z = z*scalar;
      
      return result;
}

VERTEX VERTEX::InvertR()
{
      VERTEX result;
      
      result.x = -x;
      result.y = -y;
      result.z = -z;
      
      return result;
}

float VERTEX::dot(VERTEX b)
{
      return x*b.x + y*b.y + z*b.z;
}

VERTEX VERTEX::interpolatewith(VERTEX other, float percent)
{
      VERTEX vout;
      
      vout.x = (other.x - x)*percent + x;
      vout.y = (other.y - y)*percent + y;
      vout.z = (other.z - z)*percent + z;
      
      return vout;
}

float * VERTEX::v3()
{
      float3[0] = x;
      float3[1] = y;
      float3[2] = z;
      
      return float3;
}

void VERTEX::DebugPrint(ostream & out)
{
      out << x << "," << y << "," << z << endl;
}

void VERTEX::zero()
{
      x = y = z = 0.0f;
}

void VERTEX::Set(float * f3)
{
      x = f3[0];
      y = f3[1];
      z = f3[2];
}

bool VERTEX::nan()
{
      if (!(x < 0 || x >= 0) || !(y < 0 || y >= 0) || !(z < 0 || z >= 0))
            return true;
      
      return false;
}

bool VERTEX::equals(VERTEX other)
{
      return (x == other.x && y == other.y && z == other.z);
}

VERTEX QUATERNION::RelativeMove(VERTEX input)
{
      QUATERNION dirconj, dir;
      VERTEX position;

      dirconj = ReturnConjugate();
      dir.w = w;
      dir.x = x;
      dir.y = y;
      dir.z = z;
      QUATERNION qtemp;
      qtemp.w = 0;
      qtemp.x = input.x;
      qtemp.y = input.y;
      qtemp.z = input.z;
      QUATERNION qout;
      qout = dirconj * qtemp * dir;
      
      position.x = qout.x;
      position.y = qout.y;
      position.z = qout.z;
      return position;
}

void MATRIX3::ProjectionMatrix(VERTEX normal)
{
      float a = normal.x;
      float b = normal.y;
      float c = normal.z;
      
      row[0].x = b*b+c*c;
      row[0].y = -a*b;
      row[0].z = -a*c;
      
      row[1].x = -b*a;
      row[1].y = a*a+c*c;
      row[1].z = -b*c;
      
      row[2].x = -c*a;
      row[2].y = -c*b;
      row[2].z = a*a+b*b;
}

VERTEX MATRIX3::Multiply(VERTEX vert)
{
      VERTEX out;
      
      /*out.zero();
      out.x += row[0].x*vert.x;
      out.x += row[0].y*vert.y;
      out.x += row[0].z*vert.z;*/
      
      out.x = row[0].dot(vert);
      out.y = row[1].dot(vert);
      out.z = row[2].dot(vert);
      
      return out;
}

//---------- VERTEXD
VERTEXD VERTEXD::operator+ (VERTEXD v)
{
      VERTEXD result;
      
      result.x = x + v.x;
      result.y = y + v.y;
      result.z = z + v.z;
      
      return result;
}

VERTEXD VERTEXD::operator- (VERTEXD v)
{
      VERTEXD result;
      
      result.x = x - v.x;
      result.y = y - v.y;
      result.z = z - v.z;
      
      return result;
}

void VERTEXD::Set(double nx, double ny, double nz)
{
      x = nx;
      y = ny;
      z = nz;
}

double VERTEXD::len()
{
      //optimization!
      if (x == 0 && y == 0 && z == 0)
            return 0;
      
      double sl = x*x+y*y+z*z;
      
      if (sl < 0)
      {
            ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
            err << x << endl << y << endl << z << endl << sl << endl;
            err.close();
            assert(0);
      }
      else if (sl >=0 )
      {     
            return sqrt(sl);
      }
      else
      {
            //last ditch effort to fix the problem
            if (!(x < 0 || x >= 0))
                  x = 1;
            if (!(y < 0 || y >= 0))
                  y = 1;
            if (!(z < 0 || z >= 0))
                  z = 1;
            sl = x*x+y*y+z*z;
            
            if (!(sl >= 0))
            {
                  ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
                  err << x << endl << y << endl << z << endl << sl << endl;
                  err.close();
                  assert(0);
                  return 0;
            }
            else
                  return sqrt(sl);
      }
}

VERTEXD VERTEXD::cross(VERTEXD b)
{
      VERTEXD result;

      result.x = y*b.z - z*b.y;
      //result.y = x*b.z - z*b.x;
      result.y = z*b.x - x*b.z;
      result.z = x*b.y - y*b.x;

      return result;
}

VERTEXD VERTEXD::normalize()
{
      VERTEXD new_vec;
      double vec_len = len();
      
      if (vec_len == 0.0f)
      {
            new_vec.x = 0.0f;
            new_vec.y = 0.0f;
            new_vec.z = 0.0f;
            return new_vec;
      }

      new_vec.x = x/vec_len;
      new_vec.y = y/vec_len;
      new_vec.z = z/vec_len;

      return new_vec;
}

VERTEXD::VERTEXD()
{
      x = y = z = 0.0f;
}

void VERTEXD::Scale(double scalar)
{
      x = x*scalar;
      y = y*scalar;
      z = z*scalar;
}

VERTEXD VERTEXD::ScaleR(double scalar)
{
      VERTEXD result;
      
      result.x = x*scalar;
      result.y = y*scalar;
      result.z = z*scalar;
      
      return result;
}

VERTEXD VERTEXD::InvertR()
{
      VERTEXD result;
      
      result.x = -x;
      result.y = -y;
      result.z = -z;
      
      return result;
}

double VERTEXD::dot(VERTEXD b)
{
      return x*b.x + y*b.y + z*b.z;
}

VERTEXD VERTEXD::interpolatewith(VERTEXD other, double percent)
{
      VERTEXD vout;
      
      vout.x = (other.x - x)*percent + x;
      vout.y = (other.y - y)*percent + y;
      vout.z = (other.z - z)*percent + z;
      
      return vout;
}

double * VERTEXD::v3()
{
      double3[0] = x;
      double3[1] = y;
      double3[2] = z;
      
      return double3;
}

void VERTEXD::DebugPrint()
{
      cout << x << "," << y << "," << z << endl;
}

void VERTEXD::zero()
{
      x = y = z = 0.0f;
}

void VERTEXD::Set(double * f3)
{
      x = f3[0];
      y = f3[1];
      z = f3[2];
}

void VERTEXD::Set(float * f3)
{
      x = f3[0];
      y = f3[1];
      z = f3[2];
}

bool VERTEXD::nan()
{
      if (!(x < 0 || x >= 0) || !(y < 0 || y >= 0) || !(z < 0 || z >= 0))
            return true;
      
      return false;
}

VERTEXD::VERTEXD(VERTEX other)
{
      x = other.x;
      y = other.y;
      z = other.z;
}

VERTEXD& VERTEXD:: operator= (const VERTEX & other)
{
      x = other.x;
      y = other.y;
      z = other.z;
      
      return *this;
}

void AABB::SetFromCorners(VERTEX c1, VERTEX c2)
{
      //ensure c1 is smaller than c2
      float temp;
      if (c1.x > c2.x)
      {
            temp = c1.x;
            c1.x = c2.x;
            c2.x = temp;
      }
      if (c1.y > c2.y)
      {
            temp = c1.y;
            c1.y = c2.y;
            c2.y = temp;
      }
      if (c1.z > c2.z)
      {
            temp = c1.z;
            c1.z = c2.z;
            c2.z = temp;
      }
      
      float fluff = 0.0001;
      VERTEX fluffer;
      fluffer.Set(fluff, fluff, fluff);
      
      //add in some fudge factor
      c1 = c1 + fluffer.ScaleR(-1.0);
      c2 = c2 + fluffer;
      
      pos = c1;
      size = c2 - c1;
      center = pos + size.ScaleR(0.5);
}

bool AABB::IntersectRay(VERTEX orig, VERTEX dir)
{
      if (orig.x < pos.x)
            return false;
      if (orig.z < pos.z)
            return false;
      if (orig.x > (pos+size).x)
            return false;
      if (orig.z > (pos+size).z)
            return false;
      
      return true;
      
      VERTEX abs_segdir, abs_cross;
      float f;
      VERTEX diff;
      diff = orig - center;
      
      abs_segdir = dir;
      abs_segdir.flabs();
      VERTEX abs_diff;
      abs_diff = diff;
      abs_diff.flabs();
      
      if ( abs_diff.x > size.x && diff.x*dir.x>=0.0f )
            return false;
      if ( abs_diff.y > size.y && diff.y*dir.y>=0.0f )
            return false;
      if ( abs_diff.z > size.z && diff.z*dir.z>=0.0f )
            return false;
      
      VERTEX cross;
      cross = dir.cross(diff);
      abs_cross = cross;
      abs_cross.flabs();
      
      f = size.y*abs_segdir.z + size.z*abs_segdir.y;
      if ( abs_cross.x > f ) return false;
            
      f = size.z*abs_segdir.x + size.x*abs_segdir.z;
      if ( abs_cross.y > f ) return false;
            
      f = size.x*abs_segdir.y + size.y*abs_segdir.x;
      if ( abs_cross.z > f ) return false;
      
      return true;
}

bool AABB::IntersectSegment(VERTEX orig, VERTEX dir, float seglen)
{
      if (seglen>3e30f) return IntersectRay(orig, dir); // infinite ray
      VERTEX abs_segdir, abs_diff, abs_cross; 
      
      VERTEX segdir;
      segdir = dir.ScaleR(0.5f * seglen);
      VERTEX seg_center;
      seg_center = orig + segdir;
      VERTEX diff;
      diff = seg_center - center;
      
      abs_segdir.x = fabs(segdir.x);
      abs_segdir.y = fabs(segdir.y);
      abs_segdir.z = fabs(segdir.z);
      abs_diff.x=fabs(diff.x);
      abs_diff.y=fabs(diff.y);
      abs_diff.z=fabs(diff.z);
      float f=size.x + abs_segdir.x;
      if (abs_diff.x > f) return false;
      f=size.y + abs_segdir.y;
      if (abs_diff.y > f) return false;
      f=size.z + abs_segdir.z;
      if (abs_diff.z > f) return false;
      
      VERTEX cross;
      cross = segdir.cross(diff);
      
      abs_cross.x = fabs(cross.x);
      abs_cross.y = fabs(cross.y);
      abs_cross.z = fabs(cross.z);
      
      f = size.y*abs_segdir.z + size.z*abs_segdir.y;
      if ( abs_cross.x > f ) return false;
            
      f = size.z*abs_segdir.x + size.x*abs_segdir.z;
      if ( abs_cross.y > f ) return false;
            
      f = size.x*abs_segdir.y + size.y*abs_segdir.x;
      if ( abs_cross.z > f ) return false;
      
      return true;
}

void AABB::DebugPrint()
{
      cout << "pos:    ";
      pos.DebugPrint();
      cout << "size:   ";
      size.DebugPrint();
      cout << "center: ";
      center.DebugPrint();
}

bool AABB::IntersectAABB(AABB & other)
{
      VERTEX otherc1 = other.GetPos();
      VERTEX otherc2 = otherc1 + other.GetSize();
      
      VERTEX c1 = pos;
      VERTEX c2 = pos + size;
      
      //bias checks for non-collisions
      if (c1.x > otherc2.x || c2.x < otherc1.x)
            return false;
      
      if (c1.z > otherc2.z || c2.z < otherc1.z)
            return false;
      
      if (c1.y > otherc2.y || c2.y < otherc1.y)
            return false;
      
      return true;
}

void AABB::GetVerts(VERTEX * eightverts)
{
      VERTEX temp;
      
      eightverts[0] = pos;
      
      temp.Set(size.x,0,0);
      eightverts[1] = pos+temp;
      
      temp.Set(size.x,size.y,0);
      eightverts[2] = pos+temp;
      
      temp.Set(0,size.y,0);
      eightverts[3] = pos+temp;
      
      temp.Set(0,0,size.z);
      eightverts[4] = pos+temp;
      
      temp.Set(size.x,0,size.z);
      eightverts[5] = pos+temp;
      
      temp.Set(size.x,size.y,size.z);
      eightverts[6] = pos+temp;
      
      temp.Set(0,size.y,size.z);
      eightverts[7] = pos+temp;
}

Generated by  Doxygen 1.6.0   Back to index