diff --git a/basic-geometry/matrix2x2.c b/basic-geometry/matrix2x2.c index bb0e5c1..0f4927b 100644 --- a/basic-geometry/matrix2x2.c +++ b/basic-geometry/matrix2x2.c @@ -18,6 +18,9 @@ extern inline double bgc_matrix2x2_get_determinant_fp64(const BgcMatrix2x2FP64* extern inline int bgc_matrix2x2_is_singular_fp32(const BgcMatrix2x2FP32* matrix); extern inline int bgc_matrix2x2_is_singular_fp64(const BgcMatrix2x2FP64* matrix); +extern inline int bgc_matrix2x2_is_rotation_fp32(const BgcMatrix2x2FP32* matrix); +extern inline int bgc_matrix2x2_is_rotation_fp64(const BgcMatrix2x2FP64* matrix); + extern inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP32* destination); extern inline void bgc_matrix2x2_copy_fp64(const BgcMatrix2x2FP64* source, BgcMatrix2x2FP64* destination); diff --git a/basic-geometry/matrix2x2.h b/basic-geometry/matrix2x2.h index ad46eff..6bf6541 100644 --- a/basic-geometry/matrix2x2.h +++ b/basic-geometry/matrix2x2.h @@ -109,6 +109,32 @@ inline int bgc_matrix2x2_is_singular_fp64(const BgcMatrix2x2FP64* matrix) return bgc_is_zero_fp64(bgc_matrix2x2_get_determinant_fp64(matrix)); } +// ================ Is Rotation ================= // + +inline int bgc_matrix2x2_is_rotation_fp32(const BgcMatrix2x2FP32* matrix) +{ + const float product_r1c1 = matrix->r1c1 * matrix->r1c1 + matrix->r1c2 * matrix->r2c1; + const float product_r1c2 = matrix->r1c1 * matrix->r1c2 + matrix->r1c2 * matrix->r2c2; + + const float product_r2c1 = matrix->r2c1 * matrix->r1c1 + matrix->r2c2 * matrix->r2c1; + const float product_r2c2 = matrix->r2c1 * matrix->r1c2 + matrix->r2c2 * matrix->r2c2; + + return bgc_is_unit_fp32(product_r1c1) && bgc_is_zero_fp32(product_r1c2) + && bgc_is_zero_fp32(product_r2c1) && bgc_is_unit_fp32(product_r2c2); +} + +inline int bgc_matrix2x2_is_rotation_fp64(const BgcMatrix2x2FP64* matrix) +{ + const double product_r1c1 = matrix->r1c1 * matrix->r1c1 + matrix->r1c2 * matrix->r2c1; + const double product_r1c2 = matrix->r1c1 * matrix->r1c2 + matrix->r1c2 * matrix->r2c2; + + const double product_r2c1 = matrix->r2c1 * matrix->r1c1 + matrix->r2c2 * matrix->r2c1; + const double product_r2c2 = matrix->r2c1 * matrix->r1c2 + matrix->r2c2 * matrix->r2c2; + + return bgc_is_unit_fp64(product_r1c1) && bgc_is_zero_fp64(product_r1c2) + && bgc_is_zero_fp64(product_r2c1) && bgc_is_unit_fp64(product_r2c2); +} + // ==================== Copy ==================== // inline void bgc_matrix2x2_copy_fp32(const BgcMatrix2x2FP32* source, BgcMatrix2x2FP32* destination) diff --git a/basic-geometry/matrix3x3.c b/basic-geometry/matrix3x3.c index a67f481..fc61d36 100644 --- a/basic-geometry/matrix3x3.c +++ b/basic-geometry/matrix3x3.c @@ -24,6 +24,9 @@ extern inline double bgc_matrix3x3_get_determinant_fp64(const BgcMatrix3x3FP64* extern inline int bgc_matrix3x3_is_singular_fp32(const BgcMatrix3x3FP32* matrix); extern inline int bgc_matrix3x3_is_singular_fp64(const BgcMatrix3x3FP64* matrix); +extern inline int bgc_matrix3x3_is_rotation_fp32(const BgcMatrix3x3FP32* matrix); +extern inline int bgc_matrix3x3_is_rotation_fp64(const BgcMatrix3x3FP64* matrix); + extern inline void bgc_matrix3x3_transpose_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* transposed); extern inline void bgc_matrix3x3_transpose_fp64(const BgcMatrix3x3FP64* matrix, BgcMatrix3x3FP64* transposed); diff --git a/basic-geometry/matrix3x3.h b/basic-geometry/matrix3x3.h index c1ab5c2..a716d67 100644 --- a/basic-geometry/matrix3x3.h +++ b/basic-geometry/matrix3x3.h @@ -272,6 +272,46 @@ inline int bgc_matrix3x3_is_singular_fp64(const BgcMatrix3x3FP64* matrix) return bgc_is_zero_fp64(bgc_matrix3x3_get_determinant_fp64(matrix)); } +// ================ Is Rotation ================= // + +inline int bgc_matrix3x3_is_rotation_fp32(const BgcMatrix3x3FP32* matrix) +{ + const float product_r1c1 = matrix->r1c1 * matrix->r1c1 + matrix->r1c2 * matrix->r2c1 + matrix->r1c3 * matrix->r3c1; + const float product_r1c2 = matrix->r1c1 * matrix->r1c2 + matrix->r1c2 * matrix->r2c2 + matrix->r1c3 * matrix->r3c2; + const float product_r1c3 = matrix->r1c1 * matrix->r1c3 + matrix->r1c2 * matrix->r2c3 + matrix->r1c3 * matrix->r3c3; + + const float product_r2c1 = matrix->r2c1 * matrix->r1c1 + matrix->r2c2 * matrix->r2c1 + matrix->r2c3 * matrix->r3c1; + const float product_r2c2 = matrix->r2c1 * matrix->r1c2 + matrix->r2c2 * matrix->r2c2 + matrix->r2c3 * matrix->r3c2; + const float product_r2c3 = matrix->r2c1 * matrix->r1c3 + matrix->r2c2 * matrix->r2c3 + matrix->r2c3 * matrix->r3c3; + + const float product_r3c1 = matrix->r3c1 * matrix->r1c1 + matrix->r3c2 * matrix->r2c1 + matrix->r3c3 * matrix->r3c1; + const float product_r3c2 = matrix->r3c1 * matrix->r1c2 + matrix->r3c2 * matrix->r2c2 + matrix->r3c3 * matrix->r3c2; + const float product_r3c3 = matrix->r3c1 * matrix->r1c3 + matrix->r3c2 * matrix->r2c3 + matrix->r3c3 * matrix->r3c3; + + return bgc_is_unit_fp32(product_r1c1) && bgc_is_zero_fp32(product_r1c2) && bgc_is_zero_fp32(product_r1c3) + && bgc_is_zero_fp32(product_r2c1) && bgc_is_unit_fp32(product_r2c2) && bgc_is_zero_fp32(product_r2c3) + && bgc_is_zero_fp32(product_r3c1) && bgc_is_zero_fp32(product_r3c2) && bgc_is_unit_fp32(product_r3c3); +} + +inline int bgc_matrix3x3_is_rotation_fp64(const BgcMatrix3x3FP64* matrix) +{ + const double product_r1c1 = matrix->r1c1 * matrix->r1c1 + matrix->r1c2 * matrix->r2c1 + matrix->r1c3 * matrix->r3c1; + const double product_r1c2 = matrix->r1c1 * matrix->r1c2 + matrix->r1c2 * matrix->r2c2 + matrix->r1c3 * matrix->r3c2; + const double product_r1c3 = matrix->r1c1 * matrix->r1c3 + matrix->r1c2 * matrix->r2c3 + matrix->r1c3 * matrix->r3c3; + + const double product_r2c1 = matrix->r2c1 * matrix->r1c1 + matrix->r2c2 * matrix->r2c1 + matrix->r2c3 * matrix->r3c1; + const double product_r2c2 = matrix->r2c1 * matrix->r1c2 + matrix->r2c2 * matrix->r2c2 + matrix->r2c3 * matrix->r3c2; + const double product_r2c3 = matrix->r2c1 * matrix->r1c3 + matrix->r2c2 * matrix->r2c3 + matrix->r2c3 * matrix->r3c3; + + const double product_r3c1 = matrix->r3c1 * matrix->r1c1 + matrix->r3c2 * matrix->r2c1 + matrix->r3c3 * matrix->r3c1; + const double product_r3c2 = matrix->r3c1 * matrix->r1c2 + matrix->r3c2 * matrix->r2c2 + matrix->r3c3 * matrix->r3c2; + const double product_r3c3 = matrix->r3c1 * matrix->r1c3 + matrix->r3c2 * matrix->r2c3 + matrix->r3c3 * matrix->r3c3; + + return bgc_is_unit_fp64(product_r1c1) && bgc_is_zero_fp64(product_r1c2) && bgc_is_zero_fp64(product_r1c3) + && bgc_is_zero_fp64(product_r2c1) && bgc_is_unit_fp64(product_r2c2) && bgc_is_zero_fp64(product_r2c3) + && bgc_is_zero_fp64(product_r3c1) && bgc_is_zero_fp64(product_r3c2) && bgc_is_unit_fp64(product_r3c3); +} + // =================== Invert =================== // int bgc_matrix3x3_invert_fp32(const BgcMatrix3x3FP32* matrix, BgcMatrix3x3FP32* inverted); diff --git a/basic-geometry/slerp.c b/basic-geometry/slerp.c index b318059..2e03264 100644 --- a/basic-geometry/slerp.c +++ b/basic-geometry/slerp.c @@ -3,8 +3,8 @@ extern inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp); extern inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp); -extern inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result); -extern inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result); +extern inline void bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result); +extern inline void bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result); void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp) { diff --git a/basic-geometry/slerp.h b/basic-geometry/slerp.h index 9289008..a4a2053 100644 --- a/basic-geometry/slerp.h +++ b/basic-geometry/slerp.h @@ -61,7 +61,7 @@ void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp); -inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result) +inline void bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result) { const float angle = slerp->radians * phase; const float cosine = cosf(angle); @@ -76,7 +76,7 @@ inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float ); } -inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result) +inline void bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result) { const double angle = slerp->radians * phase; const double cosine = cos(angle);