glquat.c

国外游戏开发者杂志1998年第二期配套代码

/*SDOC*********************************************************************

  $Header$

  Module:	GLquat.c

  Author:	Nick Bobic

  Copyright (c) 1997 Nick Bobic 
                                               
  Description: Common (and not so common) Quaternion Functions

*********************************************************************EDOC*/

/*SDOC*********************************************************************

  Revision Record

	Date		Auth	Changes
	====		====	=======

	16AUG97		NB		Created file
	17AUG97		NB		Added some exotic functions

**********************************************************************EDOC*/




// includes
#include "GLquat.h"
#include <math.h>

// some math.h do not have M_PI definition
#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288419716939937510f
#endif


#define DELTA 1e-6     // error tolerance

#if defined (WIN32)
#pragma warning (disable:4244)	// disable conversion warnings (dbl -> fl)
#endif


/*SDOC***********************************************************************

  Name:		gluQuatToMat_EXT

  Action:   Converts quaternion representation of a rotation to a matrix
			representation

  Params:   GL_QUAT* (our quaternion), GLfloat (4x4 matrix)

  Returns:  nothing

  Comments: remember matrix (in OGL) is represented in COLUMN major form

***********************************************************************EDOC*/
void APIENTRY gluQuatToMat_EXT(GL_QUAT * quat, GLfloat m[4][4])
{

  GLfloat wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

  x2 = quat->x + quat->x; y2 = quat->y + quat->y; z2 = quat->z + quat->z;
  xx = quat->x * x2;   xy = quat->x * y2;   xz = quat->x * z2;
  yy = quat->y * y2;   yz = quat->y * z2;   zz = quat->z * z2;
  wx = quat->w * x2;   wy = quat->w * y2;   wz = quat->w * z2;

  m[0][0] = 1.0 - (yy + zz);
  m[0][1] = xy - wz;
  m[0][2] = xz + wy;
  m[0][3] = 0.0;
 
  m[1][0] = xy + wz;
  m[1][1] = 1.0 - (xx + zz);
  m[1][2] = yz - wx;
  m[1][3] = 0.0;

  m[2][0] = xz - wy;
  m[2][1] = yz + wx;
  m[2][2] = 1.0 - (xx + yy);
  m[2][3] = 0.0;

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

}


/*SDOC***********************************************************************

  Name:		gluEulerToQuat_EXT

  Action:   Converts representation of a rotation from Euler angles to
			quaternion representation

  Params:   GLfloat (roll), GLfloat (pitch), GLfloat (yaw), GL_QUAT* (quat)

  Returns:  nothing

  Comments: remember:	roll  - rotation around X axis
						pitch - rotation around Y axis
						yaw   - rotation around Z axis
			
			rotations are performed in the following order:
					yaw -> pitch -> roll

			Qfinal = Qyaw Qpitch Qroll

***********************************************************************EDOC*/
void APIENTRY gluEulerToQuat_EXT(GLfloat roll, GLfloat pitch, GLfloat yaw, 
													GL_QUAT * quat)
{
	
	GLfloat cr, cp, cy, sr, sp, sy, cpcy, spsy;

	cr = cos(roll/2);
	cp = cos(pitch/2);
	cy = cos(yaw/2);

	sr = sin(roll/2);
	sp = sin(pitch/2);
	sy = sin(yaw/2);
	
	cpcy = cp * cy;
	spsy = sp * sy;

	quat->w = cr * cpcy + sr * spsy;
	quat->x = sr * cpcy - cr * spsy;
	quat->y = cr * sp * cy + sr * cp * sy;
	quat->z = cr * cp * sy - sr * sp * cy;

}



