/*
MATRIX_MATH.C

symbols in this file:
000F8900 0040:
	_matrix4x3_identity (0000)
000F8940 0030:
	_matrix4x3_transpose (0000)
000F8970 00f0:
	_matrix4x3_inverse (0000)
000F8A60 0040:
	_matrix4x3_scale (0000)
000F8AA0 0050:
	_matrix4x3_translation (0000)
000F8AF0 00e0:
	_matrix4x3_rotation_from_axis_and_angle (0000)
000F8BD0 0120:
	_matrix4x3_rotation_from_quaternion (0000)
000F8CF0 0030:
	_matrix4x3_from_point_and_quaternion (0000)
000F8D20 0040:
	_matrix4x3_from_orientation (0000)
000F8D60 0050:
	_matrix4x3_to_point_and_vectors (0000)
000F8DB0 0080:
	_matrix4x3_transform_point (0000)
000F8E30 0070:
	_matrix4x3_transform_vector (0000)
000F8EA0 0060:
	_matrix4x3_transform_normal (0000)
000F8F00 00a0:
	_matrix4x3_inverse_transform_point (0000)
000F8FA0 0070:
	_matrix4x3_inverse_transform_vector (0000)
000F9010 0060:
	_matrix4x3_inverse_transform_normal (0000)
000F9070 0150:
	_matrix4x3_multiply (0000)
000F91C0 0050:
	_matrix3x3_determinant (0000)
000F9210 0070:
	_matrix3x3_transpose (0000)
000F9280 0140:
	_matrix3x3_inverse (0000)
000F93C0 00d0:
	_matrix3x3_from_axis_and_angle (0000)
000F9490 0120:
	_matrix3x3_multiply (0000)
000F95B0 0080:
	_matrix3x3_transform_vector (0000)
000F9630 0080:
	_matrix4x3_rotation_from_vectors (0000)
000F96B0 00b0:
	_matrix4x3_rotation_from_angles (0000)
000F9760 0080:
	_matrix4x3_rotation_to_angles (0000)
000F97E0 0150:
	_matrix4x3_rotation_to_quaternion (0000)
000F9930 0040:
	_matrix4x3_from_point_and_vectors (0000)
000F9970 0070:
	_vector_from_matrices4x3 (0000)
000F99E0 0080:
	_matrix4x3_transform_plane (0000)
000F9A60 0080:
	_matrix4x3_inverse_transform_plane (0000)
000F9AE0 0070:
	_matrix3x3_from_forward_and_up (0000)
000F9B50 0150:
	_matrix3x3_rotation_to_quaternion (0000)
000F9CA0 0040:
	_valid_real_plane3d (0000)
000F9CE0 00b0:
	_matrix4x3_from_plane (0000)
0027AC18 001c:
	??_C@_0BM@OIPNJJOM@?$CBrealcmp?$CIdeterminant?0?50?40f?$CJ?$AA@ (0000)
0027AC34 0022:
	??_C@_0CC@FNOHDKIK@c?3?2halo?2SOURCE?2math?2matrix_math?4@ (0000)
0027AC58 0008:
	__real@3f1a36e2eb1c432d (0000)
0027AC60 001a:
	??_C@_0BK@BHMHOMPE@valid_real_plane3d?$CIplane?$CJ?$AA@ (0000)
0030790C 000e:
	_data_0030790c (0000)
*/

/* ---------- headers */

#include "cseries.h"
#include "real_math.h"

/* ---------- constants */

/* ---------- macros */

/* ---------- structures */

/* ---------- prototypes */

/* ---------- globals */

/* ---------- public code */

void
matrix4x3_identity(
	real_matrix4x3 *matrix)
{
	matrix->scale = 1.f;
	matrix->forward.i = 1.f;
	matrix->forward.j = 0.f;
	matrix->forward.k = 0.f;
	matrix->left.i = 0.f;
	matrix->left.j = 1.f;
	matrix->left.k = 0.f;
	matrix->up.i = 0.f;
	matrix->up.j = 0.f;
	matrix->up.k = 1.f;
	matrix->position.x = 0.f;
	matrix->position.y = 0.f;
	matrix->position.z = 0.f;

	return;
}

void
matrix4x3_transpose(
	real_matrix4x3 *matrix)
{
	real temporary;

	temporary = matrix->left.i;
	matrix->left.i = matrix->forward.j;
	matrix->forward.j = temporary;

	temporary = matrix->up.i;
	matrix->up.i = matrix->forward.k;
	matrix->forward.k = temporary;

	temporary = matrix->up.j;
	matrix->up.j = matrix->left.k;
	matrix->left.k = temporary;

	return;
}

void
matrix4x3_scale(
	real_matrix4x3 *matrix,
	real scale)
{
	matrix->scale = scale;
	matrix->forward.i = 1.f;
	matrix->forward.j = 0.f;
	matrix->forward.k = 0.f;
	matrix->left.i = 0.f;
	matrix->left.j = 1.f;
	matrix->left.k = 0.f;
	matrix->up.i = 0.f;
	matrix->up.j = 0.f;
	matrix->up.k = 1.f;
	matrix->position.x = 0.f;
	matrix->position.y = 0.f;
	matrix->position.z = 0.f;

	return;
}

void matrix4x3_translation(
	real_matrix4x3 *matrix,
	real_point3d const *point)
{
	matrix->scale = 1.f;
	matrix->forward.i = 1.f;
	matrix->forward.j = 0.f;
	matrix->forward.k = 0.f;
	matrix->left.i = 0.f;
	matrix->left.j = 1.f;
	matrix->left.k = 0.f;
	matrix->up.i = 0.f;
	matrix->up.j = 0.f;
	matrix->up.k = 1.f;
	matrix->position = *point;

	return;
}

void matrix4x3_rotation_from_quaternion(
	real_matrix4x3 *matrix,
	real_quaternion const *quaternion)
{
	real scale =
		quaternion->v.i * quaternion->v.i +
		quaternion->v.j * quaternion->v.j +
		quaternion->v.k * quaternion->v.k +
		quaternion->w * quaternion->w;
	real x;
	real y;
	real z;
	real wx;
	real wy;
	real wz;
	real xx;
	real xy;
	real xz;
	real yy;
	real yz;
	real zz;

	if (scale != 0.f)
		scale = 2.f / scale;
	else
		scale = 0.f;

	x = scale * quaternion->v.i;
	y = scale * quaternion->v.j;
	z = scale * quaternion->v.k;
	wx = quaternion->w * x;
	wy = quaternion->w * y;
	wz = quaternion->w * z;
	xx = quaternion->v.i * x;
	xy = quaternion->v.i * y;
	xz = quaternion->v.i * z;
	yy = quaternion->v.j * y;
	yz = quaternion->v.j * z;
	zz = quaternion->v.k * z;

	matrix->scale = 1.f;
	matrix->forward.i = 1.f - (yy + zz);
	matrix->forward.j = xy - wz;
	matrix->forward.k = xz + wy;
	matrix->left.i = xy + wz;
	matrix->left.j = 1.f - (xx + zz);
	matrix->left.k = yz - wx;
	matrix->up.i = xz - wy;
	matrix->up.j = yz + wx;
	matrix->up.k = 1.f - (xx + yy);
	matrix->position.x = 0.f;
	matrix->position.y = 0.f;
	matrix->position.z = 0.f;

	return;
}

void matrix4x3_from_point_and_quaternion(
	real_matrix4x3 *matrix,
	real_point3d const *point,
	real_quaternion const *quaternion)
{
	matrix4x3_rotation_from_quaternion(matrix, quaternion);
	matrix->position = *point;

	return;
}

void matrix4x3_from_orientation(
	real_matrix4x3 *matrix,
	real_orientation const *orientation)
{
	matrix4x3_rotation_from_quaternion(matrix, &orientation->rotation);
	matrix->scale = orientation->scale;
	matrix->position = orientation->translation;

	return;
}

