quat.c

模型冲突检测

    sinRoll = sin(half_roll);


    destQuat[X] = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    destQuat[Y] = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    destQuat[Z] = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

    destQuat[W] = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;

}	/* q_from_euler */




/*****************************************************************************
 * 
    qp_to_matrix: Convert quaternion to 3x4 PPHIGS rotation matrix.
   	    	    Quaternion need not be unit magnitude.
 
    notes:
    	- a call to this function took about 20-50us on Sun4's as of 7/92
 *
 *****************************************************************************/
void 
qp_to_matrix(Q_MatrixType destMatrix, q_type srcQuat)
{
    double s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz;

    /* For unit srcQuat, just set s = 2.0; or set xs = srcQuat[X] + 
     *   srcQuat[X], etc. 
     */
    s = 2.0 / (srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
	       srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W]);

    xs = srcQuat[X] * s;   ys = srcQuat[Y] * s;   zs = srcQuat[Z] * s;
    wx = srcQuat[W] * xs;  wy = srcQuat[W] * ys;  wz = srcQuat[W] * zs;
    xx = srcQuat[X] * xs;  xy = srcQuat[X] * ys;  xz = srcQuat[X] * zs;
    yy = srcQuat[Y] * ys;  yz = srcQuat[Y] * zs;  zz = srcQuat[Z] * zs;

    /* set up 4x4 destMatrixrix */
    destMatrix[X][X] = 1.0 - (yy + zz);
    destMatrix[X][Y] = xy - wz;
    destMatrix[X][Z] = xz + wy;
    destMatrix[X][W] = 0.0;

    destMatrix[Y][X] = xy + wz;
    destMatrix[Y][Y] = 1.0 - (xx + zz);
    destMatrix[Y][Z] = yz - wx;
    destMatrix[Y][W] = 0.0;

    destMatrix[Z][X] = xz - wy;
    destMatrix[Z][Y] = yz + wx;
    destMatrix[Z][Z] = 1.0 - (xx + yy);
    destMatrix[Z][W] = 0.0;

}   /* qp_to_matrix */


/*****************************************************************************
 * 
    q_to_row_matrix: Convert quaternion to 4x4 row-major rotation matrix.
  	    	    Quaternion need not be unit magnitude.
 *
 *****************************************************************************/
void 
q_to_row_matrix(q_matrix_type destMatrix, q_type srcQuat)
{
    double  s;
    double  xs, ys, zs,
    	    wx, wy, wz,
    	    xx, xy, xz,
	        yy, yz, zz;

    /* 
     * For unit srcQuat, just set s = 2.0; or set xs = srcQuat[X] + 
     *   srcQuat[X], etc. 
     */

    s = 2.0 / (srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	       srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W]);

    xs = srcQuat[X] * s;   ys = srcQuat[Y] * s;   zs = srcQuat[Z] * s;
    wx = srcQuat[W] * xs;  wy = srcQuat[W] * ys;  wz = srcQuat[W] * zs;
    xx = srcQuat[X] * xs;  xy = srcQuat[X] * ys;  xz = srcQuat[X] * zs;
    yy = srcQuat[Y] * ys;  yz = srcQuat[Y] * zs;  zz = srcQuat[Z] * zs;

    /* set up 4x4 destMatrixrix */
    destMatrix[X][X] = 1.0 - (yy + zz);
    destMatrix[X][Y] = xy + wz;
    destMatrix[X][Z] = xz - wy;
    destMatrix[X][W] = 0.0;

    destMatrix[Y][X] = xy - wz;
    destMatrix[Y][Y] = 1.0 - (xx + zz);
    destMatrix[Y][Z] = yz + wx;
    destMatrix[Y][W] = 0.0;

    destMatrix[Z][X] = xz + wy;
    destMatrix[Z][Y] = yz - wx;
    destMatrix[Z][Z] = 1.0 - (xx + yy);
    destMatrix[Z][W] = 0.0;

    destMatrix[W][X] = 0.0;
    destMatrix[W][Y] = 0.0;
    destMatrix[W][Z] = 0.0;
    destMatrix[W][W] = 1.0;

}   /* q_to_row_matrix */



/*****************************************************************************
 * q_to_col_matrix: Convert quaternion to 4x4 column-major rotation matrix.
 *  	    	    Quaternion need not be unit magnitude.
 *****************************************************************************/
void 
q_to_col_matrix(q_matrix_type destMatrix, q_type srcQuat)
{
    double s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz;

    /* For unit srcQuat, just set s = 2.0; or set xs = srcQuat[X] + 
     *  srcQuat[X], etc. 
     *****************************************************************************/
    s = 2.0 / (srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	       srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W]);

    xs = srcQuat[X] * s;   ys = srcQuat[Y] * s;   zs = srcQuat[Z] * s;
    wx = srcQuat[W] * xs;  wy = srcQuat[W] * ys;  wz = srcQuat[W] * zs;
    xx = srcQuat[X] * xs;  xy = srcQuat[X] * ys;  xz = srcQuat[X] * zs;
    yy = srcQuat[Y] * ys;  yz = srcQuat[Y] * zs;  zz = srcQuat[Z] * zs;

    /* set up 4x4 destMatrixrix */
    destMatrix[X][X] = 1.0 - (yy + zz);
    destMatrix[X][Y] = xy - wz;
    destMatrix[X][Z] = xz + wy;
    destMatrix[X][W] = 0.0;

    destMatrix[Y][X] = xy + wz;
    destMatrix[Y][Y] = 1.0 - (xx + zz);
    destMatrix[Y][Z] = yz - wx;
    destMatrix[Y][W] = 0.0;

    destMatrix[Z][X] = xz - wy;
    destMatrix[Z][Y] = yz + wx;
    destMatrix[Z][Z] = 1.0 - (xx + yy);
    destMatrix[Z][W] = 0.0;

    destMatrix[W][X] = 0.0;
    destMatrix[W][Y] = 0.0;
    destMatrix[W][Z] = 0.0;
    destMatrix[W][W] = 1.0;

}   /* q_to_col_matrix */



/*****************************************************************************
 *
   qp_from_matrix- Convert 3x4 PPHIGS rotation matrix to unit quaternion.
 *
 *****************************************************************************/

void 
qp_from_matrix(q_type destQuat, Q_MatrixType srcMatrix)
{
    double  	trace, s;
    int     	i, j, k;
    static int 	next[3] = {Y, Z, X};

    trace = srcMatrix[X][X] + srcMatrix[Y][Y] + srcMatrix[Z][Z];

    if (trace > 0.0) 
        {
        s = sqrt(trace + 1.0);
        destQuat[W] = s * 0.5;
        s = 0.5 / s;
    
        destQuat[X] = (srcMatrix[Z][Y] - srcMatrix[Y][Z]) * s;
        destQuat[Y] = (srcMatrix[X][Z] - srcMatrix[Z][X]) * s;
        destQuat[Z] = (srcMatrix[Y][X] - srcMatrix[X][Y]) * s;
        } 
    else 
        {
        i = X;
        if (srcMatrix[Y][Y] > srcMatrix[X][X])
	    i = Y;
        if (srcMatrix[Z][Z] > srcMatrix[i][i])
	    i = Z;
    
        j = next[i];  
        k = next[j];
    
        s = sqrt( (srcMatrix[i][i] - (srcMatrix[j][j]+srcMatrix[k][k])) + 1.0 );
    
        destQuat[i] = s * 0.5;
    
        s = 0.5 / s;
    
        destQuat[W] = (srcMatrix[k][j] - srcMatrix[j][k]) * s;
        destQuat[j] = (srcMatrix[j][i] + srcMatrix[i][j]) * s;
        destQuat[k] = (srcMatrix[k][i] + srcMatrix[i][k]) * s;
    
        }

}   /* qp_from_matrix */



