quat.c

模型冲突检测

/*****************************************************************************
 *
    quat.c-  code for quaternion-specific routines.

    (see quat.h for revision history and more documentation.)
 *
 *****************************************************************************/


#include "quat.h"

/* define local X, Y, Z, W to override any external definition;  don't use
 *  Q_X, etc, because it makes this code a LOT harder to read;  don't use 
 *  pphigs definition of X-W since it could theoretically change.
 */

#undef X
#undef Y
#undef Z
#undef W

#define X   	Q_X
#define Y   	Q_Y
#define Z   	Q_Z
#define W   	Q_W


/* version information for use with "ident"	*/
static char rcsid[] = 
"$Id: quat.c,v 2.22 1997/01/09 23:08:07 livingst Exp $";


/*****************************************************************************
 *
   q_print- print a quaternion
 *
 *****************************************************************************/

void
q_print(q_type quat)
{
    printf("  [ (%lf, %lf, %lf), %lf ]\n", 
    	    quat[X], quat[Y], quat[Z], quat[W]);

}	/* q_print */




/*****************************************************************************
 * 
    q_xform: Compute quaternion product destQuat = q * vec * qinv.
   	    vec can be same storage as destVec
 *
 *****************************************************************************/

void 
q_xform(q_vec_type destVec, q_type q, q_vec_type vec)
{
    q_type  inverse;
    q_type  vecQuat;
    q_type  tempVecQuat;
    q_type  resultQuat;

    /* copy vector into temp quaternion for multiply	*/
    q_from_vec(vecQuat, vec);

    /* invert multiplier	*/
    q_invert(inverse, q);

    /* do q * vec * q(inv)	*/
    q_mult(tempVecQuat, q, vecQuat);
    q_mult(resultQuat, tempVecQuat, inverse);

    /* back to vector land	*/
    q_to_vec(destVec, resultQuat);

}   /* q_xform	*/




/*****************************************************************************
 * q_mult: Compute quaternion product destQuat = qLeft * qRight.
 *  	    destQuat can be same as either qLeft or qRight or both.
 *****************************************************************************/
void 
q_mult(q_type destQuat, q_type qLeft, q_type qRight)
{
    q_type  tempDest;

    tempDest[W] = qLeft[W]*qRight[W] - qLeft[X]*qRight[X] - 
    	          qLeft[Y]*qRight[Y] - qLeft[Z]*qRight[Z];

    tempDest[X] = qLeft[W]*qRight[X] + qLeft[X]*qRight[W] + 
    	          qLeft[Y]*qRight[Z] - qLeft[Z]*qRight[Y];

    tempDest[Y] = qLeft[W]*qRight[Y] + qLeft[Y]*qRight[W] + 
    	          qLeft[Z]*qRight[X] - qLeft[X]*qRight[Z];

    tempDest[Z] = qLeft[W]*qRight[Z] + qLeft[Z]*qRight[W] + 
    	          qLeft[X]*qRight[Y] - qLeft[Y]*qRight[X];

    q_copy(destQuat, tempDest);

}   /* q_mult	*/



/*****************************************************************************
 * q_copy: copy quaternion q to destQuat
 *****************************************************************************/
void 
q_copy(q_type destQuat, q_type srcQuat)
{
    destQuat[X] = srcQuat[X];
    destQuat[Y] = srcQuat[Y];
    destQuat[Z] = srcQuat[Z];
    destQuat[W] = srcQuat[W];

}   /* q_copy	*/


/*****************************************************************************
 * q_from_vec- convert vec to quaternion
 *****************************************************************************/

void 
q_from_vec(q_type destQuat, q_vec_type vec)
{
    destQuat[X] = vec[X];
    destQuat[Y] = vec[Y];
    destQuat[Z] = vec[Z];
    destQuat[W] = 0.0;

}   /* q_from_vec   */


/*****************************************************************************
 * q_to_vec- convert quaternion to vector;  q[W] is ignored
 *****************************************************************************/
void 
q_to_vec(q_vec_type destVec, q_type srcQuat)
{
    destVec[X] = srcQuat[X];
    destVec[Y] = srcQuat[Y];
    destVec[Z] = srcQuat[Z];

}   /* q_to_vec	*/


/*****************************************************************************
 * q_normalize-  normalize quaternion.  src and dest can be same
 *****************************************************************************/
void 
q_normalize(q_type destQuat, q_type srcQuat)
{
    double normalizeFactor;

    normalizeFactor = 1.0 / sqrt( srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
      	    	    	          srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W] );

    destQuat[X] = srcQuat[X] * normalizeFactor;
    destQuat[Y] = srcQuat[Y] * normalizeFactor;
    destQuat[Z] = srcQuat[Z] * normalizeFactor;
    destQuat[W] = srcQuat[W] * normalizeFactor;

}   /* q_normalize  */



