Battlezone Forums

Code: Select all

				FrameTransformMatrix {

					 0.890873,-0.227687,0.393069,0.000000,
					 0.438325,0.658008,-0.612288,0.000000,
					 -0.119233,0.717763,0.686003,0.000000,
					 0.256096,0.551972,0.156151,1.000000;;
				}

Code: Select all

typedef float F32;

__declspec(align(16)) struct Quaternion
{
	F32			s;			// scalar part
	Vector		v;			// vector part
[...]
};

__declspec(align(16)) struct Matrix
{
public:

	Vector right;
	F32    rightw;

	Vector up;
	F32    upw;

	Vector front;
	F32    frontw;

	Vector posit;
	F32    positw;
[...]

	F32 MatrixTrace(void) const 
	{
		return right.x + up.y + front.z;
	}

	F32 &Value(U32 r, U32 c) const
	{
		return ((F32 *)&right.x)[r*4+c];
	}
};

void Quaternion::Set(const Matrix &matrix)
{
	F32 w = matrix.MatrixTrace();
	if (w > 0.0f)
	{
		w = sqrtf(w + 1.0f);
		s = w * 0.5f;

		w = 0.5f / w;
		v.x = (matrix.front.y - matrix.up.z) * w;
		v.y = (matrix.right.z - matrix.front.x) * w;
		v.z = (matrix.up.x - matrix.right.y) * w;
	}
	else
	{
		static U32 next[] = { 1, 2, 0 };
		U32 i = 0;
		if (matrix.up.y > matrix.right.x)
		{
			i = 1;
		}
		if (matrix.front.z > matrix.Value(i, i))
		{
			i = 2;
		}
		U32 j = next[i];
		U32 k = next[j];
		w = sqrtf((matrix.Value(i, i) - (matrix.Value(j, j) + matrix.Value(k, k))) + 1.0f);
		F32 *qi = (F32 *) &v + i;
		F32 *qj = (F32 *) &v + j;
		F32 *qk = (F32 *) &v + k;
		*qi = w * 0.5f;
		w   = 0.5f / w;
		s   = (matrix.Value(k, j) - matrix.Value(j, k)) * w;
		*qj = (matrix.Value(j, i) + matrix.Value(i, j)) * w;
		*qk = (matrix.Value(k, i) + matrix.Value(i, k)) * w;
	}

	_ASSERTE (Sanity ());
}

void Matrix::Set( const Quaternion &qq)
{
	F32 n =  2.0f / (qq.v.x * qq.v.x + qq.v.y * qq.v.y + qq.v.z * qq.v.z + qq.s * qq.s);
	F32 xn = qq.v.x * n;	F32 yn = qq.v.y * n;	F32 zn = qq.v.z * n;
	F32 sx = qq.s   * xn;	F32 sy = qq.s   * yn;	F32 sz = qq.s   * zn;
	F32 xx = qq.v.x * xn;	F32 xy = qq.v.x * yn;	F32 xz = qq.v.x * zn;
	F32 yy = qq.v.y * yn;	F32 yz = qq.v.y * zn;	F32 zz = qq.v.z * zn;

	right.x = 1.0f - (yy + zz);
	right.y = xy - sz;
	right.z = xz + sy;

	up.x = xy + sz;
	up.y = 1.0f - (xx + zz);
	up.z = yz - sx;

	front.x = xz - sy;
	front.y = yz + sx;
	front.z = 1.0f - (xx + yy);
}