/*****************************************************************************
 * q_from_col_matrix- Convert 4x4 column-major rotation matrix 
 *  	    	    	to unit quaternion.
 *****************************************************************************/

void 
q_from_col_matrix(q_type destQuat, q_matrix_type srcMatrix)
{
    double  	trace, s;
    int     	i, j, k;
    static int 	next[3] = {Y, Z, X};

    trace = srcMatrix[X][X] + srcMatrix[Y][Y] + srcMatrix[Z][Z];

    if (trace > 0.0) 
        {
        s = sqrt(trace + 1.0);
    
        destQuat[W] = s * 0.5;
    
        s = 0.5 / s;
    
        destQuat[X] = (srcMatrix[Z][Y] - srcMatrix[Y][Z]) * s;
        destQuat[Y] = (srcMatrix[X][Z] - srcMatrix[Z][X]) * s;
        destQuat[Z] = (srcMatrix[Y][X] - srcMatrix[X][Y]) * s;
        } 

    else 
        {
        i = X;
        if (srcMatrix[Y][Y] > srcMatrix[X][X])
	    i = Y;
        if (srcMatrix[Z][Z] > srcMatrix[i][i])
	    i = Z;
        j = next[i];  
        k = next[j];
    
        s = sqrt( (srcMatrix[i][i] - (srcMatrix[j][j]+srcMatrix[k][k])) + 1.0 );
        destQuat[i] = s * 0.5;
        s = 0.5 / s;
    
        destQuat[W] = (srcMatrix[k][j] - srcMatrix[j][k]) * s;
        destQuat[j] = (srcMatrix[j][i] + srcMatrix[i][j]) * s;
        destQuat[k] = (srcMatrix[k][i] + srcMatrix[i][k]) * s;
        }

}   /* q_from_col_matrix */



/*****************************************************************************
 * 
   q_from_row_matrix: Convert 4x4 row-major rotation matrix to unit quaternion
 *
 *****************************************************************************/

void 
q_from_row_matrix(q_type destQuat, q_matrix_type srcMatrix)
{
    double  	trace, s;
    int     	i, j, k;
    static int 	next[3] = {Y, Z, X};

    trace = srcMatrix[X][X] + srcMatrix[Y][Y]+ srcMatrix[Z][Z];

    if (trace > 0.0)
        {
        s = sqrt(trace + 1.0);
        destQuat[W] = s * 0.5;
        s = 0.5 / s;
    
        destQuat[X] = (srcMatrix[Y][Z] - srcMatrix[Z][Y]) * s;
        destQuat[Y] = (srcMatrix[Z][X] - srcMatrix[X][Z]) * s;
        destQuat[Z] = (srcMatrix[X][Y] - srcMatrix[Y][X]) * s;
        } 

    else 
        {
        i = X;
        if (srcMatrix[Y][Y] > srcMatrix[X][X])
	    i = Y;
        if (srcMatrix[Z][Z] > srcMatrix[i][i])
	    i = Z;
        j = next[i];  
        k = next[j];
    
        s = sqrt( (srcMatrix[i][i] - (srcMatrix[j][j]+srcMatrix[k][k])) + 1.0 );
        destQuat[i] = s * 0.5;
    
        s = 0.5 / s;
    
        destQuat[W] = (srcMatrix[j][k] - srcMatrix[k][j]) * s;
        destQuat[j] = (srcMatrix[i][j] + srcMatrix[j][i]) * s;
        destQuat[k] = (srcMatrix[i][k] + srcMatrix[k][i]) * s;
        }

}   /* q_from_row_matrix */



/*****************************************************************************
 *
   qgl_to_matrix - convert from quat to GL 4x4 float row matrix
    
    notes:
    	- same as q_to_row_matrix, except base type is float, not double
 *
 *****************************************************************************/


