Matrix/Vector multiplication and changes
Change Mat3 values to column-major (I think?) order. Add Matrix/Matrix multiplication and tweak Matrix/Vector multiplication. Fix Matrix rotation values since Mat3 representation changed. Added more UnitTest tests.
This commit is contained in:
parent
2cffea2604
commit
de126aa150
3 changed files with 266 additions and 35 deletions
116
src/sc_sacd.cpp
116
src/sc_sacd.cpp
|
|
@ -27,6 +27,30 @@ SC_SACD_Vec3 operator/(const SC_SACD_Vec3 &a, float scalar) {
|
|||
return SC_SACD_Vec3{a.x / scalar, a.y / scalar, a.z / scalar};
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 operator*(const SC_SACD_Mat3 &a, const SC_SACD_Mat3 &b) {
|
||||
SC_SACD_Mat3 mat;
|
||||
|
||||
mat.x0 = b.x0 * a.x0 + b.y0 * a.x1 + b.z0 * a.x2;
|
||||
mat.y0 = b.x0 * a.y0 + b.y0 * a.y1 + b.z0 * a.y2;
|
||||
mat.z0 = b.x0 * a.z0 + b.y0 * a.z1 + b.z0 * a.z2;
|
||||
|
||||
mat.x1 = b.x1 * a.x0 + b.y1 * a.x1 + b.z1 * a.x2;
|
||||
mat.y1 = b.x1 * a.y0 + b.y1 * a.y1 + b.z1 * a.y2;
|
||||
mat.z1 = b.x1 * a.z0 + b.y1 * a.z1 + b.z1 * a.z2;
|
||||
|
||||
mat.x2 = b.x2 * a.x0 + b.y2 * a.x1 + b.z2 * a.x2;
|
||||
mat.y2 = b.x2 * a.y0 + b.y2 * a.y1 + b.z2 * a.y2;
|
||||
mat.z2 = b.x2 * a.z0 + b.y2 * a.z1 + b.z2 * a.z2;
|
||||
|
||||
return mat;
|
||||
}
|
||||
|
||||
SC_SACD_Vec3 operator*(const SC_SACD_Mat3 &mat, const SC_SACD_Vec3 &vec) {
|
||||
return SC_SACD_Vec3{vec.x * mat.x0 + vec.y * mat.x1 + vec.z * mat.x2,
|
||||
vec.x * mat.y0 + vec.y * mat.y1 + vec.z * mat.y2,
|
||||
vec.x * mat.z0 + vec.y * mat.z1 + vec.z * mat.z2};
|
||||
}
|
||||
|
||||
std::vector<SC_SACD_Vec3> SC_SACD_Get_Box_Normals(
|
||||
const SC_SACD_Generic_Box *box) {
|
||||
std::vector<SC_SACD_Vec3> normals;
|
||||
|
|
@ -294,13 +318,17 @@ SC_SACD_Vec3 SC_SACD_Cross_Product(const SC_SACD_Vec3 a, const SC_SACD_Vec3 b) {
|
|||
a.x * b.y - a.y * b.x};
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 SC_SACD_Mat3_Identity(void) {
|
||||
return SC_SACD_Mat3{1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F};
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 SC_SACD_Mat3_Mult(const SC_SACD_Mat3 *a, const SC_SACD_Mat3 *b) {
|
||||
return (*a) * (*b);
|
||||
}
|
||||
|
||||
SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(const SC_SACD_Mat3 *mat,
|
||||
const SC_SACD_Vec3 vec) {
|
||||
return SC_SACD_Vec3{
|
||||
vec.x * mat->x0 + vec.y * mat->y0 + vec.z * mat->z0,
|
||||
vec.x * mat->x1 + vec.y * mat->y1 + vec.z * mat->z1,
|
||||
vec.x * mat->x2 + vec.y * mat->y2 + vec.z * mat->z2,
|
||||
};
|
||||
return (*mat) * vec;
|
||||
}
|
||||
|
||||
SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis,
|
||||
|
|
@ -326,45 +354,69 @@ SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis,
|
|||
SC_SACD_Vec3 result;
|
||||
|
||||
// About x_axis.
|
||||
mat.x0 = 1.0F;
|
||||
mat.y0 = 0.0F;
|
||||
mat.z0 = 0.0F;
|
||||
mat.x1 = 0.0F;
|
||||
mat.y1 = std::cos(x_axis);
|
||||
mat.z1 = -std::sin(x_axis);
|
||||
mat.x2 = 0.0F;
|
||||
mat.y2 = -mat.z1;
|
||||
mat.z2 = mat.y1;
|
||||
mat = SC_SACD_Rotation_Mat3_XAxis(x_axis);
|
||||
|
||||
result = SC_SACD_Mat3_Vec3_Mult(&mat, vec);
|
||||
|
||||
// About y_axis.
|
||||
mat.x0 = std::cos(y_axis);
|
||||
mat.y0 = 0.0F;
|
||||
mat.z0 = std::sin(y_axis);
|
||||
mat.x1 = 0.0F;
|
||||
mat.y1 = 1.0F;
|
||||
mat.z1 = 0.0F;
|
||||
mat.x2 = -mat.z0;
|
||||
mat.y2 = 0.0F;
|
||||
mat.z2 = mat.x0;
|
||||
mat = SC_SACD_Rotation_Mat3_YAxis(y_axis);
|
||||
|
||||
result = SC_SACD_Mat3_Vec3_Mult(&mat, result);
|
||||
|
||||
// About z_axis.
|
||||
mat.x0 = std::cos(z_axis);
|
||||
mat.y0 = -std::sin(z_axis);
|
||||
mat.z0 = 0.0F;
|
||||
mat.x1 = -mat.y0;
|
||||
mat.y1 = mat.x0;
|
||||
mat.z1 = 0.0F;
|
||||
mat.x2 = 0.0F;
|
||||
mat.y2 = 0.0F;
|
||||
mat.z2 = 1.0F;
|
||||
mat = SC_SACD_Rotation_Mat3_ZAxis(z_axis);
|
||||
|
||||
return SC_SACD_Mat3_Vec3_Mult(&mat, result);
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians) {
|
||||
SC_SACD_Mat3 mat;
|
||||
|
||||
mat.x0 = 1.0F;
|
||||
mat.x1 = 0.0F;
|
||||
mat.x2 = 0.0F;
|
||||
mat.y0 = 0.0F;
|
||||
mat.y1 = std::cos(x_radians);
|
||||
mat.y2 = -std::sin(x_radians);
|
||||
mat.z0 = 0.0F;
|
||||
mat.z1 = -mat.y2;
|
||||
mat.z2 = mat.y1;
|
||||
|
||||
return mat;
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians) {
|
||||
SC_SACD_Mat3 mat;
|
||||
|
||||
mat.x0 = std::cos(y_radians);
|
||||
mat.x1 = 0.0F;
|
||||
mat.x2 = std::sin(y_radians);
|
||||
mat.y0 = 0.0F;
|
||||
mat.y1 = 1.0F;
|
||||
mat.y2 = 0.0F;
|
||||
mat.z0 = -mat.x2;
|
||||
mat.z1 = 0.0F;
|
||||
mat.z2 = mat.x0;
|
||||
|
||||
return mat;
|
||||
}
|
||||
|
||||
SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians) {
|
||||
SC_SACD_Mat3 mat;
|
||||
|
||||
mat.x0 = std::cos(z_radians);
|
||||
mat.x1 = -std::sin(z_radians);
|
||||
mat.x2 = 0.0F;
|
||||
mat.y0 = -mat.x1;
|
||||
mat.y1 = mat.x0;
|
||||
mat.y2 = 0.0F;
|
||||
mat.z0 = 0.0F;
|
||||
mat.z1 = 0.0F;
|
||||
mat.z2 = 1.0F;
|
||||
|
||||
return mat;
|
||||
}
|
||||
|
||||
SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized(const SC_SACD_Vec3 *pos,
|
||||
const SC_SACD_Vec3 *dir,
|
||||
const SC_SACD_Vec3 *point) {
|
||||
|
|
|
|||
|
|
@ -25,9 +25,9 @@ typedef struct SC_SACD_EXPORT SC_SACD_Vec3 {
|
|||
} SC_SACD_Vec3;
|
||||
|
||||
typedef struct SC_SACD_EXPORT SC_SACD_Mat3 {
|
||||
float x0, y0, z0;
|
||||
float x1, y1, z1;
|
||||
float x2, y2, z2;
|
||||
float x0, x1, x2;
|
||||
float y0, y1, y2;
|
||||
float z0, z1, z2;
|
||||
} SC_SACD_Mat3;
|
||||
|
||||
typedef struct SC_SACD_EXPORT SC_SACD_AABB_Box {
|
||||
|
|
@ -94,6 +94,11 @@ SC_SACD_EXPORT float SC_SACD_Dot_Product(const SC_SACD_Vec3 a,
|
|||
SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Cross_Product(const SC_SACD_Vec3 a,
|
||||
const SC_SACD_Vec3 b);
|
||||
|
||||
SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Identity(void);
|
||||
|
||||
SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Mult(const SC_SACD_Mat3 *a,
|
||||
const SC_SACD_Mat3 *b);
|
||||
|
||||
SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(const SC_SACD_Mat3 *mat,
|
||||
const SC_SACD_Vec3 vec);
|
||||
|
||||
|
|
@ -101,6 +106,10 @@ SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec,
|
|||
float x_axis, float y_axis,
|
||||
float z_axis);
|
||||
|
||||
SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians);
|
||||
SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians);
|
||||
SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians);
|
||||
|
||||
SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized(
|
||||
const SC_SACD_Vec3 *pos, const SC_SACD_Vec3 *dir,
|
||||
const SC_SACD_Vec3 *point);
|
||||
|
|
|
|||
170
src/test.cpp
170
src/test.cpp
|
|
@ -1,3 +1,4 @@
|
|||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <numbers>
|
||||
|
||||
|
|
@ -25,6 +26,16 @@ static int checks_passed = 0;
|
|||
} \
|
||||
} while (false);
|
||||
|
||||
#define CHECK_FLOAT(var, value) \
|
||||
do { \
|
||||
++checks_checked; \
|
||||
if ((var) > (value)-0.0001F && (var) < (value) + 0.0001F) { \
|
||||
++checks_passed; \
|
||||
} else { \
|
||||
std::cout << "CHECK_FLOAT at line " << __LINE__ << " failed!\n"; \
|
||||
} \
|
||||
} while (false);
|
||||
|
||||
#include "sc_sacd.h"
|
||||
|
||||
int main() {
|
||||
|
|
@ -358,6 +369,165 @@ int main() {
|
|||
CHECK_TRUE(SC_SACD_Sphere_Box_Collision(&sphere, &box));
|
||||
}
|
||||
|
||||
// Test matrix/vector multiplication.
|
||||
{
|
||||
SC_SACD_Mat3 mat_a{1.0F, 2.0F, 3.0F, 4.0F, 5.0F, 6.0F, 7.0F, 8.0F, 9.0F};
|
||||
|
||||
SC_SACD_Mat3 mat_b{1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F};
|
||||
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Mult(&mat_a, &mat_b);
|
||||
CHECK_TRUE(mat_a.x0 == result.x0);
|
||||
CHECK_TRUE(mat_a.x1 == result.x1);
|
||||
CHECK_TRUE(mat_a.x2 == result.x2);
|
||||
CHECK_TRUE(mat_a.y0 == result.y0);
|
||||
CHECK_TRUE(mat_a.y1 == result.y1);
|
||||
CHECK_TRUE(mat_a.y2 == result.y2);
|
||||
CHECK_TRUE(mat_a.z0 == result.z0);
|
||||
CHECK_TRUE(mat_a.z1 == result.z1);
|
||||
CHECK_TRUE(mat_a.z2 == result.z2);
|
||||
}
|
||||
|
||||
mat_b.x0 = 2.0F;
|
||||
mat_b.y1 = 0.0F;
|
||||
mat_b.z2 = 0.0F;
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Mult(&mat_a, &mat_b);
|
||||
CHECK_FLOAT(result.x0, 2.0F);
|
||||
CHECK_FLOAT(result.y0, 8.0F);
|
||||
CHECK_FLOAT(result.z0, 14.0F);
|
||||
CHECK_FLOAT(result.x1, 0.0F);
|
||||
CHECK_FLOAT(result.y1, 0.0F);
|
||||
CHECK_FLOAT(result.z1, 0.0F);
|
||||
CHECK_FLOAT(result.x2, 0.0F);
|
||||
CHECK_FLOAT(result.y2, 0.0F);
|
||||
CHECK_FLOAT(result.z2, 0.0F);
|
||||
}
|
||||
|
||||
mat_b = SC_SACD_Mat3_Identity();
|
||||
SC_SACD_Vec3 vec_a{1.0F, 0.0F, 0.0F};
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_b, vec_a);
|
||||
CHECK_TRUE(result.x == vec_a.x);
|
||||
CHECK_TRUE(result.y == vec_a.y);
|
||||
CHECK_TRUE(result.z == vec_a.z);
|
||||
}
|
||||
|
||||
// Rotations about each axis.
|
||||
mat_a = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v<float> / 2.0F);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < 1.0001F && result.y > 0.9999F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
mat_a = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v<float>);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < -0.9999F && result.x > -1.0001F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
mat_a =
|
||||
SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v<float> * 3.0F / 2.0F);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < -0.9999F && result.y > -1.0001F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
mat_a = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v<float> / 2.0F);
|
||||
vec_a.x = 0.0F;
|
||||
vec_a.y = 1.0F;
|
||||
vec_a.z = 0.0F;
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < 1.0001F && result.z > 0.9999F);
|
||||
}
|
||||
|
||||
mat_a = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v<float>);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < -0.9999F && result.y > -1.0001F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
mat_a =
|
||||
SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v<float> * 3.0F / 2.0F);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < -0.9999F && result.z > -1.0001F);
|
||||
}
|
||||
|
||||
mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v<float> / 2.0F);
|
||||
vec_a.x = 0.0F;
|
||||
vec_a.y = 0.0F;
|
||||
vec_a.z = 1.0F;
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 1.0001F && result.x > 0.9999F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v<float>);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < -0.9999F && result.z > -1.0001F);
|
||||
}
|
||||
|
||||
mat_a =
|
||||
SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v<float> * 3.0F / 2.0F);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_TRUE(result.x < -0.9999F && result.x > -1.0001F);
|
||||
CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F);
|
||||
CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F);
|
||||
}
|
||||
|
||||
// Combined axis rotation.
|
||||
vec_a.x = 1.0F;
|
||||
vec_a.y = 0.0F;
|
||||
vec_a.z = 0.0F;
|
||||
mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v<float> / 4.0F);
|
||||
mat_b = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v<float> / 4.0F);
|
||||
// Apply mat_a, then mat_b.
|
||||
mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_FLOAT(result.x, 0.5F);
|
||||
CHECK_FLOAT(result.y, 0.5F);
|
||||
CHECK_FLOAT(result.z, -std::sqrt(2.0F) / 2.0F);
|
||||
}
|
||||
// Apply another rotation on combined mat_a.
|
||||
mat_b = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v<float> / 4.0F);
|
||||
mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_FLOAT(result.x, 0.0F);
|
||||
CHECK_FLOAT(result.y, std::sqrt(2.0F) / 2.0F);
|
||||
CHECK_FLOAT(result.z, -std::sqrt(2.0F) / 2.0F);
|
||||
}
|
||||
// Apply another rotation on combined mat_a.
|
||||
mat_b = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v<float> / 2.0F);
|
||||
mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a);
|
||||
{
|
||||
auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a);
|
||||
CHECK_FLOAT(result.x, 0.0F);
|
||||
CHECK_FLOAT(result.y, std::sqrt(2.0F) / 2.0F);
|
||||
CHECK_FLOAT(result.z, std::sqrt(2.0F) / 2.0F);
|
||||
}
|
||||
}
|
||||
std::cout << "Checks checked: " << checks_checked << '\n'
|
||||
<< "Checks passed: " << checks_passed << '\n';
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue