Skip to content

Commit

Permalink
Reorganized matrix_math
Browse files Browse the repository at this point in the history
  • Loading branch information
@twist84
twist84 committed Jan 20, 2025
1 parent 7068921 commit c56d1e9
Show file tree
Hide file tree
Showing 2 changed files with 173 additions and 94 deletions.
249 changes: 163 additions & 86 deletions game/source/math/matrix_math.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,139 +4,216 @@

#include <math.h>

void __cdecl matrix4x3_rotation_from_vectors(real_matrix4x3* matrix, real_vector3d const* forward, real_vector3d const* up)
//.text:005B0330 ; real __cdecl matrix3x3_determinant(real_matrix3x3 const*)
//.text:005B03B0 ; void __cdecl matrix3x3_from_angles(real_matrix3x3*, real, real, real)
//.text:005B04F0 ; void __cdecl matrix3x3_from_angles_cpp(real_matrix3x3*, real, real, real)
//.text:005B0630 ; real_matrix3x3* __cdecl matrix3x3_from_axis_and_angle(real_matrix3x3*, real_vector3d const*, real, real)
//.text:005B0740 ; real_matrix3x3* __cdecl matrix3x3_from_forward_and_up(real_matrix3x3*, real_vector3d const*, real_vector3d const*)
//.text:005B07C0 ; real_matrix3x3* __cdecl matrix3x3_inverse(real_matrix3x3 const*, real, real_matrix3x3*)
//.text:005B0900 ; real_vector3d* __cdecl matrix3x3_inverse_transform_vector(real_matrix3x3 const*, real_vector3d const*, real_vector3d*)
//.text:005B09B0 ; real_matrix3x3* __cdecl matrix3x3_multiply(real_matrix3x3 const*, real_matrix3x3 const*, real_matrix3x3*)
//.text:005B0BB0 ; bool __cdecl matrix3x3_normalize(real_matrix3x3*)

real_matrix3x3* __cdecl matrix3x3_rotation_from_quaternion(real_matrix3x3* matrix, real_quaternion const* quaternion)
{
matrix->scale = 1.0f;
matrix->forward = *forward;
cross_product3d(up, forward, &matrix->left);
matrix->up = *up;
set_real_point3d(&matrix->position, 0.0f, 0.0f, 0.0f);
return INVOKE(0x005B0F80, matrix3x3_rotation_from_quaternion, matrix, quaternion);
}

//.text:005B10E0 ; real_quaternion* __cdecl matrix3x3_rotation_to_quaternion(real_matrix3x3 const*, real_quaternion*)
//.text:005B12E0 ; real_vector3d* __cdecl matrix3x3_transform_vector(real_matrix3x3 const*, real_vector3d const*, real_vector3d*)
//.text:005B1390 ; real_matrix3x3* __cdecl matrix3x3_transpose(real_matrix3x3 const*, real_matrix3x3*)
//.text:005B1410 ; void __cdecl matrix4x3_apply_orientation(real_matrix4x3 const restrict*, real_orientation const restrict*, real_matrix4x3 restrict*)
//.text:005B1780 ; void __cdecl matrix4x3_apply_orientation_cpp(real_matrix4x3 const*, real_orientation const*, real_matrix4x3*)
//.text:005B1AF0 ; void __cdecl matrix4x3_from_orientation(real_matrix4x3*, real_orientation const*)
//.text:005B1C70 ; void __cdecl matrix4x3_from_plane(real_matrix4x3*, real_plane3d const*)
//.text:005B1E10 ; void __cdecl matrix4x3_from_point_and_nonaxial_forward_and_left(real_matrix4x3*, real_point3d const*, real_vector3d const*, real_vector3d const*)
//.text:005B1E70 ; void __cdecl matrix4x3_from_point_and_nonaxial_vectors(real_matrix4x3*, real_point3d const*, real_vector3d const*, real_vector3d const*)
//.text:005B1ED0 ; void __cdecl matrix4x3_from_point_and_nonaxial_vectors_using_up(real_matrix4x3*, real_point3d const*, real_vector3d const*, real_vector3d const*)