void
qgl_to_matrix(qgl_matrix_type destMatrix, q_type srcQuat)
{
    double  s;
    double  xs, ys, zs,
    	    wx, wy, wz,
    	    xx, xy, xz,
	        yy, yz, zz;

    /* 
     * For unit srcQuat, just set s = 2.0; or set xs = srcQuat[X] + 
     *   srcQuat[X], etc. 
     */

    s = 2.0 / (srcQuat[X]*srcQuat[X] + srcQuat[Y]*srcQuat[Y] + 
    	       srcQuat[Z]*srcQuat[Z] + srcQuat[W]*srcQuat[W]);

    xs = srcQuat[X] * s;   ys = srcQuat[Y] * s;   zs = srcQuat[Z] * s;
    wx = srcQuat[W] * xs;  wy = srcQuat[W] * ys;  wz = srcQuat[W] * zs;
    xx = srcQuat[X] * xs;  xy = srcQuat[X] * ys;  xz = srcQuat[X] * zs;
    yy = srcQuat[Y] * ys;  yz = srcQuat[Y] * zs;  zz = srcQuat[Z] * zs;

    /* set up 4x4 destMatrixrix */
    destMatrix[X][X] = 1.0 - (yy + zz);
    destMatrix[X][Y] = xy + wz;
    destMatrix[X][Z] = xz - wy;
    destMatrix[X][W] = 0.0;

    destMatrix[Y][X] = xy - wz;
    destMatrix[Y][Y] = 1.0 - (xx + zz);
    destMatrix[Y][Z] = yz + wx;
    destMatrix[Y][W] = 0.0;

    destMatrix[Z][X] = xz + wy;
    destMatrix[Z][Y] = yz - wx;
    destMatrix[Z][Z] = 1.0 - (xx + yy);
    destMatrix[Z][W] = 0.0;

    destMatrix[W][X] = 0.0;
    destMatrix[W][Y] = 0.0;
    destMatrix[W][Z] = 0.0;
    destMatrix[W][W] = 1.0;

}	/* qgl_to_matrix */


/*****************************************************************************
 * 
   qgl_from_matrix: Convert GL 4x4 row-major 
    	    	    	    rotation matrix to unit quaternion.
 
    notes:
    	- same as q_from_row_matrix, except basic type is float, not double
 *
 *****************************************************************************/

void 
qgl_from_matrix(q_type destQuat, qgl_matrix_type srcMatrix)
{
    double  	trace, s;
    int     	i, j, k;
    static int 	next[3] = {Y, Z, X};

    trace = srcMatrix[X][X] + srcMatrix[Y][Y]+ srcMatrix[Z][Z];

    if (trace > 0.0)
        {
        s = sqrt(trace + 1.0);
        destQuat[W] = s * 0.5;
        s = 0.5 / s;
    
        destQuat[X] = (srcMatrix[Y][Z] - srcMatrix[Z][Y]) * s;
        destQuat[Y] = (srcMatrix[Z][X] - srcMatrix[X][Z]) * s;
        destQuat[Z] = (srcMatrix[X][Y] - srcMatrix[Y][X]) * s;
        } 

    else 
        {
        i = X;
        if (srcMatrix[Y][Y] > srcMatrix[X][X])
	    i = Y;
        if (srcMatrix[Z][Z] > srcMatrix[i][i])
	    i = Z;
        j = next[i];  
        k = next[j];
    
        s = sqrt( (srcMatrix[i][i] - (srcMatrix[j][j]+srcMatrix[k][k])) + 1.0 );
        destQuat[i] = s * 0.5;
    
        s = 0.5 / s;
    
        destQuat[W] = (srcMatrix[j][k] - srcMatrix[k][j]) * s;
        destQuat[j] = (srcMatrix[i][j] + srcMatrix[j][i]) * s;
        destQuat[k] = (srcMatrix[i][k] + srcMatrix[k][i]) * s;
        }

}   /* qgl_from_matrix */