/* NonMatching: target and candidate are both 0x70 bytes with no relocations.
   Every instruction after the opening argument loads is exact; VC7 reverses
   only the load/compare order of matrix and result in this reconstruction. */
void matrix3x3_transpose(
	real_matrix3x3 const *matrix,
	real_matrix3x3 *result)
{
	if (result == matrix)
	{
		real temporary;

		temporary = matrix->forward.j;
		result->forward.j = matrix->left.i;
		result->left.i = temporary;

		temporary = matrix->forward.k;
		result->forward.k = matrix->up.i;
		result->up.i = temporary;

		temporary = matrix->left.k;
		result->left.k = matrix->up.j;
		result->up.j = temporary;
	}
	else
	{
		result->forward.i = matrix->forward.i;
		result->forward.j = matrix->left.i;
		result->forward.k = matrix->up.i;
		result->left.i = matrix->forward.j;
		result->left.j = matrix->left.j;
		result->left.k = matrix->up.j;
		result->up.i = matrix->forward.k;
		result->up.j = matrix->left.k;
		result->up.k = matrix->up.k;
	}

	return;
}

/* NonMatching: target and candidate are both 0x70 bytes with no relocations.
   The matrix values and cross-product order agree; the remaining difference
   is VC7 register allocation and scheduling around the inlined cross product. */
void matrix3x3_from_forward_and_up(
	real_matrix3x3 *matrix,
	real_vector3d const *forward,
	real_vector3d const *up)
{
	matrix->forward = *forward;
	cross_product3d(up, forward, &matrix->left);
	matrix->up = *up;

	return;
}

void matrix4x3_inverse(
	real_matrix4x3 const *matrix,
	real_matrix4x3 *result)
{
	if (matrix->scale!=0.f)
	{
		real temp;

		real x = -matrix->position.x;
		real y = -matrix->position.y;
		real z = -matrix->position.z;

		if (matrix->scale!=1.f)
		{
			real scale = 1.f / matrix->scale;
			result->scale = scale;
			x *= scale;
			y *= scale;
			z *= scale;
		}
		else
		{
			result->scale = 1.f;
		}

		result->n[0][0] = matrix->n[0][0];
		result->n[1][1] = matrix->n[1][1];
		result->n[2][2] = matrix->n[2][2];

		temp = matrix->n[1][0];
		result->n[1][0] = matrix->n[0][1];
		result->n[0][1] = temp;

		temp = matrix->n[2][0];
		result->n[2][0] = matrix->n[0][2];
		result->n[0][2] = temp;

		temp = matrix->n[2][1];
		result->n[2][1] = matrix->n[1][2];
		result->n[1][2] = temp;

		result->n[3][0] = (x*result->n[0][0]) + y*result->n[1][0] + (z*result->n[2][0]);
		result->n[3][1] = (x*result->n[0][1]) + y*result->n[1][1] + (z*result->n[2][1]);
		result->n[3][2] = (x*result->n[0][2]) + y*result->n[1][2] + (z*result->n[2][2]);
	}
	else
	{
		memset(result, 0, sizeof(*result));
	}

	return;
}

void matrix4x3_rotation_from_vectors(
	real_matrix4x3 *matrix,
	real_vector3d const *forward,
	real_vector3d const *up)

{
	matrix->scale = 1.f;
	matrix->forward = *forward;
	cross_product3d(up, forward, &matrix->left);

	matrix->up = *up;
	set_real_point3d(&matrix->position, 0.f, 0.f, 0.f);

	return;
}

void matrix4x3_from_point_and_vectors(
	real_matrix4x3 *matrix,
	real_point3d const *point,
	real_vector3d const *forward,
	real_vector3d const *up)
{
	matrix4x3_rotation_from_vectors(matrix, forward, up);
	matrix->position = *point;
	
	return;
}

