quat.h

模型冲突检测

/* prevent multiple includes	*/
#ifndef Q_INCLUDED
#define Q_INCLUDED


/*****************************************************************************
 *
    quat.h -  include file for quaternion, vector and matrix routines.  

    
    Overview:
    
    	quatlib is a library of routines that implements a grab-bag of 
	useful routines for dealing with quaternions, vectors, and 
	matrices.  See the quatlib man page for an overview.


    Notes:

      - to address the quaternion elements, use the Q_X, Q_Y, Q_Z and Q_W
    	#defines from this file, or X,Y,Z,W from pphigs.h, although the
	latter is not guaranteed to work forever.
      
      - to find out which version of the library you're using, do:
	    
      	    % ident  <path>/libquat.a
	
	(this information is in the rcsid string in quat.c)
    
      - see /afs/unc/proj/hmd/src/quat/{quat,vector,matrix}.c 
        for implementation details.


    Conventions:

      - general-purpose quaternion routines start with q_

      - pphigs-specific routines start with qp_ and expect data
	   types as defined in pphigs.h (vectors are currently 
	   arrays of floats)

      - all non-integer values are doubles by default-  the exceptions
      	to this are routines dealing with PPHIGS vectors and matrices,
	which use floats.
      
      - vector routines have the string "vec" somewhere in their name
      
      - matrix routines have the string "matrix" somewhere in their name

      - all matrices are 4x4 unless otherwise noted by qp_ prefix

      - positive rotation directions are as follows:

		about Z axis: from X axis to Y axis
		about X axis: from Y axis to Z axis
		about Y axis: from Z axis to X axis

      - all angles are specified in radians

      - destination parameter (if any) is always first argument (as in
      	    Unix string routines)

      - src and dest parameters can always be the same, as long as they 
      	    are of the same type (copying is done if necessary) 

      - naming conventions for conversion routines:
      
	    q{p}_{to,from}_whatever for routines involving quaternions
	    q{p}_x_to_y for all others (ie., no "from" is used)


   RCS Header:
   $Id: quat.h,v 2.27 1997/01/09 23:08:07 livingst Exp $
   
   Revision History (for whole library, not just this file):

   Author	    	Date	  Comments
   ------	    	--------  ----------------------------
   Mark Livingston      01/09/96   Added routines for OpenGL matrices
   Rich Holloway    	09/27/93   Added Gary Bishop's matrix to euler rtn
   Rich Holloway    	07/16/92   Added q_euler_to_col_matrix(), routines
    	    	    	    	    for working with GL matrices, added
				    documentation for euler angle routines
   Erik Erikson/        06/26/92   Added q_xyz_quat_compose
   Stefan Gottschalk/
   Russ Taylor
   Rich Holloway    	05/13/92   Added Q_NULL_VECTOR, Q_ID_MATRIX
   Jon Leech/           04/29/92   Added CM_ prototypes
   Erik Erikson
   
   Rich Holloway	09/21/90   Made into library, made all matrices 4x4,
  	    	    	    	    added matrix routines for 
    	    	    	    	    4x4 (standard) or 3x4 (for PPHIGS),
				    changed names of 
    	    	    	    	    routines (to avoid name conflicts with
				    non-library routines) by prefixing
				    everything with "q_".
   
   Russ Taylor	    	 1990	   Modified q_slerp to pick shortest path
    	    	    	    	    between two angles
   
   Warren Robinett  	12/89 	   Added PPHIGS support routines
   
   Ken Shoemake	    	 1985      Initial version
 
 *
 *****************************************************************************/


#include <stdio.h>
#include <math.h>
#include <cm_macros.h>
#include <pdefs.h>

/*****************************************************************************
 *
    #defines
 *
 *****************************************************************************/

/* for accessing the elements of q_type and q_vec_type	*/
#define Q_X 	0
#define Q_Y 	1
#define Q_Z 	2
#define Q_W 	3

/* tolerance for quaternion operations	*/
#define	Q_EPSILON   (1e-10)

/* min and max macros	*/
#define Q_MAX(x, y)       ( ((x) > (y)) ? (x) : (y) )
#define Q_MIN(x, y)       ( ((x) < (y)) ? (x) : (y) )

#define Q_ABS(x)       ( ((x) > 0 ) ? (x) : (-(x)) )

/* 
 * use local definition of PI for machines that have no def in math.h; this
 *  value stolen from DEC Ultrix 4.1 math.h
 */
#define Q_PI    3.14159265358979323846

#define Q_ID_QUAT   { 0.0, 0.0, 0.0, 1.0 }

#define Q_ID_MATRIX { {1.0, 0.0, 0.0, 0.0}, \
    	    	      {0.0, 1.0, 0.0, 0.0}, \
		      {0.0, 0.0, 1.0, 0.0}, \
		      {0.0, 0.0, 0.0, 1.0} }

#define QP_ID_MATRIX { {1.0, 0.0, 0.0, 0.0}, \
    	    	       {0.0, 1.0, 0.0, 0.0}, \
		       {0.0, 0.0, 1.0, 0.0} }