void __cdecl matrix4x3_from_point_and_quaternion(real_matrix4x3* matrix, real_point3d const* point, real_quaternion const* quaternion)
{
INVOKE(0x005B1F30, matrix4x3_from_point_and_quaternion, matrix, point, quaternion);
}

void __cdecl matrix4x3_from_point_and_vectors(real_matrix4x3* matrix, real_point3d const* point, real_vector3d const* forward, real_vector3d const* up)
{
//INVOKE(0x005B20C0, matrix4x3_from_point_and_vectors, matrix, point, forward, up);

ASSERT(point);

matrix4x3_rotation_from_vectors(matrix, forward, up);
matrix->position = *point;
}

real_vector3d* __cdecl matrix4x3_transform_vector(real_matrix4x3 const* matrix, real_vector3d const* in_vector, real_vector3d* out_vector)
//.text:005B2170 ; void __cdecl matrix4x3_identity(real_matrix4x3*)

void __cdecl matrix4x3_inverse(real_matrix4x3 const* matrix, real_matrix4x3* result)
{
real forward = in_vector->n[0];
real left = in_vector->n[1];
real up = in_vector->n[2];
INVOKE(0x005B21E0, matrix4x3_inverse, matrix, result);

if (matrix->scale != 1.0)
real negative_x = -matrix->position.x;
real negative_y = -matrix->position.y;
real negative_z = -matrix->position.z;

if (matrix->scale == 1.0f)
{
forward *= matrix->scale;
left *= matrix->scale;
up *= matrix->scale;
result->scale = 1.0f;
}
else
{
result->scale = 1.0f / (matrix->scale < 0.0f ? fmaxf(matrix->scale, -_real_epsilon) : fmaxf(matrix->scale, _real_epsilon));

out_vector->n[0] = ((forward * matrix->forward.n[0]) + (left * matrix->left.n[0])) + (up * matrix->up.n[0]);
out_vector->n[1] = ((forward * matrix->forward.n[1]) + (left * matrix->left.n[1])) + (up * matrix->up.n[1]);
out_vector->n[2] = ((forward * matrix->forward.n[2]) + (left * matrix->left.n[2])) + (up * matrix->up.n[2]);
negative_x *= result->scale;
negative_y *= result->scale;
negative_z *= result->scale;
}

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

real_point3d* __cdecl matrix4x3_transform_point(real_matrix4x3 const* matrix, real_point3d const* in_point, real_point3d* out_point)
{
real forward = in_point->n[0] * matrix->scale;
real left = in_point->n[1] * matrix->scale;
real up = in_point->n[2] * matrix->scale;
result->left.i = matrix->forward.j;
result->forward.j = matrix->left.i;
result->up.i = matrix->forward.k;

out_point->n[0] = (((matrix->left.n[0] * left) + (matrix->forward.n[0] * forward)) + (matrix->up.n[0] * up)) + matrix->position.n[0];
out_point->n[1] = (((matrix->left.n[1] * left) + (matrix->forward.n[1] * forward)) + (matrix->up.n[1] * up)) + matrix->position.n[1];
out_point->n[2] = (((matrix->left.n[2] * left) + (matrix->forward.n[2] * forward)) + (matrix->up.n[2] * up)) + matrix->position.n[2];
result->forward.k = matrix->up.i;
result->up.j = matrix->left.k;
result->left.k = matrix->up.j;

return out_point;
result->position.x = ((negative_x * result->forward.i) + (negative_y * result->left.i)) + (negative_z * result->up.i);
result->position.y = ((negative_x * result->forward.j) + (negative_y * result->left.j)) + (negative_z * result->up.j);
result->position.z = ((negative_x * result->forward.k) + (negative_y * result->left.k)) + (negative_z * result->up.k);
}

void __cdecl matrix4x3_transform_points(real_matrix4x3 const* matrix, long total_point_count, real_point3d const* const points, real_point3d* const out_points)
//.text:005B2320 ; real_vector3d* __cdecl matrix4x3_inverse_transform_normal(real_matrix4x3 const*, real_vector3d const*, real_vector3d*)
//.text:005B23C0 ; real_plane3d* __cdecl matrix4x3_inverse_transform_plane(real_matrix4x3 const*, real_plane3d const*, real_plane3d*)
//.text:005B24D0 ; real_point3d* __cdecl matrix4x3_inverse_transform_point(real_matrix4x3 const*, real_point3d const*, real_point3d*)
//.text:005B25D0 ; void __cdecl matrix4x3_inverse_transform_points(real_matrix4x3 const*, long, real_point3d const* const, real_point3d* const)
//.text:005B2710 ; real_vector3d* __cdecl matrix4x3_inverse_transform_vector(real_matrix4x3 const*, real_vector3d const*, real_vector3d*)

void __cdecl matrix4x3_multiply(real_matrix4x3 const* a, real_matrix4x3 const* b, real_matrix4x3* result)
{
for (long i = 0; i < total_point_count; ++i)
matrix4x3_transform_point(matrix, &points[i], &out_points[i]);
//INVOKE(0x005B2800, matrix4x3_multiply, a, b, result);

if (a == result)
csmemcpy(result, a, sizeof(real_matrix4x3));

if (b == result)
csmemcpy(result, b, sizeof(real_matrix4x3));

result->forward.n[0] = ((a->forward.n[0] * b->forward.n[0]) + (a->left.n[0] * b->forward.n[1])) + (a->up.n[0] * b->forward.n[2]);
result->forward.n[1] = ((a->forward.n[1] * b->forward.n[0]) + (a->left.n[1] * b->forward.n[1])) + (a->up.n[1] * b->forward.n[2]);
result->forward.n[2] = ((a->forward.n[2] * b->forward.n[0]) + (a->left.n[2] * b->forward.n[1])) + (a->up.n[2] * b->forward.n[2]);

result->left.n[0] = ((a->forward.n[0] * b->left.n[0]) + (a->left.n[0] * b->left.n[1])) + (a->up.n[0] * b->left.n[2]);
result->left.n[1] = ((a->forward.n[1] * b->left.n[0]) + (a->left.n[1] * b->left.n[1])) + (a->up.n[1] * b->left.n[2]);
result->left.n[2] = ((a->forward.n[2] * b->left.n[0]) + (a->left.n[2] * b->left.n[1])) + (a->up.n[2] * b->left.n[2]);

result->up.n[0] = ((a->forward.n[0] * b->up.n[0]) + (a->left.n[0] * b->up.n[1])) + (a->up.n[0] * b->up.n[2]);
result->up.n[1] = ((a->forward.n[1] * b->up.n[0]) + (a->left.n[1] * b->up.n[1])) + (a->up.n[1] * b->up.n[2]);
result->up.n[2] = ((a->forward.n[2] * b->up.n[0]) + (a->left.n[2] * b->up.n[1])) + (a->up.n[2] * b->up.n[2]);

result->position.n[0] = a->position.n[0] + (a->scale * (((a->forward.n[0] * b->position.n[0]) + (a->left.n[0] * b->position.n[1])) + (a->up.n[0] * b->position.n[2])));
result->position.n[1] = a->position.n[1] + (a->scale * (((a->forward.n[1] * b->position.n[0]) + (a->left.n[1] * b->position.n[1])) + (a->up.n[1] * b->position.n[2])));
result->position.n[2] = a->position.n[2] + (a->scale * (((a->forward.n[2] * b->position.n[0]) + (a->left.n[2] * b->position.n[1])) + (a->up.n[2] * b->position.n[2])));

result->scale = a->scale * b->scale;
}

void __cdecl matrix4x3_multiply(real_matrix4x3 const* in_matrix0, real_matrix4x3 const* in_matrix1, real_matrix4x3* out_matrix)
{
if (in_matrix0 == out_matrix)
csmemcpy(out_matrix, in_matrix0, sizeof(real_matrix4x3));
//.text:005B2A90 ; void __cdecl matrix4x3_multiply_cpp(real_matrix4x3 const* a, real_matrix4x3 const* b, real_matrix4x3* result)
//.text:005B2D20 ; void __cdecl matrix4x3_rotation_between_vectors(real_matrix4x3*, real_vector3d const*, real_vector3d const*)
//.text:005B3100 ; void __cdecl matrix4x3_rotation_from_angles(real_matrix4x3*, real, real, real)
//.text:005B3270 ; void __cdecl matrix4x3_rotation_from_axis_and_angle(real_matrix4x3*, real_vector3d const*, real, real)
//.text:005B33A0 ; void __cdecl matrix4x3_rotation_from_nonaxial_forward_and_left(real_matrix4x3*, real_vector3d const*, real_vector3d const*)
//.text:005B33E0 ; void __cdecl matrix4x3_rotation_from_nonaxial_vectors(real_matrix4x3*, real_vector3d const*, real_vector3d const*)
//.text:005B3420 ; void __cdecl matrix4x3_rotation_from_nonaxial_vectors_using_up(real_matrix4x3*, real_vector3d const*, real_vector3d const*)
//.text:005B3460 ; void __cdecl matrix4x3_rotation_from_quaternion(real_matrix4x3*, real_quaternion const*)

if (in_matrix1 == out_matrix)
csmemcpy(out_matrix, in_matrix1, sizeof(real_matrix4x3));
void __cdecl matrix4x3_rotation_from_vectors(real_matrix4x3* matrix, real_vector3d const* forward, real_vector3d const* up)
{
//INVOKE(0x005B35D0, matrix4x3_rotation_from_vectors, matrix, forward, up);

out_matrix->forward.n[0] = ((in_matrix0->forward.n[0] * in_matrix1->forward.n[0]) + (in_matrix0->left.n[0] * in_matrix1->forward.n[1])) + (in_matrix0->up.n[0] * in_matrix1->forward.n[2]);
out_matrix->forward.n[1] = ((in_matrix0->forward.n[1] * in_matrix1->forward.n[0]) + (in_matrix0->left.n[1] * in_matrix1->forward.n[1])) + (in_matrix0->up.n[1] * in_matrix1->forward.n[2]);
out_matrix->forward.n[2] = ((in_matrix0->forward.n[2] * in_matrix1->forward.n[0]) + (in_matrix0->left.n[2] * in_matrix1->forward.n[1])) + (in_matrix0->up.n[2] * in_matrix1->forward.n[2]);
matrix->scale = 1.0f;
matrix->forward = *forward;
cross_product3d(up, forward, &matrix->left);
matrix->up = *up;
set_real_point3d(&matrix->position, 0.0f, 0.0f, 0.0f);
}

out_matrix->left.n[0] = ((in_matrix0->forward.n[0] * in_matrix1->left.n[0]) + (in_matrix0->left.n[0] * in_matrix1->left.n[1])) + (in_matrix0->up.n[0] * in_matrix1->left.n[2]);
out_matrix->left.n[1] = ((in_matrix0->forward.n[1] * in_matrix1->left.n[0]) + (in_matrix0->left.n[1] * in_matrix1->left.n[1])) + (in_matrix0->up.n[1] * in_matrix1->left.n[2]);
out_matrix->left.n[2] = ((in_matrix0->forward.n[2] * in_matrix1->left.n[0]) + (in_matrix0->left.n[2] * in_matrix1->left.n[1])) + (in_matrix0->up.n[2] * in_matrix1->left.n[2]);
//.text:005B3670 ; void __cdecl matrix4x3_rotation_to_angles(real_matrix4x3*, real_euler_angles3d*)
//.text:005B37D0 ; void __cdecl matrix4x3_rotation_to_quaternion(real_matrix4x3 const*, real_quaternion*)
//.text:005B37E0 ; void __cdecl matrix4x3_scale(real_matrix4x3*, real)
//.text:005B3850 ; void __cdecl matrix4x3_to_orientation(real_matrix4x3 const*, real_orientation*)
//.text:005B3890 ; void __cdecl matrix4x3_to_point_and_vectors(real_matrix4x3 const*, real_point3d*, real_vector3d*, real_vector3d*)

out_matrix->up.n[0] = ((in_matrix0->forward.n[0] * in_matrix1->up.n[0]) + (in_matrix0->left.n[0] * in_matrix1->up.n[1])) + (in_matrix0->up.n[0] * in_matrix1->up.n[2]);
out_matrix->up.n[1] = ((in_matrix0->forward.n[1] * in_matrix1->up.n[0]) + (in_matrix0->left.n[1] * in_matrix1->up.n[1])) + (in_matrix0->up.n[1] * in_matrix1->up.n[2]);
out_matrix->up.n[2] = ((in_matrix0->forward.n[2] * in_matrix1->up.n[0]) + (in_matrix0->left.n[2] * in_matrix1->up.n[1])) + (in_matrix0->up.n[2] * in_matrix1->up.n[2]);
real_vector3d* __cdecl matrix4x3_transform_normal(real_matrix4x3 const* matrix, real_vector3d const* normal, real_vector3d* result)
{
//return INVOKE(0x005B38D0, matrix4x3_transform_normal, matrix, normal, result);

out_matrix->position.n[0] = in_matrix0->position.n[0] + (in_matrix0->scale * (((in_matrix0->forward.n[0] * in_matrix1->position.n[0]) + (in_matrix0->left.n[0] * in_matrix1->position.n[1])) + (in_matrix0->up.n[0] * in_matrix1->position.n[2])));
out_matrix->position.n[1] = in_matrix0->position.n[1] + (in_matrix0->scale * (((in_matrix0->forward.n[1] * in_matrix1->position.n[0]) + (in_matrix0->left.n[1] * in_matrix1->position.n[1])) + (in_matrix0->up.n[1] * in_matrix1->position.n[2])));
out_matrix->position.n[2] = in_matrix0->position.n[2] + (in_matrix0->scale * (((in_matrix0->forward.n[2] * in_matrix1->position.n[0]) + (in_matrix0->left.n[2] * in_matrix1->position.n[1])) + (in_matrix0->up.n[2] * in_matrix1->position.n[2])));
result->i = ((normal->i * matrix->forward.i) + (normal->j * matrix->left.i)) + (normal->k * matrix->up.i);
result->j = ((normal->i * matrix->forward.j) + (normal->j * matrix->left.j)) + (normal->k * matrix->up.j);
result->k = ((normal->i * matrix->forward.k) + (normal->j * matrix->left.k)) + (normal->k * matrix->up.k);

out_matrix->scale = in_matrix0->scale * in_matrix1->scale;
return result;
}

void __cdecl matrix4x3_inverse(real_matrix4x3 const* matrix, real_matrix4x3* out_matrix)
real_plane3d* __cdecl matrix4x3_transform_plane(real_matrix4x3 const* matrix, real_plane3d const* plane, real_plane3d* result)
{
real negative_x = -matrix->position.x;
real negative_y = -matrix->position.y;
real negative_z = -matrix->position.z;
//return INVOKE(0x005B3970, matrix4x3_transform_plane, matrix, plane, result);

if (matrix->scale == 1.0f)
{
out_matrix->scale = 1.0f;
}
else
{
out_matrix->scale = 1.0f / (matrix->scale < 0.0f ? fmaxf(matrix->scale, -_real_epsilon) : fmaxf(matrix->scale, _real_epsilon));
matrix4x3_transform_normal(matrix, &plane->n, &result->n);
result->d = (matrix->scale * plane->d) + dot_product3d((real_vector3d*)&matrix->position, &result->n);

negative_x *= out_matrix->scale;
negative_y *= out_matrix->scale;
negative_z *= out_matrix->scale;
}
return result;
}

out_matrix->forward.i = matrix->forward.i;
out_matrix->left.j = matrix->left.j;
out_matrix->up.k = matrix->up.k;
real_point3d* __cdecl matrix4x3_transform_point(real_matrix4x3 const* matrix, real_point3d const* point, real_point3d* result)
{
//return INVOKE(0x005B3A40, matrix4x3_transform_point, matrix, point, result);

out_matrix->left.i = matrix->forward.j;
out_matrix->forward.j = matrix->left.i;
out_matrix->up.i = matrix->forward.k;
real forward = point->n[0] * matrix->scale;
real left = point->n[1] * matrix->scale;
real up = point->n[2] * matrix->scale;

out_matrix->forward.k = matrix->up.i;
out_matrix->up.j = matrix->left.k;
out_matrix->left.k = matrix->up.j;
result->n[0] = (((matrix->left.n[0] * left) + (matrix->forward.n[0] * forward)) + (matrix->up.n[0] * up)) + matrix->position.n[0];
result->n[1] = (((matrix->left.n[1] * left) + (matrix->forward.n[1] * forward)) + (matrix->up.n[1] * up)) + matrix->position.n[1];
result->n[2] = (((matrix->left.n[2] * left) + (matrix->forward.n[2] * forward)) + (matrix->up.n[2] * up)) + matrix->position.n[2];

out_matrix->position.x = ((negative_x * out_matrix->forward.i) + (negative_y * out_matrix->left.i)) + (negative_z * out_matrix->up.i);
out_matrix->position.y = ((negative_x * out_matrix->forward.j) + (negative_y * out_matrix->left.j)) + (negative_z * out_matrix->up.j);
out_matrix->position.z = ((negative_x * out_matrix->forward.k) + (negative_y * out_matrix->left.k)) + (negative_z * out_matrix->up.k);
return result;
}

real_vector3d* __cdecl matrix4x3_transform_normal(real_matrix4x3 const* matrix, real_vector3d const* vector, real_vector3d* out_vector)
void __cdecl matrix4x3_transform_points(real_matrix4x3 const* matrix, long point_count, real_point3d const* const points, real_point3d* const results)
{
out_vector->i = ((vector->i * matrix->forward.i) + (vector->j * matrix->left.i)) + (vector->k * matrix->up.i);
out_vector->j = ((vector->i * matrix->forward.j) + (vector->j * matrix->left.j)) + (vector->k * matrix->up.j);
out_vector->k = ((vector->i * matrix->forward.k) + (vector->j * matrix->left.k)) + (vector->k * matrix->up.k);
//INVOKE(0x005B3B00, matrix4x3_transform_points, matrix, point_count, points, results);

return out_vector;
for (long i = 0; i < point_count; ++i)
matrix4x3_transform_point(matrix, &points[i], &results[i]);
}

real_plane3d* __cdecl matrix4x3_transform_plane(real_matrix4x3 const* matrix, real_plane3d const* plane, real_plane3d* out_plane)
real_vector3d* __cdecl matrix4x3_transform_vector(real_matrix4x3 const* matrix, real_vector3d const* vector, real_vector3d* result)
{
matrix4x3_transform_normal(matrix, &plane->n, &out_plane->n);
out_plane->d = (matrix->scale * plane->d) + dot_product3d((real_vector3d*)&matrix->position, &out_plane->n);
//return INVOKE(0x005B3BD0, matrix4x3_transform_vector, matrix, vector, result);

real forward = vector->n[0];
real left = vector->n[1];
real up = vector->n[2];

if (matrix->scale != 1.0)
{
forward *= matrix->scale;
left *= matrix->scale;
up *= matrix->scale;
}

result->n[0] = ((forward * matrix->forward.n[0]) + (left * matrix->left.n[0])) + (up * matrix->up.n[0]);
result->n[1] = ((forward * matrix->forward.n[1]) + (left * matrix->left.n[1])) + (up * matrix->up.n[1]);
result->n[2] = ((forward * matrix->forward.n[2]) + (left * matrix->left.n[2])) + (up * matrix->up.n[2]);

return out_plane;
return result;
}

//.text:005B3C90 ; void __cdecl matrix4x3_translation(real_matrix4x3*, real_point3d const*)
//.text:005B3CF0 ; void __cdecl matrix4x3_transpose(real_matrix4x3*)

//.text:005B3F40 ; real_vector3d* __cdecl vector_from_matrices4x3(real_matrix4x3 const*, real_matrix4x3 const*, real_vector3d*)

Loading

0 comments on commit c56d1e9

Please sign in to comment.