/*****************************************************************************
 * q_conjugate- conjugate quaternion.  src == dest ok.
 *****************************************************************************/
void 
q_conjugate(q_type destQuat, q_type srcQuat)
{
    destQuat[X] = -srcQuat[X];
    destQuat[Y] = -srcQuat[Y];
    destQuat[Z] = -srcQuat[Z];
    destQuat[W] =  srcQuat[W];

}   /* q_conjugate  */


/*****************************************************************************
 * q_invert: Invert quaternion; That is, form its multiplicative inverse.
 *  	    	src == dest ok.
 *****************************************************************************/
void 
q_invert(q_type destQuat, q_type srcQuat)
{
    double srcQuatNorm;

    srcQuatNorm = 1.0 / (srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	    	         srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W]);

    destQuat[X] = -srcQuat[X] * srcQuatNorm;
    destQuat[Y] = -srcQuat[Y] * srcQuatNorm;
    destQuat[Z] = -srcQuat[Z] * srcQuatNorm;
    destQuat[W] =  srcQuat[W] * srcQuatNorm;

}   /* q_invert	*/


/*****************************************************************************
 * q_exp: Exponentiate quaternion, assuming scalar part 0.  src == dest ok.
 *****************************************************************************/
void 
q_exp(q_type destQuat, q_type srcQuat)
{
    double theta, scale;

    theta = sqrt(srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	         srcQuat[Z]*srcQuat[Z]);

    if (theta > Q_EPSILON)
        scale = sin(theta)/theta;
    else
        scale = 1.0;

    destQuat[X] = scale*srcQuat[X];
    destQuat[Y] = scale*srcQuat[Y];
    destQuat[Z] = scale*srcQuat[Z];
    destQuat[W] = cos(theta);

}   /* q_exp	*/



/*****************************************************************************
 * q_log: Take the natural logarithm of unit quaternion.  src == dest ok.
 *****************************************************************************/
void 
q_log(q_type destQuat, q_type srcQuat)
{
    double theta, scale;

    scale = sqrt(srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	    	    	    	         srcQuat[Z]*srcQuat[Z]);
    theta = atan2(scale, srcQuat[W]);

    if (scale > 0.0)
        scale = theta/scale;

    destQuat[X] = scale * srcQuat[X];
    destQuat[Y] = scale * srcQuat[Y];
    destQuat[Z] = scale * srcQuat[Z];
    destQuat[W] = 0.0;

}   /* q_log	*/




/*****************************************************************************
 *
   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.
 *
 *****************************************************************************/
void 
q_slerp(q_type destQuat, q_type startQuat, q_type endQuat, double t)
{
    q_type  startQ;  	    	    	/* temp copy of startQuat	*/
    double  omega, cosOmega, sinOmega;
    double  startScale, endScale;
    int     i;

    q_copy(startQ, startQuat);

    cosOmega = startQ[X]*endQuat[X] + startQ[Y]*endQuat[Y] + 
    	       startQ[Z]*endQuat[Z] + startQ[W]*endQuat[W];

    /* If the above dot product is negative, it would be better to
     *  go between the negative of the initial and the final, so that
     *  we take the shorter path.  
     */
    if ( cosOmega < 0.0 ) 
        {
        cosOmega *= -1;
        for (i = X; i <= W; i++)
	        startQ[i] *= -1;
        }

    if ( (1.0 + cosOmega) > Q_EPSILON ) 
        {
        /* usual case */
        if ( (1.0 - cosOmega) > Q_EPSILON ) 
    	    {
	    /* usual case */
	    omega = acos(cosOmega);
	    sinOmega = sin(omega);
	    startScale = sin((1.0 - t)*omega) / sinOmega;
	    endScale = sin(t*omega) / sinOmega;
    	    } 
        else 
    	    {
	    /* ends very close */
	    startScale = 1.0 - t;
	    endScale = t;
    	    }
        for (i = X; i <= W; i++)
	    destQuat[i] = startScale*startQ[i] + endScale*endQuat[i];
        } 
    else 
        {
        /* ends nearly opposite */
        destQuat[X] = -startQ[Y];  
        destQuat[Y] =  startQ[X];
        destQuat[Z] = -startQ[W];  
        destQuat[W] =  startQ[Z];
    
        startScale = sin((0.5 - t) * Q_PI);
        endScale = sin(t * Q_PI);
        for (i = X; i <= Z; i++)
	    destQuat[i] = startScale*startQ[i] + endScale*destQuat[i];
        }

}   /* q_slerp	*/



/*****************************************************************************
 * 
    q_make:  make a quaternion given an axis and an angle (in radians)
	 
    	notes:
	    - rotation is counter-clockwise when rotation axis vector is 
	    	pointing at you
    	    - if angle or vector are 0, the identity quaternion is returned.
	    
 *
 *****************************************************************************/