void
matrix4x3_to_point_and_vectors(
	real_matrix4x3 const *matrix,
	real_point3d *point,
	real_vector3d *forward,
	real_vector3d *up)
{
	*forward = matrix->forward;
	*up = matrix->up;
	*point = matrix->position;

	return;
}

real_vector3d *vector_from_matrices4x3(
	real_matrix4x3 const *a,
	real_matrix4x3 const *b,
	real_vector3d *rotation)
{
	real_matrix4x3 inverse_b;
	real_matrix4x3 relative;
	real_quaternion quaternion;
	real angle;

	matrix4x3_inverse(b, &inverse_b);
	matrix4x3_multiply(a, &inverse_b, &relative);
	matrix4x3_rotation_to_quaternion(&relative, &quaternion);
	quaternion_to_angle_and_vector(&quaternion, &angle, rotation);

	rotation->i *= angle;
	rotation->j *= angle;
	rotation->k *= angle;

	return rotation;
}

real_point3d *matrix4x3_transform_point(
	real_matrix4x3 const *matrix,
	real_point3d const *point,
	real_point3d *result)
{
	real x = point->x;
	real y = point->y;
	real z = point->z;

	if (matrix->scale != 1.f)
	{
		x *= matrix->scale;
		y *= matrix->scale;
		z *= matrix->scale;
	}

	result->x = matrix->up.i*z + matrix->left.i*y + matrix->forward.i*x + matrix->position.x;
	result->y = matrix->up.j*z + matrix->left.j*y + matrix->forward.j*x + matrix->position.y;
	result->z = matrix->up.k*z + matrix->left.k*y + matrix->forward.k*x + matrix->position.z;
	return result;
}

real_vector3d *matrix4x3_transform_vector(
	real_matrix4x3 const *matrix,
	real_vector3d const *vector,
	real_vector3d *result)
{
	real i = vector->i;
	real j = vector->j;
	real k = vector->k;

	if (matrix->scale != 1.f)
	{
		i *= matrix->scale;
		j *= matrix->scale;
		k *= matrix->scale;
	}

	result->i = i*matrix->forward.i + j*matrix->left.i + k*matrix->up.i;
	result->j = i*matrix->forward.j + j*matrix->left.j + k*matrix->up.j;
	result->k = i*matrix->forward.k + j*matrix->left.k + k*matrix->up.k;

	return result;
}

real_point3d *matrix4x3_inverse_transform_point(
	real_matrix4x3 const *matrix,
	real_point3d const *point,
	real_point3d *result)
{
	if (matrix->scale != 0.f)
	{
		real x = point->x - matrix->position.x;
		real y = point->y - matrix->position.y;
		real z = point->z - matrix->position.z;

		if (matrix->scale != 1.f)
		{
			real scale = 1.f / matrix->scale;
			x *= scale;
			y *= scale;
			z *= scale;
		}

		result->x = x * matrix->forward.i + y * matrix->forward.j + z * matrix->forward.k;
		result->y = x * matrix->left.i + y * matrix->left.j + z * matrix->left.k;
		result->z = x * matrix->up.i + y * matrix->up.j + z * matrix->up.k;
	}
	else
	{
		result->x = 0.f;
		result->y = 0.f;
		result->z = 0.f;
	}

	return result;
}

real_vector3d *matrix4x3_transform_normal(
	real_matrix4x3 const *matrix,
	real_vector3d const *normal,
	real_vector3d *result)
{
	real i = normal->i;
	real j = normal->j;
	real k = normal->k;

	result->i = i*matrix->forward.i + j*matrix->left.i + k*matrix->up.i;
	result->j = i*matrix->forward.j + j*matrix->left.j + k*matrix->up.j;
	result->k = i*matrix->forward.k + j*matrix->left.k + k*matrix->up.k;
	return result;
}

