ivu_quat.cxx

hl2 source code. Do not use it illegal.

    dist = (IVP_DOUBLE)(1.0f / IVP_Inline_Math::sqrtd(temp));

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

	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 = (IVP_DOUBLE)(1.0f / IVP_Inline_Math::sqrtd(temp));

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


    // we have to use half-angle formulae (sin^2 t = ( 1 - cos (2t) ) /2)
	
	ss = (IVP_DOUBLE)IVP_Inline_Math::sqrtd(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 = IVP_Inline_Math::sqrtd(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 IVP_U_Quat::set_mult_quat(const IVP_U_Quat* q1,const  IVP_U_Quat* q2) {
  this->inline_set_mult_quat(q1,q2);
}



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

  Name:		gluQuatDiv_EXT

  Action:   Divide two quaternions

  Params:   GL_QUAT* (q1 / q2 = res)

  Returns:  nothing

  Comments: none

***********************************************************************EDOC*/
void IVP_U_Quat::set_div_unit_quat(const IVP_U_Quat* q1,const  IVP_U_Quat* q2) {
    IVP_U_Quat *res = this;
    IVP_U_Quat q;
    q.set_invert_unit_quat(q2);
    res->inline_set_mult_quat( q1,&q);
}

void IVP_U_Quat::set_invert_mult(const IVP_U_Quat* q1,const  IVP_U_Quat* q2) {
    IVP_U_Quat *res = this;
    IVP_U_Quat q;
    q.set_invert_unit_quat(q1);
    res->inline_set_mult_quat( &q,q2);
}

#if 0


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

  Name:		gluEulerToQuat_EXT

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

  Params:   GLIVP_FLOAT (roll), GLIVP_FLOAT (pitch), GLIVP_FLOAT (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(GLIVP_FLOAT roll, GLIVP_FLOAT pitch, GLIVP_FLOAT yaw, 
													GL_QUAT * quat)
{
	
	IVP_FLOAT cr, cp, cy, sr, sp, sy, cpcy, spsy;

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

	sr = sinf(roll/2);
	sp = sinf(pitch/2);
	sy = sinf(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:		gluQuatGetValue_EXT

  Action:   Disassembles quaternion to an axis and an angle

  Params:   GL_QUAT* (quaternion), GLIVP_FLOAT* (x, y, z - axis), GLIVP_FLOAT (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, GLIVP_FLOAT *x, GLIVP_FLOAT *y, 
											GLIVP_FLOAT *z, GLIVP_FLOAT *radians)
{
    GLIVP_FLOAT	len;
    GLIVP_FLOAT tx, ty, tz;

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

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

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

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


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

  Name:		gluQuatSetValue_EXT

  Action:   Assembles quaternion from an axis and an angle

  Params:   GL_QUAT* (quaternion), GLIVP_FLOAT (x, y, z - axis), GLIVP_FLOAT (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, GLIVP_FLOAT x, GLIVP_FLOAT y, 
												GLIVP_FLOAT z, GLIVP_FLOAT angle)
{
	GLIVP_FLOAT temp, dist;

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

    dist = (GLIVP_FLOAT)(1.0f / IVP_Inline_Math::sqrtd(temp));

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

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

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



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

  Name:		gluQuatScaleAngle_EXT

  Action:   Scales the rotation angle of a quaternion

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

  Returns:  nothing

  Comments: none

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

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

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


/*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)
{
	GLIVP_FLOAT  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)
{
	GLIVP_FLOAT  length, m, r1, r2;
	GL_QUAT r;

	length = IVP_Inline_Math::sqrtd (q1->w * q1->w + q1->x * q1->x + q1->y * q1->y);
	if (length != 0.0f) 
		length = 1.0f / length; 
	else length = 1.0f;

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

	m = 1.0f / IVP_Inline_Math::sqrtd (r.w * r.w + r.x * r.x);
	r1 = IVP_Inline_Math::sqrtd ((1.0f + r.y) * 0.5f);
	r2 = IVP_Inline_Math::sqrtd ((1.0f - r.y) * 0.5f);

	res->x = IVP_Inline_Math::sqrtd (length) * r2 * r.x * m;
	res->y = IVP_Inline_Math::sqrtd (length) * r1;
	res->z = q1->z;
	res->w = IVP_Inline_Math::sqrtd (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:  (GLIVP_FLOAT) Dot product

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

***********************************************************************EDOC*/
GLIVP_FLOAT APIENTRY gluQuatDot_EXT(GL_QUAT* q1, GL_QUAT* q2)
{
  return (GLIVP_FLOAT)(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:  GLIVP_FLOAT (length)

  Comments: none

***********************************************************************EDOC*/
GLIVP_FLOAT APIENTRY gluQuatLength_EXT(GL_QUAT* q1)
{
  return IVP_Inline_Math::sqrtd (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)
{
	GLIVP_FLOAT  len1, len2;

	len1 = (GLIVP_FLOAT) IVP_Inline_Math::sqrtd (q1->x * q1->x + q1->y * q1->y + q1->z * q1->z);
	if (len1 > 0.0f) 
		len2 = (GLIVP_FLOAT)sin(len1) / len1; 
	else 
		len2 = 1.0f;

	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)
{
	GLIVP_FLOAT  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.0f) 
		length = atan (length / q1->w); 
	else length = (GLIVP_FLOAT)IVP_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;
	GLIVP_FLOAT  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.0f) len1 = atan (len / s); else len1 = IVP_PI/2;
	if (len != 0.0f) len1 /= len;
	temp.w = 0.0f;
	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
#endif