/*SDOC***********************************************************************

  Name:		gluMatToQuat_EXT

  Action:   Converts matrix representation of a rotation to a quaternion
			representation

  Params:   GLfloat (matrix), GL_QUAT* (quat)

  Returns:  nothing

  Comments: remember matrix (in OGL) is represented in COLUMN major form

***********************************************************************EDOC*/
void APIENTRY gluMatToQuat_EXT(GLfloat m[4][4], GL_QUAT * quat)
{

  GLfloat  tr, s;
  GLfloat  q[4];
  GLint    i, j, k;

  int nxt[3] = {1, 2, 0};

  tr = m[0][0] + m[1][1] + m[2][2];

  // check the diagonal

  if (tr > 0.0) 
  {
    s = sqrt (tr + 1.0);

    quat->w = s / 2.0;
    
	s = 0.5 / s;

    quat->x = (m[1][2] - m[2][1]) * s;
    quat->y = (m[2][0] - m[0][2]) * s;
    quat->z = (m[0][1] - m[1][0]) * s;

  } else {		
	  
	  // diagonal is negative
    
	  i = 0;

      if (m[1][1] > m[0][0]) i = 1;
	  if (m[2][2] > m[i][i]) i = 2;
    
	  j = nxt[i];
      k = nxt[j];

      s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
      
	  q[i] = s * 0.5;

      if (s != 0.0) s = 0.5 / s;

	  q[3] = (m[j][k] - m[k][j]) * s;
      q[j] = (m[i][j] + m[j][i]) * s;
      q[k] = (m[i][k] + m[k][i]) * s;

	  quat->x = q[0];
	  quat->y = q[1];
	  quat->z = q[2];
	  quat->w = q[3];
  }

}


/*SDOC***********************************************************************

  Name:		gluQuatSlerp_EXT

  Action:	Smoothly (spherically, shortest path on a quaternion sphere) 
			interpolates between two UNIT quaternion positions

  Params:   GLQUAT (first and second quaternion), GLfloat (interpolation
			parameter [0..1]), GL_QUAT (resulting quaternion; inbetween)

  Returns:  nothing

  Comments: Most of this code is optimized for speed and not for readability

			As t goes from 0 to 1, qt goes from p to q.
		slerp(p,q,t) = (p*sin((1-t)*omega) + q*sin(t*omega)) / sin(omega)

***********************************************************************EDOC*/
void APIENTRY gluQuatSlerp_EXT(GL_QUAT * from, GL_QUAT * to, GLfloat t, 
															GL_QUAT * res)
{
        GLfloat           to1[4];
        GLdouble          omega, cosom, sinom;
        GLdouble          scale0, scale1;

        // calc cosine
        cosom = from->x * to->x + from->y * to->y + from->z * to->z
			       + from->w * to->w;

        // adjust signs (if necessary)
        if ( cosom < 0.0 )
		{
			cosom = -cosom;

			to1[0] = - to->x;
			to1[1] = - to->y;
			to1[2] = - to->z;
			to1[3] = - to->w;

        } else  {

			to1[0] = to->x;
			to1[1] = to->y;
			to1[2] = to->z;
			to1[3] = to->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
		res->x = scale0 * from->x + scale1 * to1[0];
		res->y = scale0 * from->y + scale1 * to1[1];
		res->z = scale0 * from->z + scale1 * to1[2];
		res->w = scale0 * from->w + scale1 * to1[3];

}



/*SDOC***********************************************************************

  Name:		gluQuatLerp_EXT

  Action:   Linearly interpolates between two quaternion positions

  Params:   GLQUAT (first and second quaternion), GLfloat (interpolation
			parameter [0..1]), GL_QUAT (resulting quaternion; inbetween)

  Returns:  nothing

  Comments: fast but not as nearly as smooth as Slerp

***********************************************************************EDOC*/
void APIENTRY gluQuatLerp_EXT(GL_QUAT * from, GL_QUAT * to, GLfloat t, 
															GL_QUAT * res)
{
        GLfloat           to1[4];
        GLdouble          cosom;
        GLdouble          scale0, scale1;

        // calc cosine
        cosom = from->x * to->x + from->y * to->y + from->z * to->z
			       + from->w * to->w;

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

        } else  {

			to1[0] = to->x;
			to1[1] = to->y;
			to1[2] = to->z;
			to1[3] = to->w;

        }

 
		// interpolate linearly
        scale0 = 1.0 - t;
        scale1 = t;
 
		// calculate final values
		res->x = scale0 * from->x + scale1 * to1[0];
		res->y = scale0 * from->y + scale1 * to1[1];
		res->z = scale0 * from->z + scale1 * to1[2];
		res->w = scale0 * from->w + scale1 * to1[3];

}