real_vector3d *
matrix4x3_inverse_transform_normal(
	real_matrix4x3 const *matrix,
	real_vector3d const *normal,
	real_vector3d *result)
{
	real i = normal->i;
	real j = normal->j;
	real k = normal->k;

	result->i = i * matrix->forward.i + j * matrix->forward.j + k * matrix->forward.k;
	result->j = i * matrix->left.i + j * matrix->left.j + k * matrix->left.k;
	result->k = i * matrix->up.i + j * matrix->up.j + k * matrix->up.k;

	return result;
}

real_vector3d *matrix4x3_inverse_transform_vector(
	real_matrix4x3 const *matrix,
	real_vector3d const *vector,
	real_vector3d *result)
{
	real i = vector->i;
	real j = vector->j;
	real k = vector->k;

	if (matrix->scale != 1.f)
	{
		real scale = 1.f / matrix->scale;
		i *= scale;
		j *= scale;
		k *= scale;
	}

	result->i = i * matrix->forward.i + j * matrix->forward.j + k * matrix->forward.k;
	result->j = i * matrix->left.i + j * matrix->left.j + k * matrix->left.k;
	result->k = i * matrix->up.i + j * matrix->up.j + k * matrix->up.k;

	return result;
}

real_plane3d *matrix4x3_transform_plane(
	real_matrix4x3 const *matrix,
	real_plane3d const *plane,
	real_plane3d *result)
{
	matrix4x3_transform_normal(matrix, &plane->n, &result->n);
	result->d = plane->d * matrix->scale + dot_product3d((real_vector3d const *)&matrix->position, &result->n);

	return result;
}

real_plane3d *matrix4x3_inverse_transform_plane(
	real_matrix4x3 const *matrix,
	real_plane3d const *plane,
	real_plane3d *result)
{
	if (matrix->scale != 0.f)
	{
		result->d = plane->d - dot_product3d((real_vector3d const *)&matrix->position, &plane->n);

		if (matrix->scale != 1.f)
			result->d /= matrix->scale;
	}
	else
	{
		result->d = 0.f;
	}

	matrix4x3_inverse_transform_vector(matrix, &plane->n, &result->n);

	return result;
}

real
matrix3x3_determinant(
	real_matrix3x3 const *matrix)
{
	return matrix->up.i * matrix->forward.j * matrix->left.k
		+ matrix->forward.k * matrix->left.i * matrix->up.j
		+ matrix->up.k * matrix->forward.i * matrix->left.j
		- matrix->forward.i * matrix->up.j * matrix->left.k
		- matrix->up.k * matrix->forward.j * matrix->left.i
		- matrix->up.i * matrix->forward.k * matrix->left.j;
}

real_vector3d *matrix3x3_transform_vector(
	real_matrix3x3 const *matrix,
	real_vector3d const *vector,
	real_vector3d *result)
{
	real_vector3d temporary;

	if (vector == result)
	{
		temporary = *vector;
		vector = &temporary;
	}

	result->i = matrix->up.i * vector->k + matrix->left.i * vector->j + matrix->forward.i * vector->i;
	result->j = matrix->up.j * vector->k + matrix->forward.j * vector->i + matrix->left.j * vector->j;
	result->k = matrix->up.k * vector->k + matrix->forward.k * vector->i + matrix->left.k * vector->j;

	return result;
}

void matrix4x3_from_plane(
	real_matrix4x3 *matrix,
	real_plane3d const *plane)
{
	real_vector3d forward;
	real_point3d point;

	match_assert("c:\\halo\\SOURCE\\math\\matrix_math.c", 370, valid_real_plane3d(plane));

	perpendicular3d(&plane->n, &forward);
	normalize3d(&forward);

	point.x = plane->n.i * plane->d;
	point.y = plane->n.j * plane->d;
	point.z = plane->n.k * plane->d;

	matrix4x3_rotation_from_vectors(matrix, &forward, &plane->n);
	matrix->position = point;

	return;
}

boolean valid_real_plane3d(
	real_plane3d const *plane)
{
	return
		valid_real_normal3d(&plane->n) &&
		valid_real(plane->d);
}

/* ---------- private code */