/*****************************************************************************
 * 
    q_from_ogl_matrix: Convert OpenGL matrix to quaternion

    notes:
        - same as q_from_row_matrix, except matrix is single index
 *
 *****************************************************************************/
void
q_from_ogl_matrix(q_type destQuat, qogl_matrix_type matrix )
{
    double     trace, s;
    int        i, j, k;
    static int next[3] = { Y, Z, X };

    trace = matrix[X*4+X] + matrix[Y*4+Y] + matrix[Z*4+Z];
    if( trace > 0.0 ) {
      s = sqrt( trace + 1.0 );
      destQuat[ W ] = s * 0.5;
      s = 0.5 / s;

      destQuat[ X ] = ( matrix[Y*4+Z] - matrix[Z*4+Y] ) * s;
      destQuat[ Y ] = ( matrix[Z*4+X] - matrix[X*4+Z] ) * s;
      destQuat[ Z ] = ( matrix[X*4+Y] - matrix[Y*4+X] ) * s;
    } else {
      i = X;
      if( matrix[Y*4+Y] > matrix[X*4+X] )
	i = Y;
      if( matrix[Z*4+Z] > matrix[i*4+i] )
	i = Z;
      j = next[i];
      k = next[j];

      s = sqrt(( matrix[i*4+i] - ( matrix[j*4+j] + matrix[k*4+k] )) + 1.0 );
      destQuat[i] = s * 0.5;
      s = 0.5 / s;

      destQuat[W] = ( matrix[j*4+k] - matrix[k*4+j] ) * s;
      destQuat[j] = ( matrix[i*4+j] + matrix[j*4+i] ) * s;
      destQuat[k] = ( matrix[i*4+k] + matrix[k*4+i] ) * s;
    }

    return;
} /* q_from_ogl_matrix */


/*****************************************************************************
 * 
    q_to_ogl_matrix: Convert quaternion to OpenGL matrix.
  	    	    Quaternion need not be unit magnitude.
 *
 *****************************************************************************/
void 
q_to_ogl_matrix( qogl_matrix_type matrix, q_type srcQuat )
{
    double s;
    double  xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;

    /* For unit srcQuat, just set s = 2.0, or set xs = srcQuat[X] +
    ** srcQuat[X], etc.
    */
    s = 2.0 / ( srcQuat[X] * srcQuat[X] + srcQuat[Y] * srcQuat[Y] + 
	        srcQuat[Z] * srcQuat[Z] + srcQuat[W] * srcQuat[W] );

    xs = srcQuat[X] * s;   ys = srcQuat[Y] * s;   zs = srcQuat[Z] * s;
    wx = srcQuat[W] * xs;  wy = srcQuat[W] * ys;  wz = srcQuat[W] * zs;
    xx = srcQuat[X] * xs;  xy = srcQuat[X] * ys;  xz = srcQuat[X] * zs;
    yy = srcQuat[Y] * ys;  yz = srcQuat[Y] * zs;  zz = srcQuat[Z] * zs;

    /* set up 4x4 matrix */
    matrix[X*4+X] = 1.0 - ( yy + zz );
    matrix[X*4+Y] = xy + wz;
    matrix[X*4+Z] = xz - wy;
    matrix[X*4+W] = 0.0;

    matrix[Y*4+X] = xy - wz;
    matrix[Y*4+Y] = 1.0 - ( xx + zz );
    matrix[Y*4+Z] = yz + wx;
    matrix[Y*4+W] = 0.0;

    matrix[Z*4+X] = xz + wy;
    matrix[Z*4+Y] = yz - wx;
    matrix[Z*4+Z] = 1.0 - ( xx + yy );
    matrix[Z*4+W] = 0.0;

    matrix[W*4+X] = 0.0;
    matrix[W*4+Y] = 0.0;
    matrix[W*4+Z] = 0.0;
    matrix[W*4+W] = 1.0;

} /* q_to_ogl_matrix */