#define Q_NULL_VECTOR	{ 0.0, 0.0, 0.0 }

/* 
 * degree/radian conversion
 */
#define Q_DEG_TO_RAD(deg)       ( ((deg)*Q_PI)/180.0 )
#define Q_RAD_TO_DEG(rad)       ( (((rad)*180.0)/Q_PI) )

/* compatibility w/ old for a while */
#define q_vec_set   q_set_vec


/*****************************************************************************
 *
    typedefs
 *
 *****************************************************************************/

/* basic quaternion type- scalar part is last element in array    */
typedef double	q_type[4];

/* basic vector type	*/
typedef double	q_vec_type[3];

/* for row and column matrices	*/
typedef double	q_matrix_type[4][4];

/* for working with gl or other 4x4 float matrices  */
typedef float	qgl_matrix_type[4][4];

/* for working with OpenGL matrices - these are really just like row matrices
** (i.e. same bits in same order), but the decl is a 1-D array, not 2-D, sigh
*/
typedef double  qogl_matrix_type[16];

/* special transformation type using quaternions and vectors	*/
typedef struct  q_xyz_quat_struct
    {
    q_vec_type	xyz;  /* translation  */
    q_type  	quat;  /* rotation	    */
    } q_xyz_quat_type;



/*****************************************************************************
 *****************************************************************************
 *
    function declarations
 *
 *****************************************************************************
 *****************************************************************************/



/*****************************************************************************
 *
    strictly quaternion operations
 *
 *****************************************************************************/

/*  prints a quaternion	*/
CM_EXTERN_FUNCTION( void q_print, ( q_type quat ));

/* make a quaternion given an axis and an angle;  x,y,z is axis of 
 *  rotation;  angle is angle of rotation in radians (see also q_from_two_vecs)
 *
 *  rotation is counter-clockwise when rotation axis vector is 
 *	    	pointing at you
 *
 * if angle or vector are 0, the identity quaternion is returned.
 */
CM_EXTERN_FUNCTION( void q_make, ( q_type destQuat,
				   double x,  double y,  double z,
				   double angle ));

/*  copy srcQuat to destQuat    */
CM_EXTERN_FUNCTION( void q_copy, ( q_type destQuat, q_type srcQuat ));

/* normalizes quaternion;  src and dest can be same */
CM_EXTERN_FUNCTION( void q_normalize, ( q_type destQuat, 
				        q_type srcQuat ));

/* invert quat;  src and dest can be the same	*/
CM_EXTERN_FUNCTION( void q_invert, ( q_type destQuat, q_type srcQuat ));

/*
 * computes quaternion product destQuat = qLeft * qRight.
 *  	    destQuat can be same as either qLeft or qRight or both.
 */
CM_EXTERN_FUNCTION( void q_mult, ( q_type destQuat, 
				   q_type qLeft,  
				   q_type qRight ));

/* conjugate quat; src and dest can be same */
CM_EXTERN_FUNCTION( void q_conjugate, ( q_type destQuat, 
				        q_type srcQuat ));

/* take natural log of unit quat; src and dest can be same  */
CM_EXTERN_FUNCTION( void q_log, ( q_type destQuat, 
				  q_type srcQuat ));

/* exponentiate quaternion, assuming scalar part 0.  src can be same as dest */
CM_EXTERN_FUNCTION( void q_exp, ( q_type destQuat, 
				  q_type srcQuat ));


/*
 * q_slerp: Spherical linear interpolation of unit quaternions.
 *
 *  	As t goes from 0 to 1, destQuat goes from startQ to endQuat.
 *      This routine should always return a point along the shorter
 *  	of the two paths between the two.  That is why the vector may be
 *  	negated in the end.
 *  	
 *  	src == dest should be ok, although that doesn't seem to make much
 *  	sense here.
 */
CM_EXTERN_FUNCTION( void q_slerp, ( q_type       destQuat, 
				    q_type startQuat,
				    q_type endQuat, 
				    double       t ));

/*****************************************************************************
 *  
    q_from_euler - converts 3 euler angles (in radians) to a quaternion
     
	Assumes roll is rotation about X, pitch
	is rotation about Y, yaw is about Z.  Assumes order of 
	yaw, pitch, roll applied as follows:
	    
	    p' = roll( pitch( yaw(p) ) )

    	See comments for q_euler_to_col_matrix for more on this.
 *
 *****************************************************************************/
CM_EXTERN_FUNCTION( void q_from_euler, ( q_type destQuat, 
					 double yaw, 
					 double pitch, 
					 double roll ));


/*****************************************************************************
 *
    mixed quaternion operations:  conversions to and from vectors & matrices
 *
 *****************************************************************************/

/* destVec = q * vec * q(inverse);  vec can be same storage as destVec	*/
CM_EXTERN_FUNCTION( void q_xform, ( q_vec_type destVec, 
				    q_type q,
				    q_vec_type vec ));