glquat.c

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

  Params:   GL_QUAT* (quaternion)

  Returns:  nothing

  Comments: none
Returns the inverse of the quaternion (1/q).  check conjugate
***********************************************************************EDOC*/
void APIENTRY gluQuatInverse_EXT(GL_QUAT *quat)
{
	GLfloat norm, invNorm;

	norm = quat->x * quat->x + quat->y * quat->y + quat->z * quat->z
		               + quat->w * quat->w;
	
	invNorm = (GLfloat) (1.0 / norm);
	
	quat->x = -quat->x * invNorm;
    quat->y = -quat->y * invNorm;
    quat->z = -quat->z * invNorm;
    quat->w =  quat->w * invNorm;

}


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

  Name:		gluQuatSetFromAx_EXT

  Action:   Constructs quaternion to rotate from one direction vector to 
			another

  Params:   GLfloat (x1, y1, z1 - from vector), 
			GLfloat (x2, y2, z2 - to vector), GL_QUAT* (resulting quaternion)

  Returns:  nothing

  Comments: Two vectors have to be UNIT vectors (so make sure you normalize
			them before calling this function
			Some parts are heavily optimized so readability is not so great :(
***********************************************************************EDOC*/
void APIENTRY gluQuatSetFromAx_EXT(GLfloat x1,GLfloat y1, GLfloat z1, 
						 GLfloat x2,GLfloat y2, GLfloat z2, GL_QUAT *quat)

{
    GLfloat tx, ty, tz, temp, dist;

    GLfloat	cost, len, ss;

	// get dot product of two vectors
    cost = x1 * x2 + y1 * y2 + z1 * z2;

    // check if parallel
    if (cost > 0.99999f) {
	quat->x = quat->y = quat->z = 0.0f;
	quat->w = 1.0f;
	return;
    }
    else if (cost < -0.99999f) {		// check if opposite

	// check if we can use cross product of from vector with [1, 0, 0]
	tx = 0.0;
	ty = x1;
	tz = -y1;

	len = sqrt(ty*ty + tz*tz);

	if (len < DELTA)
	{
		// nope! we need cross product of from vector with [0, 1, 0]
		tx = -z1;
		ty = 0.0;
		tz = x1;
	}

	// normalize
	temp = tx*tx + ty*ty + tz*tz;

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

    tx *= dist;
    ty *= dist;
    tz *= dist;
	
	quat->x = tx;
	quat->y = ty;
	quat->z = tz;
	quat->w = 0.0;

	return;
    }

	// ... else we can just cross two vectors

	tx = y1 * z2 - z1 * y2;
	ty = z1 * x2 - x1 * z2;
	tz = x1 * y2 - y1 * x2;

	temp = tx*tx + ty*ty + tz*tz;

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

    tx *= dist;
    ty *= dist;
    tz *= dist;


    // we have to use half-angle formulae (sin^2 t = ( 1 - cos (2t) ) /2)
	
	ss = (float)sqrt(0.5f * (1.0f - cost));

	tx *= ss;
	ty *= ss;
    tz *= ss;

    // scale the axis to get the normalized quaternion
    quat->x = tx;
    quat->y = ty;
    quat->z = tz;

    // cos^2 t = ( 1 + cos (2t) ) / 2
    // w part is cosine of half the rotation angle
    quat->w = (float)sqrt(0.5f * (1.0f + cost));

}




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

  Name:		gluQuatMul_EXT

  Action:   Multiplies two quaternions

  Params:   GL_QUAT ( q1 * q2 = res)

  Returns:  nothing

  Comments: NOTE: multiplication is not commutative

***********************************************************************EDOC*/
void APIENTRY gluQuatMul_EXT(GL_QUAT* q1, GL_QUAT* q2, GL_QUAT* res)
{

	res->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
	res->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
	res->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
	res->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;


	// make sure the resulting quaternion is a unit quat.
	gluQuatNormalize_EXT(res);

}


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

  Name:		gluQuatAdd_EXT

  Action:   Adds two quaternions

  Params:   GL_QUAT* (q1 + q2 = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatAdd_EXT(GL_QUAT* q1, GL_QUAT* q2, GL_QUAT* res)
{
	res->x = q1->x + q2->x;
	res->y = q1->y + q2->y;
	res->z = q1->z + q2->z;
	res->w = q1->w + q2->w;

	// make sure the resulting quaternion is a unit quat.
	gluQuatNormalize_EXT(res);
}


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

  Name:		gluQuatSub_EXT

  Action:   Subtracts two quaternions

  Params:   GL_QUAT* (q1 - q2 = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatSub_EXT(GL_QUAT* q1, GL_QUAT* q2, GL_QUAT* res)
{
	res->x = q1->x - q2->x;
	res->y = q1->y - q2->y;
	res->z = q1->z - q2->z;
	res->w = q1->w - q2->w;

	// make sure the resulting quaternion is a unit quat.
	gluQuatNormalize_EXT(res);
}


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

  Name:		gluQuatDiv_EXT

  Action:   Divide two quaternions

  Params:   GL_QUAT* (q1 / q2 = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatDiv_EXT(GL_QUAT* q1, GL_QUAT* q2, GL_QUAT* res)
{
	GL_QUAT q, r, s;

	gluQuatCopy_EXT(q2, &q);

	// invert vector
    q.x = -q.x;
    q.y = -q.y;
    q.z = -q.z;
	
	gluQuatMul_EXT(q1, &q, &r);
	gluQuatMul_EXT(&q, &q, &s);

	res->x = r.x / s.w;
	res->y = r.y / s.w;
	res->z = r.z / s.w;
	res->w = r.w / s.w;

}


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

  Name:		gluQuatCopy_EXT

  Action:   copies q1 into q2

  Params:   GL_QUAT* (q1 and q2)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatCopy_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
	q2->x = q1->x;
	q2->y = q1->y;
	q2->z = q1->z;
	q2->w = q1->w;
}



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

  Name:		gluQuatSquare_EXT

  Action:   Square quaternion

  Params:   GL_QUAT* (q1 * q1 = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatSquare_EXT(GL_QUAT* q1, GL_QUAT* res)
{
	GLfloat  tt;


	tt = 2 * q1->w;
	res->x = tt * q1->x;
	res->y = tt * q1->y;
	res->z = tt * q1->z;
	res->w = (q1->w * q1->w - q1->x * q1->x - q1->y * q1->y - q1->z * q1->z);
}


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

  Name:		gluQuatSqrt_EXT

  Action:   Find square root of a quaternion

  Params:   GL_QUAT* (sqrt(q1) = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatSqrt_EXT(GL_QUAT* q1, GL_QUAT* res)
{
	GLfloat  length, m, r1, r2;
	GL_QUAT r;

	length = sqrt (q1->w * q1->w + q1->x * q1->x + q1->y * q1->y);
	if (length != 0.0) 
		length = 1.0 / length; 
	else length = 1.0;

	r.x = q1->x * length;
	r.y = q1->z * length;
	r.z = 0.0f;
	r.w = q1->w * length;

	m = 1.0 / sqrt (r.w * r.w + r.x * r.x);
	r1 = sqrt ((1.0 + r.y) * 0.5);
	r2 = sqrt ((1.0 - r.y) * 0.5);

	res->x = sqrt (length) * r2 * r.x * m;
	res->y = sqrt (length) * r1;
	res->z = q1->z;
	res->w = sqrt (length) * r1 * r.w * m;

}


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

  Name:		gluQuatDot_EXT

  Action:   Computes the dot product of two unit quaternions

  Params:   GL_QUAT (first and second quaternion)

  Returns:  (GLfloat) Dot product

  Comments: Quaternion has to be normalized (i.e. it's a unit quaternion)

***********************************************************************EDOC*/
GLfloat APIENTRY gluQuatDot_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
  return (GLfloat)(q1->w * q2->w + q1->x * q2->x + q1->y * q2->y+q1->z*q2->z);
}


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

  Name:		gluQuatLength_EXT

  Action:   Calculates the length of a quaternion

  Params:   GL_QUAT* (quaternion)

  Returns:  GLfloat (length)

  Comments: none

***********************************************************************EDOC*/
GLfloat APIENTRY gluQuatLength_EXT(GL_QUAT* q1)
{
  return sqrt (q1->w * q1->w + q1->x * q1->x + q1->y * q1->y + q1->z * q1->z);
}


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

  Name:		gluQuatNegate_EXT

  Action:   Negates vector part of a quaternion

  Params:   GL_QUAT (source and destination quaternion)

  Returns:  nothing

  Comments: Source quaternion does NOT have to be normalized 

***********************************************************************EDOC*/
void APIENTRY gluQuatNegate_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
	gluQuatCopy_EXT(q1, q2);

	gluQuatNormalize_EXT(q2);
	q2->x = -q2->x;
	q2->y = -q2->y;
	q2->z = -q2->z;
}

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

  Name:		gluQuatExp_EXT

  Action:   Calculates exponent of a quaternion

  Params:   GL_QUAT* (Source and destination quaternion)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatExp_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
	GLfloat  len1, len2;

	len1 = (GLfloat) sqrt (q1->x * q1->x + q1->y * q1->y + q1->z * q1->z);
	if (len1 > 0.0) 
		len2 = (GLfloat)sin(len1) / len1; 
	else 
		len2 = 1.0;

	q2->x = q1->x * len2;
	q2->y = q1->y * len2;
	q2->z = q1->z * len2;
	q2->w = cos (len1);
}


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

  Name:		gluQuatLog_EXT

  Action:   Calculates natural logarithm of a quaternion

  Params:   GL_QUAT* (Source and destination quaternion)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatLog_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
	GLfloat  length;

	length = sqrt (q1->x * q1->x + q1->y * q1->y + q1->z * q1->z);

	//make sure we do not divide by 0
	if (q1->w != 0.0) 
		length = atan (length / q1->w); 
	else length = (GLfloat)M_PI/2;

	q2->w = 0.0f;
	q2->x = q1->x * length;
	q2->y = q1->y * length;
	q2->z = q1->z * length;
}

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

  Name:		gluQuatLnDif_EXT

  Action:   Computes the "natural log difference" of two quaternions,
			q1 and q2 as  ln(qinv(q1)*q2)

  Params:   GL_QUAT* (Source quaternions  and a destination quaternion)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void APIENTRY gluQuatLnDif_EXT(GL_QUAT *q1, GL_QUAT *q2, GL_QUAT *res)
{

	GL_QUAT inv, dif, temp;
	GLfloat  len, len1, s;

	qt_inverse (a, &inv);
	qt_mul (&inv, b, &dif);
	len = sqrt (dif.x*dif.x + dif.y*dif.y + dif.z*dif.z);
	s = qt_dot (a, b);
	if (s != 0.0) len1 = atan (len / s); else len1 = M_PI/2;
	if (len != 0.0) len1 /= len;
	temp.w = 0.0;
	temp.x = dif.x * len1;
	temp.y = dif.y * len1;
	temp.z = dif.z * len1;
	qt_copy (&temp, out);
}




// cleanup stuff we changed
#if defined (WIN32)
#pragma warning( default : 4244 )	// set it to default again
#endif