void
q_make(q_type destQuat, double x, double y, double z, double angle)
    /* destquat -- quat to be made  */
    /* x,y,z -- axis of rotation */
    /* angle -- angle of rotation about axis in radians */
{
    double length, cosA, sinA;

    /* normalize vector */
    length = sqrt( x*x + y*y + z*z );

    /* if zero vector passed in, just return identity quaternion	*/
    if ( length < Q_EPSILON )
        {
        destQuat[X] = 0;
        destQuat[Y] = 0;
        destQuat[Z] = 0;
        destQuat[W] = 1;
        return;
        }

    x /= length;
    y /= length;
    z /= length;

    cosA = cos(angle / 2.0);
    sinA = sin(angle / 2.0);

    destQuat[W] = cosA;
    destQuat[X] = sinA * x;
    destQuat[Y] = sinA * y;
    destQuat[Z] = sinA * z;

}   /* q_make */



/*****************************************************************************
 *
   q_from_two_vecs - create a quaternion from two vectors that rotates
    	    	     	v1 to v2 about an axis perpendicular to both
 *
 *****************************************************************************/
void q_from_two_vecs( q_type destQuat, q_vec_type v1, q_vec_type v2 )
{
    q_vec_type u1, u2 ;
    q_vec_type axis ;					 /* axis of rotation */
    double theta ;				     /* angle of rotation about axis */
    double theta_complement ;
    double crossProductMagnitude ;

    /*
    ** Normalize both vectors and take cross product to get rotation axis. 
    */
    q_vec_normalize( u1, v1 ) ;
    q_vec_normalize( u2, v2 ) ;
    q_vec_cross_product( axis, u1, u2 ) ;

    /*
    ** | u1 X u2 | = |u1||u2|sin(theta)
    **
    ** Since u1 and u2 are normalized, 
    **
    **  theta = arcsin(|axis|)
    */
    crossProductMagnitude = sqrt( q_vec_dot_product( axis, axis ) ) ;

    /*
    ** Occasionally, even though the vectors are normalized, the magnitude will
    ** be calculated to be slightly greater than one.  If this happens, just
    ** set it to 1 or asin() will barf.
    */
    if( crossProductMagnitude > 1.0 )
       crossProductMagnitude = 1.0 ;

    /*
    ** Take arcsin of magnitude of rotation axis to compute rotation angle.
    ** Since crossProductMagnitude=[0,1], we will have theta=[0,pi/2].
    */
    theta = asin( crossProductMagnitude ) ;
    theta_complement = Q_PI - theta ;

    /*
    ** If cos(theta) < 0, use complement of theta as rotation angle.
    */
    if( q_vec_dot_product(u1, u2) < 0.0 )
       {
       theta = theta_complement ;
       theta_complement = Q_PI - theta ;
       }

    /* if angle is 0, just return identity quaternion   */
    if( theta < Q_EPSILON )
       {
       destQuat[Q_X] = 0.0 ;
       destQuat[Q_Y] = 0.0 ;
       destQuat[Q_Z] = 0.0 ;
       destQuat[Q_W] = 1.0 ;
       }
    else
       {
       if( theta_complement < Q_EPSILON )
          {
          /*
          ** The two vectors are opposed.  Find some arbitrary axis vector.
          ** First try cross product with x-axis if u1 not parallel to x-axis.
          */
          if( (u1[Y]*u1[Y] + u1[Z]*u1[Z]) >= Q_EPSILON )
	     {
	     axis[X] = 0.0 ;
	     axis[Y] = u1[Z] ;
	     axis[Z] = -u1[Y] ;
	     }
          else
	     {
	     /*
	     ** u1 is parallel to to x-axis.  Use z-axis as axis of rotation.
	     */
	     axis[X] = axis[Y] = 0.0 ;
	     axis[Z] = 1.0 ;
	     }
          }

       q_vec_normalize( axis, axis ) ;
       q_make( destQuat, axis[Q_X], axis[Q_Y], axis[Q_Z], theta ) ;
       q_normalize( destQuat, destQuat ) ;
       }
}	/* q_from_two_vecs */


/*****************************************************************************
 *  
    q_from_euler - converts 3 euler angles (in radians) to a quaternion
     
	angles are in radians;  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.
 *
 *****************************************************************************/

void
q_from_euler(q_type destQuat, double yaw, double pitch, double roll)
{
    double  cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
    double  half_roll, half_pitch, half_yaw;


    /* put angles into radians and divide by two, since all angles in formula
     *  are (angle/2)
     */

    half_yaw = yaw / 2.0;
    half_pitch = pitch / 2.0;
    half_roll = roll / 2.0;

    cosYaw = cos(half_yaw);
    sinYaw = sin(half_yaw);

    cosPitch = cos(half_pitch);
    sinPitch = sin(half_pitch);

    cosRoll = cos(half_roll);