/*SDOC***********************************************************************

  Name:		gluQuatNormalize_EXT

  Action:   Normalizes quaternion (i.e. w^2 + x^2 + y^2 + z^2 = 1)

  Params:   GL_QUAT* (quaternion)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatNormalize_EXT(GL_QUAT *quat)
{
    GLfloat	dist, square;

	square = quat->x * quat->x + quat->y * quat->y + quat->z * quat->z
		+ quat->w * quat->w;
	
	if (square > 0.0)
		dist = (GLfloat)(1.0 / sqrt(square));
	else dist = 1;

    quat->x *= dist;
    quat->y *= dist;
    quat->z *= dist;
    quat->w *= dist;
}



/*SDOC***********************************************************************

  Name:		gluQuatGetValue_EXT

  Action:   Disassembles quaternion to an axis and an angle

  Params:   GL_QUAT* (quaternion), GLfloat* (x, y, z - axis), GLfloat (angle)

  Returns:  nothing

  Comments: NOTE: vector has been split into x, y, z so that you do not have
			to change your vector library (i.e. greater portability)

			NOTE2: angle is in RADIANS

***********************************************************************EDOC*/
void APIENTRY gluQuatGetValue_EXT(GL_QUAT *quat, GLfloat *x, GLfloat *y, 
											GLfloat *z, GLfloat *radians)
{
    GLfloat	len;
    GLfloat tx, ty, tz;

	// cache variables
	tx = quat->x;
	ty = quat->y;
	tz = quat->z;

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

    if (len > DELTA) 
	{
		*x = tx * (1.0f / len);
		*y = ty * (1.0f / len);
		*z = tz * (1.0f / len);
	    *radians = (GLfloat)(2.0 * acos(quat->w));
    }

    else {
		*x = 0.0;
		*y = 0.0;
		*z = 1.0;
	    *radians = 0.0;
    }
}


/*SDOC***********************************************************************

  Name:		gluQuatSetValue_EXT

  Action:   Assembles quaternion from an axis and an angle

  Params:   GL_QUAT* (quaternion), GLfloat (x, y, z - axis), GLfloat (angle)

  Returns:  nothing

  Comments: NOTE: vector has been split into x, y, z so that you do not have
			to change your vector library (i.e. greater portability)

			NOTE2: angle has to be in RADIANS

***********************************************************************EDOC*/
void APIENTRY gluQuatSetValue_EXT(GL_QUAT *quat, GLfloat x, GLfloat y, 
												GLfloat z, GLfloat angle)
{
	GLfloat temp, dist;

	// normalize
	temp = x*x + y*y + z*z;

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

    x *= dist;
    y *= dist;
    z *= dist;

	quat->x = x;
	quat->y = y;
	quat->z = z;

	quat->w = (GLfloat)cos(angle / 2.0f);
	
}



/*SDOC***********************************************************************

  Name:		gluQuatScaleAngle_EXT

  Action:   Scales the rotation angle of a quaternion

  Params:   GL_QUAT* (quaternion), GLfloat (scale value)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatScaleAngle_EXT(GL_QUAT * quat, GLfloat scale)
{
    GLfloat x, y, z;	// axis
    GLfloat angle;		// and angle

	gluQuatGetValue_EXT(quat, &x, &y, &z, &angle);

    gluQuatSetValue_EXT(quat, x, y, z, (angle * scale));
}



/*SDOC***********************************************************************

  Name:		gluQuatInverse_EXT

  Action:   Inverts quaternion's rotation ( q^(-1) )