Quaternion

D3DXVECTOR4 QuatSlerp(D3DXVECTOR4 &from, D3DXVECTOR4 &to, float t)
{
	float        to1[4];
	float        omega, cosom, sinom, scale0, scale1;

	// calc cosine
	cosom = ((from.x * to.x) + (from.y * to.y) + (from.z * to.z) + (from.w * to.w));
	
	// adjust signs (if necessary)
	if ( cosom <0.0 )
	{ 
		  cosom = -cosom; 
		  to1[0] = - to.x;
          to1[1] = - to.y;
          to1[2] = - to.z;
          to1[3] = - to.w;
	} 
	else 
	{
          to1[0] = to.x;
          to1[1] = to.y;
          to1[2] = to.z;
          to1[3] = to.w;
	}

	 // calculate coefficients
	if ( (1.0 - cosom) > DELTA )
	{
          // standard case (slerp)
          omega = (float)acos(cosom);
          sinom = (float)sin(omega);
          scale0 = (float)sin((1.0 - t) * omega) / sinom;
          scale1 = (float)sin(t * omega) / sinom;

	} 
	else
	{        
      // "from" and "to" quaternions are very close 
      //  ... so we can do a linear interpolation
          scale0 = 1.0f - t;
          scale1 = t;
	}

	D3DXVECTOR4 res;
	// calculate final values
	res.x = scale0 * from.x + scale1 * to1[0];
	res.y = scale0 * from.y + scale1 * to1[1];
	res.z = scale0 * from.z + scale1 * to1[2];
	res.w = scale0 * from.w + scale1 * to1[3];

	return res;
}


void MatrixFromQuat( D3DXMATRIX &m, D3DXVECTOR4 &quat )
{
	float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
             
	// calculate coefficients
    
	x2 = quat.x + quat.x; 
	y2 = quat.y + quat.y; 
	z2 = quat.z + quat.z;
	xx = quat.x * x2;   xy = quat.x * y2;   xz = quat.x * z2;
	yy = quat.y * y2;   yz = quat.y * z2;   zz = quat.z * z2;
	wx = quat.w * x2;   wy = quat.w * y2;   wz = quat.w * z2;

	m.m[0][0] = 1.f - (yy + zz);
	m.m[1][0] = xy - wz;
	m.m[2][0] = xz + wy;
                  
	m.m[0][1] = xy + wz;
	m.m[1][1] = 1.f - (xx + zz);
	m.m[2][1] = yz - wx;

	m.m[0][2] = xz - wy;
	m.m[1][2] = yz + wx;
	m.m[2][2] = 1.f - (xx + yy);
}

void QuatFromMatrix( D3DXVECTOR4 &quat, D3DXMATRIX &m )
{
	float  tr, s, q[4];
	int    i, j, k;
	int    nxt[3] = {1, 2, 0};  

	tr = m._11 + m._22 + m._33;

	// check the diagonal
	if (tr > 0.0)
	{
		s = (float)sqrt (tr + 1.f);
		quat.w = s / 2.0f;
		s = 0.5f / s;
		quat.x = (m.m[2][1] - m.m[1][2]) * s;
		quat.y = (m.m[0][2] - m.m[2][0]) * s;
		quat.z = (m.m[1][0] - m.m[0][1]) * s;
	} 
	else
	{                
		// diagonal is negative
		i = 0;
		if (m._22 > m._11) i = 1;
		if (m._33 > m.m) i = 2;
		j = nxt;
		k = nxt[j];

	    s = (float)sqrt (m.m - m.m[j][j] + m.m[k][k] + 1.0);
                       
		q = s * (float)0.5;
                             
		if (s != 0.0f) s = 0.5f / s;

		q[3] = (((m.m[j][k]) - m.m[k][j])) * s;
		q[j] = (((m.m[j]) + m.m[j])) * s;
		q[k] = (((m.m[k]) + m.m[k])) * s;

		quat.x = q[0];
		quat.y = q[1];
		quat.z = q[2];
		quat.w = q[3];
	}
}