diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c index bc0c0a6..eedfc22 100644 --- a/basic-geometry-dev/main.c +++ b/basic-geometry-dev/main.c @@ -53,12 +53,12 @@ structure_fp32_t* make_structures(const unsigned int amount) void print_versor_fp32(const BgcVersorFP32* versor) { - printf("Versor (s0 = %0.12f, x1 = %0.12f, x2 = %0.12f, x3 = %0.12f)\n", versor->s0, versor->x1, versor->x2, versor->x3); + printf("Versor (s0 = %0.12f, x1 = %0.12f, x2 = %0.12f, x3 = %0.12f)\n", versor->_s0, versor->_x1, versor->_x2, versor->_x3); } void print_versor_fp64(const BgcVersorFP64* versor) { - printf("Versor (s0 = %0.20f, x1 = %0.20f, x2 = %0.20f, x3 = %0.20f)\n", versor->s0, versor->x1, versor->x2, versor->x3); + printf("Versor (s0 = %0.20f, x1 = %0.20f, x2 = %0.20f, x3 = %0.20f)\n", versor->_s0, versor->_x1, versor->_x2, versor->_x3); } void print_vector_fp32(const BgcVector3FP32* vector) @@ -469,11 +469,16 @@ void test_basis_difference_fp64() print_versor_fp64(&turn); } +typedef union { + int si32; + float fp32; +} uni_number; + int main() { //BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f }; //BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f }; - + /* BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f }; BgcVersorFP32 end = { 0.9999f, 0.01414f, 0.0f, 0.0f }; BgcSlerpFP32 slerp; @@ -482,8 +487,14 @@ int main() bgc_slerp_get_turn_for_phase_fp32(&slerp, 0.5f, &result); print_versor_fp32(&result); - + */ //test_basis_difference_fp64(); + uni_number number; + + number.fp32 = 2.0f; + + printf("%x\n", number.si32); + return 0; } diff --git a/basic-geometry-test/tests/complex/complex_arithmetics.c b/basic-geometry-test/tests/complex/complex_arithmetics.c index eaf3b45..c3cf581 100644 --- a/basic-geometry-test/tests/complex/complex_arithmetics.c +++ b/basic-geometry-test/tests/complex/complex_arithmetics.c @@ -159,35 +159,6 @@ void test_complex_subtract_fp32() print_testing_success(); } -void test_complex_subtract_scaled_fp32() -{ - BgcComplexFP32 vector1, vector2, result; - - print_testing_name("bgc_complex_subtract_scaled_fp32"); - - bgc_complex_set_values_fp32(10.0f, -20.0f, &vector1); - bgc_complex_set_values_fp32(4.0f, 5.0f, &vector2); - - bgc_complex_subtract_scaled_fp32(&vector1, &vector2, 2.0f, &result); - - if (!bgc_are_close_fp32(result.real, 2.0f) || !bgc_are_close_fp32(result.imaginary, -30.0f)) { - print_testing_error("first test failed"); - return; - } - - bgc_complex_set_values_fp32(0.36f, 100.4f, &vector1); - bgc_complex_set_values_fp32(1.09f, 0.1f, &vector2); - - bgc_complex_subtract_scaled_fp32(&vector1, &vector2, 4.0f, &result); - - if (!bgc_are_close_fp32(result.real, -4.0f) || !bgc_are_close_fp32(result.imaginary, 100.0f)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_complex_subtract_fp64() { BgcComplexFP64 vector1, vector2, result; @@ -217,42 +188,10 @@ void test_complex_subtract_fp64() print_testing_success(); } -void test_complex_subtract_scaled_fp64() -{ - BgcComplexFP64 vector1, vector2, result; - - print_testing_name("bgc_complex_subtract_scaled_fp64"); - - bgc_complex_set_values_fp64(10.0, 20.0, &vector1); - bgc_complex_set_values_fp64(4.0, 5.0, &vector2); - - bgc_complex_subtract_scaled_fp64(&vector1, &vector2, 2.5, &result); - - if (!bgc_are_close_fp64(result.real, 0.0) || !bgc_are_close_fp64(result.imaginary, 7.5)) { - print_testing_error("first test failed"); - return; - } - - bgc_complex_set_values_fp64(-0.27, 100.3, &vector1); - bgc_complex_set_values_fp64(-1.29, -0.1, &vector2); - - bgc_complex_subtract_scaled_fp64(&vector1, &vector2, 3.0, &result); - - if (!bgc_are_close_fp64(result.real, 3.6) || !bgc_are_close_fp64(result.imaginary, 100.6)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_complex_subtract() { test_complex_subtract_fp32(); test_complex_subtract_fp64(); - - test_complex_subtract_scaled_fp32(); - test_complex_subtract_scaled_fp64(); } // ================== Multiply ================== // @@ -265,7 +204,7 @@ void test_complex_multiply_fp32() bgc_complex_set_values_fp32(10.0f, -20.0f, &vector); - bgc_complex_multiply_fp32(&vector, 0.5f, &result); + bgc_complex_multiply_by_number_fp32(&vector, 0.5f, &result); if (!bgc_are_close_fp32(result.real, 5.0f) || !bgc_are_close_fp32(result.imaginary, -10.0f)) { print_testing_error("first test failed"); @@ -274,7 +213,7 @@ void test_complex_multiply_fp32() bgc_complex_set_values_fp32(1.78f, -0.1f, &vector); - bgc_complex_multiply_fp32(&vector, 2.0f, &result); + bgc_complex_multiply_by_number_fp32(&vector, 2.0f, &result); if (!bgc_are_close_fp32(result.real, 3.56f) || !bgc_are_close_fp32(result.imaginary, -0.2f)) { print_testing_error("second test failed"); @@ -292,7 +231,7 @@ void test_complex_multiply_fp64() bgc_complex_set_values_fp64(30.0, -10.0, &vector); - bgc_complex_multiply_fp64(&vector, 0.3, &result); + bgc_complex_multiply_by_number_fp64(&vector, 0.3, &result); if (!bgc_are_close_fp64(result.real, 9.0) || !bgc_are_close_fp64(result.imaginary, -3.0)) { print_testing_error("first test failed"); @@ -301,7 +240,7 @@ void test_complex_multiply_fp64() bgc_complex_set_values_fp64(1.18, -0.25, &vector); - bgc_complex_multiply_fp64(&vector, 4.0, &result); + bgc_complex_multiply_by_number_fp64(&vector, 4.0, &result); if (!bgc_are_close_fp64(result.real, 4.72) || !bgc_are_close_fp64(result.imaginary, -1.0)) { print_testing_error("second test failed"); @@ -327,7 +266,7 @@ void test_complex_divide_fp32() bgc_complex_set_values_fp32(10.0f, -20.0f, &vector); - bgc_complex_divide_fp32(&vector, 10.0f, &result); + bgc_complex_divide_by_number_fp32(&vector, 10.0f, &result); if (!bgc_are_close_fp32(result.real, 1.0f) || !bgc_are_close_fp32(result.imaginary, -2.0f)) { print_testing_error("first test failed"); @@ -336,7 +275,7 @@ void test_complex_divide_fp32() bgc_complex_set_values_fp32(1.78f, -0.1f, &vector); - bgc_complex_divide_fp32(&vector, 0.2f, &result); + bgc_complex_divide_by_number_fp32(&vector, 0.2f, &result); if (!bgc_are_close_fp32(result.real, 8.9f) || !bgc_are_close_fp32(result.imaginary, -0.5f)) { print_testing_error("second test failed"); @@ -354,7 +293,7 @@ void test_complex_divide_fp64() bgc_complex_set_values_fp64(30.0, -10.0, &vector); - bgc_complex_divide_fp64(&vector, 5.0, &result); + bgc_complex_divide_by_number_fp64(&vector, 5.0, &result); if (!bgc_are_close_fp64(result.real, 6.0) || !bgc_are_close_fp64(result.imaginary, -2.0)) { print_testing_error("first test failed"); @@ -363,7 +302,7 @@ void test_complex_divide_fp64() bgc_complex_set_values_fp64(1.18, -0.25, &vector); - bgc_complex_divide_fp64(&vector, 0.5, &result); + bgc_complex_divide_by_number_fp64(&vector, 0.5, &result); if (!bgc_are_close_fp64(result.real, 2.36) || !bgc_are_close_fp64(result.imaginary, -0.5)) { print_testing_error("second test failed"); diff --git a/basic-geometry-test/tests/complex/complex_arithmetics.h b/basic-geometry-test/tests/complex/complex_arithmetics.h index ecbae95..0ba6566 100644 --- a/basic-geometry-test/tests/complex/complex_arithmetics.h +++ b/basic-geometry-test/tests/complex/complex_arithmetics.h @@ -17,12 +17,8 @@ void test_complex_add(); void test_complex_subtract_fp32(); -void test_complex_subtract_scaled_fp32(); - void test_complex_subtract_fp64(); -void test_complex_subtract_scaled_fp64(); - void test_complex_subtract(); // ================== Multiply ================== // diff --git a/basic-geometry-test/tests/quaternion/quaternion_set_to_identity.c b/basic-geometry-test/tests/quaternion/quaternion_set_to_identity.c index c493ae9..36d6766 100644 --- a/basic-geometry-test/tests/quaternion/quaternion_set_to_identity.c +++ b/basic-geometry-test/tests/quaternion/quaternion_set_to_identity.c @@ -8,7 +8,7 @@ void test_quaternion_set_to_identity_fp32() print_testing_name("bgc_quaternion_set_to_identity_fp32"); - bgc_quaternion_set_to_identity_fp32(&vector); + bgc_quaternion_make_unit_fp32(&vector); if (vector.s0 != 1.0f || vector.x1 != 0.0f || vector.x2 != 0.0f || vector.x3 != 0.0f) { print_testing_failed(); @@ -24,7 +24,7 @@ void test_quaternion_set_to_identity_fp64() print_testing_name("bgc_quaternion_set_to_identity_fp64"); - bgc_quaternion_set_to_identity_fp64(&vector); + bgc_quaternion_make_unit_fp64(&vector); if (vector.s0 != 1.0 || vector.x1 != 0.0 || vector.x2 != 0.0 || vector.x3 != 0.0) { print_testing_failed(); diff --git a/basic-geometry-test/tests/vector2/vector2_arithmetics.c b/basic-geometry-test/tests/vector2/vector2_arithmetics.c index ee87b58..727b571 100644 --- a/basic-geometry-test/tests/vector2/vector2_arithmetics.c +++ b/basic-geometry-test/tests/vector2/vector2_arithmetics.c @@ -159,35 +159,6 @@ void test_vector2_subtract_fp32() print_testing_success(); } -void test_vector2_subtract_scaled_fp32() -{ - BgcVector2FP32 vector1, vector2, result; - - print_testing_name("bgc_vector2_subtract_scaled_fp32"); - - bgc_vector2_set_values_fp32(10.0f, -20.0f, &vector1); - bgc_vector2_set_values_fp32(4.0f, 5.0f, &vector2); - - bgc_vector2_subtract_scaled_fp32(&vector1, &vector2, 2.0f, &result); - - if (!bgc_are_close_fp32(result.x1, 2.0f) || !bgc_are_close_fp32(result.x2, -30.0f)) { - print_testing_error("first test failed"); - return; - } - - bgc_vector2_set_values_fp32(0.36f, 100.4f, &vector1); - bgc_vector2_set_values_fp32(1.09f, 0.1f, &vector2); - - bgc_vector2_subtract_scaled_fp32(&vector1, &vector2, 4.0f, &result); - - if (!bgc_are_close_fp32(result.x1, -4.0f) || !bgc_are_close_fp32(result.x2, 100.0f)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_vector2_subtract_fp64() { BgcVector2FP64 vector1, vector2, result; @@ -217,42 +188,10 @@ void test_vector2_subtract_fp64() print_testing_success(); } -void test_vector2_subtract_scaled_fp64() -{ - BgcVector2FP64 vector1, vector2, result; - - print_testing_name("bgc_vector2_subtract_scaled_fp64"); - - bgc_vector2_set_values_fp64(10.0, 20.0, &vector1); - bgc_vector2_set_values_fp64(4.0, 5.0, &vector2); - - bgc_vector2_subtract_scaled_fp64(&vector1, &vector2, 2.5, &result); - - if (!bgc_are_close_fp64(result.x1, 0.0) || !bgc_are_close_fp64(result.x2, 7.5)) { - print_testing_error("first test failed"); - return; - } - - bgc_vector2_set_values_fp64(-0.27, 100.3, &vector1); - bgc_vector2_set_values_fp64(-1.29, -0.1, &vector2); - - bgc_vector2_subtract_scaled_fp64(&vector1, &vector2, 3.0, &result); - - if (!bgc_are_close_fp64(result.x1, 3.6) || !bgc_are_close_fp64(result.x2, 100.6)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_vector2_subtract() { test_vector2_subtract_fp32(); test_vector2_subtract_fp64(); - - test_vector2_subtract_scaled_fp32(); - test_vector2_subtract_scaled_fp64(); } // ================== Multiply ================== // diff --git a/basic-geometry-test/tests/vector2/vector2_arithmetics.h b/basic-geometry-test/tests/vector2/vector2_arithmetics.h index c25b5c8..da99fb1 100644 --- a/basic-geometry-test/tests/vector2/vector2_arithmetics.h +++ b/basic-geometry-test/tests/vector2/vector2_arithmetics.h @@ -17,12 +17,8 @@ void test_vector2_add(); void test_vector2_subtract_fp32(); -void test_vector2_subtract_scaled_fp32(); - void test_vector2_subtract_fp64(); -void test_vector2_subtract_scaled_fp64(); - void test_vector2_subtract(); // ================== Multiply ================== // diff --git a/basic-geometry-test/tests/vector3/vector3_arithmetics.c b/basic-geometry-test/tests/vector3/vector3_arithmetics.c index 50edbfb..c110f6f 100644 --- a/basic-geometry-test/tests/vector3/vector3_arithmetics.c +++ b/basic-geometry-test/tests/vector3/vector3_arithmetics.c @@ -159,35 +159,6 @@ void test_vector3_subtract_fp32() print_testing_success(); } -void test_vector3_subtract_scaled_fp32() -{ - BgcVector3FP32 vector1, vector2, result; - - print_testing_name("bgc_vector3_subtract_scaled_fp32"); - - bgc_vector3_set_values_fp32(10.0f, -20.0f, 1.25f, &vector1); - bgc_vector3_set_values_fp32(4.0f, 5.0f, -0.4f, &vector2); - - bgc_vector3_subtract_scaled_fp32(&vector1, &vector2, 2.0f, &result); - - if (!bgc_are_close_fp32(result.x1, 2.0f) || !bgc_are_close_fp32(result.x2, -30.0f) || !bgc_are_close_fp32(result.x3, 2.05f)) { - print_testing_error("first test failed"); - return; - } - - bgc_vector3_set_values_fp32(0.36f, 100.4f, 10, &vector1); - bgc_vector3_set_values_fp32(1.09f, 0.1f, 2.5f, &vector2); - - bgc_vector3_subtract_scaled_fp32(&vector1, &vector2, 4.0f, &result); - - if (!bgc_are_close_fp32(result.x1, -4.0f) || !bgc_are_close_fp32(result.x2, 100.0f) || !bgc_are_close_fp32(result.x3, 0.0f)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_vector3_subtract_fp64() { BgcVector3FP64 vector1, vector2, result; @@ -217,42 +188,10 @@ void test_vector3_subtract_fp64() print_testing_success(); } -void test_vector3_subtract_scaled_fp64() -{ - BgcVector3FP64 vector1, vector2, result; - - print_testing_name("bgc_vector3_subtract_scaled_fp64"); - - bgc_vector3_set_values_fp64(10.0, 20.0, 0.1, &vector1); - bgc_vector3_set_values_fp64(4.0, 5.0, -4.0, &vector2); - - bgc_vector3_subtract_scaled_fp64(&vector1, &vector2, 2.5, &result); - - if (!bgc_are_close_fp64(result.x1, 0.0) || !bgc_are_close_fp64(result.x2, 7.5) || !bgc_are_close_fp64(result.x3, 10.1)) { - print_testing_error("first test failed"); - return; - } - - bgc_vector3_set_values_fp64(-0.27, 100.3, -0.01, &vector1); - bgc_vector3_set_values_fp64(-1.29, -0.1, 0.33, &vector2); - - bgc_vector3_subtract_scaled_fp64(&vector1, &vector2, 3.0, &result); - - if (!bgc_are_close_fp64(result.x1, 3.6) || !bgc_are_close_fp64(result.x2, 100.6) || !bgc_are_close_fp64(result.x3, -1.0)) { - print_testing_error("second test failed"); - return; - } - - print_testing_success(); -} - void test_vector3_subtract() { test_vector3_subtract_fp32(); test_vector3_subtract_fp64(); - - test_vector3_subtract_scaled_fp32(); - test_vector3_subtract_scaled_fp64(); } // ================== Multiply ================== // diff --git a/basic-geometry-test/tests/versor/versor_copy.c b/basic-geometry-test/tests/versor/versor_copy.c index 07d0094..e424869 100644 --- a/basic-geometry-test/tests/versor/versor_copy.c +++ b/basic-geometry-test/tests/versor/versor_copy.c @@ -28,10 +28,10 @@ void test_versor_copy_fp32() bgc_versor_copy_fp32(&_TEST_FP32_VERSOR_LIST[i], &versor); - if (versor.s0 != _TEST_FP32_VERSOR_LIST[i].s0 || - versor.x1 != _TEST_FP32_VERSOR_LIST[i].x1 || - versor.x2 != _TEST_FP32_VERSOR_LIST[i].x2 || - versor.x3 != _TEST_FP32_VERSOR_LIST[i].x3) { + if (versor._s0 != _TEST_FP32_VERSOR_LIST[i]._s0 || + versor._x1 != _TEST_FP32_VERSOR_LIST[i]._x1 || + versor._x2 != _TEST_FP32_VERSOR_LIST[i]._x2 || + versor._x3 != _TEST_FP32_VERSOR_LIST[i]._x3) { print_testing_failed(); return; } @@ -64,10 +64,10 @@ void test_versor_copy_fp64() bgc_versor_copy_fp64(&_TEST_FP64_VERSOR_LIST[i], &versor); - if (versor.s0 != _TEST_FP64_VERSOR_LIST[i].s0 || - versor.x1 != _TEST_FP64_VERSOR_LIST[i].x1 || - versor.x2 != _TEST_FP64_VERSOR_LIST[i].x2 || - versor.x3 != _TEST_FP64_VERSOR_LIST[i].x3) { + if (versor._s0 != _TEST_FP64_VERSOR_LIST[i]._s0 || + versor._x1 != _TEST_FP64_VERSOR_LIST[i]._x1 || + versor._x2 != _TEST_FP64_VERSOR_LIST[i]._x2 || + versor._x3 != _TEST_FP64_VERSOR_LIST[i]._x3) { print_testing_failed(); return; } diff --git a/basic-geometry-test/tests/versor/versor_reset.c b/basic-geometry-test/tests/versor/versor_reset.c index 484eaee..016f2d9 100644 --- a/basic-geometry-test/tests/versor/versor_reset.c +++ b/basic-geometry-test/tests/versor/versor_reset.c @@ -10,7 +10,7 @@ void test_versor_reset_fp32() bgc_versor_reset_fp32(&versor); - if (versor.s0 != 1.0f || versor.x1 != 0.0f || versor.x2 != 0.0f || versor.x3 != 0.0f) { + if (versor._s0 != 1.0f || versor._x1 != 0.0f || versor._x2 != 0.0f || versor._x3 != 0.0f) { print_testing_failed(); return; } @@ -26,7 +26,7 @@ void test_versor_reset_fp64() bgc_versor_reset_fp64(&versor); - if (versor.s0 != 1.0 || versor.x1 != 0.0 || versor.x2 != 0.0 || versor.x3 != 0.0) { + if (versor._s0 != 1.0 || versor._x1 != 0.0 || versor._x2 != 0.0 || versor._x3 != 0.0) { print_testing_failed(); return; } diff --git a/basic-geometry-test/tests/versor/versor_set_values.c b/basic-geometry-test/tests/versor/versor_set_values.c index f15bafe..0e108b4 100644 --- a/basic-geometry-test/tests/versor/versor_set_values.c +++ b/basic-geometry-test/tests/versor/versor_set_values.c @@ -30,7 +30,7 @@ void test_versor_set_values_fp32() &versor ); - versor_module = sqrtf(versor.s0 * versor.s0 + versor.x1 * versor.x1 + versor.x2 * versor.x2 + versor.x3 * versor.x3); + versor_module = sqrtf(versor._s0 * versor._s0 + versor._x1 * versor._x1 + versor._x2 * versor._x2 + versor._x3 * versor._x3); if (!bgc_is_unit_fp32(versor_module)) { print_testing_error("Versor module is not equal to one."); @@ -41,19 +41,19 @@ void test_versor_set_values_fp32() continue; } - ratio = _TEST_FP32_VERSOR_DATA_LIST[i].s0 / versor.s0; + ratio = _TEST_FP32_VERSOR_DATA_LIST[i].s0 / versor._s0; - if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x1) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x1 / versor.x1)) { + if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x1) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x1 / versor._x1)) { print_testing_error("Versor was not normalized proportionally (x1)."); return; } - if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x2) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x2 / versor.x2)) { + if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x2) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x2 / versor._x2)) { print_testing_error("Versor was not normalized proportionally (x2)."); return; } - if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x3) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x3 / versor.x3)) { + if (!bgc_is_zero_fp32(_TEST_FP32_VERSOR_DATA_LIST[i].x3) && !bgc_are_close_fp32(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x3 / versor._x3)) { print_testing_error("Versor was not normalized proportionally (x3)."); return; } @@ -88,7 +88,7 @@ void test_versor_set_values_fp64() &versor ); - versor_module = sqrt(versor.s0 * versor.s0 + versor.x1 * versor.x1 + versor.x2 * versor.x2 + versor.x3 * versor.x3); + versor_module = sqrt(versor._s0 * versor._s0 + versor._x1 * versor._x1 + versor._x2 * versor._x2 + versor._x3 * versor._x3); if (!bgc_is_unit_fp64(versor_module)) { print_testing_error("Versor module is not equal to one."); @@ -99,19 +99,19 @@ void test_versor_set_values_fp64() continue; } - ratio = _TEST_FP64_VERSOR_DATA_LIST[i].s0 / versor.s0; + ratio = _TEST_FP64_VERSOR_DATA_LIST[i].s0 / versor._s0; - if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x1) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x1 / versor.x1)) { + if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x1) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x1 / versor._x1)) { print_testing_error("Versor was not normalized proportionally (x1)."); return; } - if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x2) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x2 / versor.x2)) { + if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x2) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x2 / versor._x2)) { print_testing_error("Versor was not normalized proportionally (x2)."); return; } - if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x3) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x3 / versor.x3)) { + if (!bgc_is_zero_fp64(_TEST_FP64_VERSOR_DATA_LIST[i].x3) && !bgc_are_close_fp64(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x3 / versor._x3)) { print_testing_error("Versor was not normalized proportionally (x3)."); return; } diff --git a/basic-geometry-test/tests/versor/versor_swap.c b/basic-geometry-test/tests/versor/versor_swap.c index f9c47de..7abb862 100644 --- a/basic-geometry-test/tests/versor/versor_swap.c +++ b/basic-geometry-test/tests/versor/versor_swap.c @@ -48,8 +48,8 @@ void test_versor_swap_fp32() bgc_versor_swap_fp32(&versor1b, &versor2b); - if (versor1a.s0 != versor2b.s0 || versor1a.x1 != versor2b.x1 || versor1a.x2 != versor2b.x2 || versor1a.x3 != versor2b.x3 || - versor2a.s0 != versor1b.s0 || versor2a.x1 != versor1b.x1 || versor2a.x2 != versor1b.x2 || versor2a.x3 != versor1b.x3) { + if (versor1a._s0 != versor2b._s0 || versor1a._x1 != versor2b._x1 || versor1a._x2 != versor2b._x2 || versor1a._x3 != versor2b._x3 || + versor2a._s0 != versor1b._s0 || versor2a._x1 != versor1b._x1 || versor2a._x2 != versor1b._x2 || versor2a._x3 != versor1b._x3) { print_testing_failed(); return; } @@ -88,8 +88,8 @@ void test_versor_swap_fp64() bgc_versor_swap_fp64(&versor1b, &versor2b); - if (versor1a.s0 != versor2b.s0 || versor1a.x1 != versor2b.x1 || versor1a.x2 != versor2b.x2 || versor1a.x3 != versor2b.x3 || - versor2a.s0 != versor1b.s0 || versor2a.x1 != versor1b.x1 || versor2a.x2 != versor1b.x2 || versor2a.x3 != versor1b.x3) { + if (versor1a._s0 != versor2b._s0 || versor1a._x1 != versor2b._x1 || versor1a._x2 != versor2b._x2 || versor1a._x3 != versor2b._x3 || + versor2a._s0 != versor1b._s0 || versor2a._x1 != versor1b._x1 || versor2a._x2 != versor1b._x2 || versor2a._x3 != versor1b._x3) { print_testing_failed(); return; } diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 3614dc7..11eab0f 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -30,6 +30,7 @@ + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 440dcb1..cc4282f 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -69,6 +69,9 @@ Файлы заголовков + + Файлы заголовков + diff --git a/basic-geometry/complex.c b/basic-geometry/complex.c index ef3a0bd..51ef75d 100644 --- a/basic-geometry/complex.c +++ b/basic-geometry/complex.c @@ -27,23 +27,35 @@ extern inline void bgc_complex_swap_fp64(BgcComplexFP64* number1, BgcComplexFP64 extern inline void bgc_complex_convert_fp64_to_fp32(const BgcComplexFP64* source, BgcComplexFP32* destination); extern inline void bgc_complex_convert_fp32_to_fp64(const BgcComplexFP32* source, BgcComplexFP64* destination); -extern inline void bgc_complex_reverse_fp32(const BgcComplexFP32* number, BgcComplexFP32* reverse); -extern inline void bgc_complex_reverse_fp64(const BgcComplexFP64* number, BgcComplexFP64* reverse); +extern inline void bgc_complex_make_opposite_fp32(BgcComplexFP32* number); +extern inline void bgc_complex_make_opposite_fp64(BgcComplexFP64* number); -extern inline int bgc_complex_normalize_fp32(const BgcComplexFP32* number, BgcComplexFP32* normalized); -extern inline int bgc_complex_normalize_fp64(const BgcComplexFP64* number, BgcComplexFP64* normalized); +extern inline void bgc_complex_get_opposite_fp32(const BgcComplexFP32* number, BgcComplexFP32* opposite); +extern inline void bgc_complex_get_opposite_fp64(const BgcComplexFP64* number, BgcComplexFP64* opposite); -extern inline void bgc_complex_conjugate_fp32(const BgcComplexFP32* number, BgcComplexFP32* conjugate); -extern inline void bgc_complex_conjugate_fp64(const BgcComplexFP64* number, BgcComplexFP64* conjugate); +extern inline int bgc_complex_normalize_fp32(BgcComplexFP32* number); +extern inline int bgc_complex_normalize_fp64(BgcComplexFP64* number); -extern inline int bgc_complex_invert_fp32(const BgcComplexFP32* number, BgcComplexFP32* inverted); -extern inline int bgc_complex_invert_fp64(const BgcComplexFP64* number, BgcComplexFP64* inverted); +extern inline int bgc_complex_get_normalized_fp32(const BgcComplexFP32* number, BgcComplexFP32* normalized); +extern inline int bgc_complex_get_normalized_fp64(const BgcComplexFP64* number, BgcComplexFP64* normalized); -extern inline void bgc_complex_get_product_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* result); -extern inline void bgc_complex_get_product_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, BgcComplexFP64* result); +extern inline void bgc_complex_conjugate_fp32(BgcComplexFP32* number); +extern inline void bgc_complex_conjugate_fp64(BgcComplexFP64* number); -extern inline int bgc_complex_get_ratio_fp32(const BgcComplexFP32* divident, const BgcComplexFP32* divisor, BgcComplexFP32* quotient); -extern inline int bgc_complex_get_ratio_fp64(const BgcComplexFP64* divident, const BgcComplexFP64* divisor, BgcComplexFP64* quotient); +extern inline void bgc_complex_get_conjugate_fp32(const BgcComplexFP32* number, BgcComplexFP32* conjugate); +extern inline void bgc_complex_get_conjugate_fp64(const BgcComplexFP64* number, BgcComplexFP64* conjugate); + +extern inline int bgc_complex_invert_fp32(BgcComplexFP32* number); +extern inline int bgc_complex_invert_fp64(BgcComplexFP64* number); + +extern inline int bgc_complex_get_inverse_fp32(const BgcComplexFP32* number, BgcComplexFP32* inverse); +extern inline int bgc_complex_get_inverse_fp64(const BgcComplexFP64* number, BgcComplexFP64* inverse); + +extern inline void bgc_complex_multiply_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* result); +extern inline void bgc_complex_multiply_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, BgcComplexFP64* result); + +extern inline int bgc_complex_devide_fp32(const BgcComplexFP32* divident, const BgcComplexFP32* divisor, BgcComplexFP32* quotient); +extern inline int bgc_complex_devide_fp64(const BgcComplexFP64* divident, const BgcComplexFP64* divisor, BgcComplexFP64* quotient); extern inline void bgc_complex_add_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* sum); extern inline void bgc_complex_add_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, BgcComplexFP64* sum); @@ -54,13 +66,10 @@ extern inline void bgc_complex_add_scaled_fp64(const BgcComplexFP64* basic_numbe extern inline void bgc_complex_subtract_fp32(const BgcComplexFP32* minuend, const BgcComplexFP32* subtrahend, BgcComplexFP32* difference); extern inline void bgc_complex_subtract_fp64(const BgcComplexFP64* minuend, const BgcComplexFP64* subtrahend, BgcComplexFP64* difference); -extern inline void bgc_complex_subtract_scaled_fp32(const BgcComplexFP32* basic_number, const BgcComplexFP32* scalable_number, const float scale, BgcComplexFP32* difference); -extern inline void bgc_complex_subtract_scaled_fp64(const BgcComplexFP64* basic_number, const BgcComplexFP64* scalable_number, const double scale, BgcComplexFP64* difference); - -extern inline void bgc_complex_multiply_fp32(const BgcComplexFP32* multiplicand, const float multiplier, BgcComplexFP32* product); +extern inline void bgc_complex_multiply_by_number_fp32(const BgcComplexFP32* multiplicand, const float multiplier, BgcComplexFP32* product); extern inline void bgc_complex_multiply_fp64(const BgcComplexFP64* multiplicand, const double multiplier, BgcComplexFP64* product); -extern inline void bgc_complex_divide_fp32(const BgcComplexFP32* dividend, const float divisor, BgcComplexFP32* quotient); +extern inline void bgc_complex_divide_by_number_fp32(const BgcComplexFP32* dividend, const float divisor, BgcComplexFP32* quotient); extern inline void bgc_complex_divide_fp64(const BgcComplexFP64* dividend, const double divisor, BgcComplexFP64* quotient); extern inline void bgc_complex_get_mean_of_two_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* mean); @@ -69,14 +78,8 @@ extern inline void bgc_complex_get_mean_of_two_fp64(const BgcComplexFP64* number extern inline void bgc_complex_get_mean_of_three_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const BgcComplexFP32* number3, BgcComplexFP32* mean); extern inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const BgcComplexFP64* number3, BgcComplexFP64* mean); -extern inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation); -extern inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation); - -extern inline void bgc_complex_minimize_fp32(const BgcComplexFP32* number, BgcComplexFP32* minimal); -extern inline void bgc_complex_minimize_fp64(const BgcComplexFP64* number, BgcComplexFP64* minimal); - -extern inline void bgc_complex_maximize_fp32(const BgcComplexFP32* number, BgcComplexFP32* maximal); -extern inline void bgc_complex_maximize_fp64(const BgcComplexFP64* number, BgcComplexFP64* maximal); +extern inline void bgc_complex_interpolate_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation); +extern inline void bgc_complex_interpolate_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation); extern inline int bgc_complex_are_close_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2); extern inline int bgc_complex_are_close_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2); diff --git a/basic-geometry/complex.h b/basic-geometry/complex.h index e551609..3d8b8c3 100644 --- a/basic-geometry/complex.h +++ b/basic-geometry/complex.h @@ -142,23 +142,75 @@ inline void bgc_complex_convert_fp32_to_fp64(const BgcComplexFP32* source, BgcCo destination->imaginary = source->imaginary; } -// ================== Reverse =================== // +// ================== Negative ================== // -inline void bgc_complex_reverse_fp32(const BgcComplexFP32* number, BgcComplexFP32* reverse) +inline void bgc_complex_make_opposite_fp32(BgcComplexFP32* number) { - reverse->real = -number->real; - reverse->imaginary = -number->imaginary; + number->real = -number->real; + number->imaginary = -number->imaginary; } -inline void bgc_complex_reverse_fp64(const BgcComplexFP64* number, BgcComplexFP64* reverse) +inline void bgc_complex_make_opposite_fp64(BgcComplexFP64* number) { - reverse->real = -number->real; - reverse->imaginary = -number->imaginary; + number->real = -number->real; + number->imaginary = -number->imaginary; +} + +inline void bgc_complex_get_opposite_fp32(const BgcComplexFP32* number, BgcComplexFP32* opposite) +{ + opposite->real = -number->real; + opposite->imaginary = -number->imaginary; +} + +inline void bgc_complex_get_opposite_fp64(const BgcComplexFP64* number, BgcComplexFP64* opposite) +{ + opposite->real = -number->real; + opposite->imaginary = -number->imaginary; } // ================= Normalize ================== // -inline int bgc_complex_normalize_fp32(const BgcComplexFP32* number, BgcComplexFP32* normalized) +inline int bgc_complex_normalize_fp32(BgcComplexFP32* number) +{ + const float square_modulus = bgc_complex_get_square_modulus_fp32(number); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; + } + + const float multiplicand = sqrtf(1.0f / square_modulus); + + number->real *= multiplicand; + number->imaginary *= multiplicand; + + return 1; +} + +inline int bgc_complex_normalize_fp64(BgcComplexFP64* number) +{ + const double square_modulus = bgc_complex_get_square_modulus_fp64(number); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double multiplicand = sqrt(1.0 / square_modulus); + + number->real *= multiplicand; + number->imaginary *= multiplicand; + + return 1; +} + +inline int bgc_complex_get_normalized_fp32(const BgcComplexFP32* number, BgcComplexFP32* normalized) { const float square_modulus = bgc_complex_get_square_modulus_fp32(number); @@ -169,6 +221,8 @@ inline int bgc_complex_normalize_fp32(const BgcComplexFP32* number, BgcComplexFP } if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + normalized->real = 0.0f; + normalized->imaginary = 0.0f; return 0; } @@ -180,7 +234,7 @@ inline int bgc_complex_normalize_fp32(const BgcComplexFP32* number, BgcComplexFP return 1; } -inline int bgc_complex_normalize_fp64(const BgcComplexFP64* number, BgcComplexFP64* normalized) +inline int bgc_complex_get_normalized_fp64(const BgcComplexFP64* number, BgcComplexFP64* normalized) { const double square_modulus = bgc_complex_get_square_modulus_fp64(number); @@ -191,6 +245,8 @@ inline int bgc_complex_normalize_fp64(const BgcComplexFP64* number, BgcComplexFP } if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + normalized->real = 0.0; + normalized->imaginary = 0.0; return 0; } @@ -204,13 +260,23 @@ inline int bgc_complex_normalize_fp64(const BgcComplexFP64* number, BgcComplexFP // ================= Conjugate ================== // -inline void bgc_complex_conjugate_fp32(const BgcComplexFP32* number, BgcComplexFP32* conjugate) +inline void bgc_complex_conjugate_fp32(BgcComplexFP32* number) +{ + number->imaginary = -number->imaginary; +} + +inline void bgc_complex_conjugate_fp64(BgcComplexFP64* number) +{ + number->imaginary = -number->imaginary; +} + +inline void bgc_complex_get_conjugate_fp32(const BgcComplexFP32* number, BgcComplexFP32* conjugate) { conjugate->real = number->real; conjugate->imaginary = -number->imaginary; } -inline void bgc_complex_conjugate_fp64(const BgcComplexFP64* number, BgcComplexFP64* conjugate) +inline void bgc_complex_get_conjugate_fp64(const BgcComplexFP64* number, BgcComplexFP64* conjugate) { conjugate->real = number->real; conjugate->imaginary = -number->imaginary; @@ -218,7 +284,7 @@ inline void bgc_complex_conjugate_fp64(const BgcComplexFP64* number, BgcComplexF // =================== Invert =================== // -inline int bgc_complex_invert_fp32(const BgcComplexFP32* number, BgcComplexFP32* inverted) +inline int bgc_complex_get_inverse_fp32(const BgcComplexFP32* number, BgcComplexFP32* inverse) { const float square_modulus = bgc_complex_get_square_modulus_fp32(number); @@ -228,13 +294,13 @@ inline int bgc_complex_invert_fp32(const BgcComplexFP32* number, BgcComplexFP32* const float multiplicand = 1.0f / square_modulus; - inverted->real = number->real * multiplicand; - inverted->imaginary = -number->imaginary * multiplicand; + inverse->real = number->real * multiplicand; + inverse->imaginary = -number->imaginary * multiplicand; return 1; } -inline int bgc_complex_invert_fp64(const BgcComplexFP64* number, BgcComplexFP64* inverted) +inline int bgc_complex_get_inverse_fp64(const BgcComplexFP64* number, BgcComplexFP64* inverse) { const double square_modulus = bgc_complex_get_square_modulus_fp64(number); @@ -244,70 +310,20 @@ inline int bgc_complex_invert_fp64(const BgcComplexFP64* number, BgcComplexFP64* const double multiplicand = 1.0 / square_modulus; - inverted->real = number->real * multiplicand; - inverted->imaginary = -number->imaginary * multiplicand; + inverse->real = number->real * multiplicand; + inverse->imaginary = -number->imaginary * multiplicand; return 1; } -// ================ Get Product ================= // - -inline void bgc_complex_get_product_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* result) +inline int bgc_complex_invert_fp32(BgcComplexFP32* number) { - const float real = number1->real * number2->real - number1->imaginary * number2->imaginary; - const float imaginary = number1->real * number2->imaginary + number1->imaginary * number2->real; - - result->real = real; - result->imaginary = imaginary; + return bgc_complex_get_inverse_fp32(number, number); } -inline void bgc_complex_get_product_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, BgcComplexFP64* result) +inline int bgc_complex_invert_fp64(BgcComplexFP64* number) { - const double real = number1->real * number2->real - number1->imaginary * number2->imaginary; - const double imaginary = number1->real * number2->imaginary + number1->imaginary * number2->real; - - result->real = real; - result->imaginary = imaginary; -} - -// ================= Get Ratio ================== // - -inline int bgc_complex_get_ratio_fp32(const BgcComplexFP32* divident, const BgcComplexFP32* divisor, BgcComplexFP32* quotient) -{ - const float square_modulus = bgc_complex_get_square_modulus_fp32(divisor); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) { - return 0; - } - - const float real = divident->real * divisor->real + divident->imaginary * divisor->imaginary; - const float imaginary = divident->imaginary * divisor->real - divident->real * divisor->imaginary; - - const float multiplier = 1.0f / square_modulus; - - quotient->real = real * multiplier; - quotient->imaginary = imaginary * multiplier; - - return 1; -} - -inline int bgc_complex_get_ratio_fp64(const BgcComplexFP64* divident, const BgcComplexFP64* divisor, BgcComplexFP64* quotient) -{ - const double square_modulus = bgc_complex_get_square_modulus_fp64(divisor); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) { - return 0; - } - - const double real = divident->real * divisor->real + divident->imaginary * divisor->imaginary; - const double imaginary = divident->imaginary * divisor->real - divident->real * divisor->imaginary; - - const double multiplier = 1.0 / square_modulus; - - quotient->real = real * multiplier; - quotient->imaginary = imaginary * multiplier; - - return 1; + return bgc_complex_get_inverse_fp64(number, number); } // =============== Get Exponation =============== // @@ -358,29 +374,35 @@ inline void bgc_complex_subtract_fp64(const BgcComplexFP64* minuend, const BgcCo difference->imaginary = minuend->imaginary - subtrahend->imaginary; } -// ============== Subtract scaled =============== // - -inline void bgc_complex_subtract_scaled_fp32(const BgcComplexFP32* basic_number, const BgcComplexFP32* scalable_number, const float scale, BgcComplexFP32* difference) -{ - difference->real = basic_number->real - scalable_number->real * scale; - difference->imaginary = basic_number->imaginary - scalable_number->imaginary * scale; -} - -inline void bgc_complex_subtract_scaled_fp64(const BgcComplexFP64* basic_number, const BgcComplexFP64* scalable_number, const double scale, BgcComplexFP64* difference) -{ - difference->real = basic_number->real - scalable_number->real * scale; - difference->imaginary = basic_number->imaginary - scalable_number->imaginary * scale; -} - // ================== Multiply ================== // -inline void bgc_complex_multiply_fp32(const BgcComplexFP32* multiplicand, const float multiplier, BgcComplexFP32* product) +inline void bgc_complex_multiply_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, BgcComplexFP32* product) +{ + const float real = number1->real * number2->real - number1->imaginary * number2->imaginary; + const float imaginary = number1->real * number2->imaginary + number1->imaginary * number2->real; + + product->real = real; + product->imaginary = imaginary; +} + +inline void bgc_complex_multiply_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, BgcComplexFP64* product) +{ + const double real = number1->real * number2->real - number1->imaginary * number2->imaginary; + const double imaginary = number1->real * number2->imaginary + number1->imaginary * number2->real; + + product->real = real; + product->imaginary = imaginary; +} + +// ============= Multiply By Number ============= // + +inline void bgc_complex_multiply_by_number_fp32(const BgcComplexFP32* multiplicand, const float multiplier, BgcComplexFP32* product) { product->real = multiplicand->real * multiplier; product->imaginary = multiplicand->imaginary * multiplier; } -inline void bgc_complex_multiply_fp64(const BgcComplexFP64* multiplicand, const double multiplier, BgcComplexFP64* product) +inline void bgc_complex_multiply_by_number_fp64(const BgcComplexFP64* multiplicand, const double multiplier, BgcComplexFP64* product) { product->real = multiplicand->real * multiplier; product->imaginary = multiplicand->imaginary * multiplier; @@ -388,14 +410,54 @@ inline void bgc_complex_multiply_fp64(const BgcComplexFP64* multiplicand, const // =================== Divide =================== // -inline void bgc_complex_divide_fp32(const BgcComplexFP32* dividend, const float divisor, BgcComplexFP32* quotient) +inline int bgc_complex_devide_fp32(const BgcComplexFP32* divident, const BgcComplexFP32* divisor, BgcComplexFP32* quotient) { - bgc_complex_multiply_fp32(dividend, 1.0f / divisor, quotient); + const float square_modulus = bgc_complex_get_square_modulus_fp32(divisor); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) { + return 0; + } + + const float real = divident->real * divisor->real + divident->imaginary * divisor->imaginary; + const float imaginary = divident->imaginary * divisor->real - divident->real * divisor->imaginary; + + const float multiplier = 1.0f / square_modulus; + + quotient->real = real * multiplier; + quotient->imaginary = imaginary * multiplier; + + return 1; } -inline void bgc_complex_divide_fp64(const BgcComplexFP64* dividend, const double divisor, BgcComplexFP64* quotient) +inline int bgc_complex_devide_fp64(const BgcComplexFP64* divident, const BgcComplexFP64* divisor, BgcComplexFP64* quotient) { - bgc_complex_multiply_fp64(dividend, 1.0 / divisor, quotient); + const double square_modulus = bgc_complex_get_square_modulus_fp64(divisor); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) { + return 0; + } + + const double real = divident->real * divisor->real + divident->imaginary * divisor->imaginary; + const double imaginary = divident->imaginary * divisor->real - divident->real * divisor->imaginary; + + const double multiplier = 1.0 / square_modulus; + + quotient->real = real * multiplier; + quotient->imaginary = imaginary * multiplier; + + return 1; +} + +// ============== Divide By Number ============== // + +inline void bgc_complex_divide_by_number_fp32(const BgcComplexFP32* dividend, const float divisor, BgcComplexFP32* quotient) +{ + bgc_complex_multiply_by_number_fp32(dividend, 1.0f / divisor, quotient); +} + +inline void bgc_complex_divide_by_number_fp64(const BgcComplexFP64* dividend, const double divisor, BgcComplexFP64* quotient) +{ + bgc_complex_multiply_by_number_fp64(dividend, 1.0 / divisor, quotient); } // ================== Average2 ================== // @@ -428,7 +490,7 @@ inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, co // =================== Linear =================== // -inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation) +inline void bgc_complex_interpolate_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation) { const float counterphase = 1.0f - phase; @@ -436,7 +498,7 @@ inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, interpolation->imaginary = number1->imaginary * counterphase + number2->imaginary * phase; } -inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation) +inline void bgc_complex_interpolate_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation) { const double counterphase = 1.0 - phase; @@ -444,54 +506,6 @@ inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, interpolation->imaginary = number1->imaginary * counterphase + number2->imaginary * phase; } -// ================== Minimal =================== // - -inline void bgc_complex_minimize_fp32(const BgcComplexFP32* number, BgcComplexFP32* minimal) -{ - if (number->real < minimal->real) { - minimal->real = number->real; - } - - if (number->imaginary < minimal->imaginary) { - minimal->imaginary = number->imaginary; - } -} - -inline void bgc_complex_minimize_fp64(const BgcComplexFP64* number, BgcComplexFP64* minimal) -{ - if (number->real < minimal->real) { - minimal->real = number->real; - } - - if (number->imaginary < minimal->imaginary) { - minimal->imaginary = number->imaginary; - } -} - -// ================== Maximal =================== // - -inline void bgc_complex_maximize_fp32(const BgcComplexFP32* number, BgcComplexFP32* maximal) -{ - if (number->real > maximal->real) { - maximal->real = number->real; - } - - if (number->imaginary > maximal->imaginary) { - maximal->imaginary = number->imaginary; - } -} - -inline void bgc_complex_maximize_fp64(const BgcComplexFP64* number, BgcComplexFP64* maximal) -{ - if (number->real > maximal->real) { - maximal->real = number->real; - } - - if (number->imaginary > maximal->imaginary) { - maximal->imaginary = number->imaginary; - } -} - // ================== Are Close ================= // inline int bgc_complex_are_close_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2) @@ -525,7 +539,7 @@ inline int bgc_complex_are_close_fp64(const BgcComplexFP64* number1, const BgcCo return square_distance <= BGC_SQUARE_EPSYLON_FP64; } - return square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus2; + return square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus2; } #endif diff --git a/basic-geometry/cotes-number.c b/basic-geometry/cotes-number.c index f00206b..38484e9 100644 --- a/basic-geometry/cotes-number.c +++ b/basic-geometry/cotes-number.c @@ -25,8 +25,17 @@ extern inline void bgc_cotes_number_swap_fp64(BgcCotesNumberFP64* number1, BgcCo extern inline void bgc_cotes_number_convert_fp64_to_fp32(const BgcCotesNumberFP64* source, BgcCotesNumberFP32* destination); extern inline void bgc_cotes_number_convert_fp32_to_fp64(const BgcCotesNumberFP32* source, BgcCotesNumberFP64* destination); -extern inline void bgc_cotes_number_invert_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* inverted); -extern inline void bgc_cotes_number_invert_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* inverted); +extern inline void bgc_cotes_number_make_opposite_fp32(BgcCotesNumberFP32* number); +extern inline void bgc_cotes_number_make_opposite_fp64(BgcCotesNumberFP64* number); + +extern inline void bgc_cotes_number_get_opposite_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* opposite); +extern inline void bgc_cotes_number_get_opposite_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* opposite); + +extern inline void bgc_cotes_number_invert_fp32(BgcCotesNumberFP32* number); +extern inline void bgc_cotes_number_invert_fp64(BgcCotesNumberFP64* number); + +extern inline void bgc_cotes_number_get_inverse_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* inverse); +extern inline void bgc_cotes_number_get_inverse_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* inverse); extern inline void bgc_cotes_number_get_exponation_fp32(const BgcCotesNumberFP32* base, const float exponent, BgcCotesNumberFP32* power); extern inline void bgc_cotes_number_get_exponation_fp64(const BgcCotesNumberFP64* base, const double exponent, BgcCotesNumberFP64* power); @@ -52,34 +61,34 @@ extern inline void bgc_cotes_number_turn_vector_back_fp64(const BgcCotesNumberFP extern inline int bgc_cotes_number_are_close_fp32(const BgcCotesNumberFP32* number1, const BgcCotesNumberFP32* number2); extern inline int bgc_cotes_number_are_close_fp64(const BgcCotesNumberFP64* number1, const BgcCotesNumberFP64* number2); -void _bgc_cotes_number_normalize_fp32(const float square_modulus, _BgcTwinCotesNumberFP32* twin) +void _bgc_cotes_number_normalize_fp32(const float square_modulus, BgcCotesNumberFP32* number) { // (square_modulus != square_modulus) is true when square_modulus is NaN if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - twin->cos = 1.0f; - twin->sin = 0.0f; + number->_cos = 1.0f; + number->_sin = 0.0f; return; } const float multiplier = sqrtf(1.0f / square_modulus); - twin->cos *= multiplier; - twin->sin *= multiplier; + number->_cos *= multiplier; + number->_sin *= multiplier; } -void _bgc_cotes_number_normalize_fp64(const double square_modulus, _BgcTwinCotesNumberFP64* twin) +void _bgc_cotes_number_normalize_fp64(const double square_modulus, BgcCotesNumberFP64* number) { // (square_modulus != square_modulus) is true when square_modulus is NaN if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - twin->cos = 1.0; - twin->sin = 0.0; + number->_cos = 1.0; + number->_sin = 0.0; return; } const double multiplier = sqrt(1.0 / square_modulus); - twin->cos *= multiplier; - twin->sin *= multiplier; + number->_cos *= multiplier; + number->_sin *= multiplier; } diff --git a/basic-geometry/cotes-number.h b/basic-geometry/cotes-number.h index 1990948..a55ac7d 100644 --- a/basic-geometry/cotes-number.h +++ b/basic-geometry/cotes-number.h @@ -12,24 +12,14 @@ typedef struct { - const float cos, sin; + float _cos, _sin; } BgcCotesNumberFP32; typedef struct { - const double cos, sin; + double _cos, _sin; } BgcCotesNumberFP64; -// ================= Dark Twins ================= // - -typedef struct { - float cos, sin; -} _BgcTwinCotesNumberFP32; - -typedef struct { - double cos, sin; -} _BgcTwinCotesNumberFP64; - // ================= Constants ================== // extern const BgcCotesNumberFP32 BGC_IDLE_COTES_NUMBER_FP32; @@ -39,37 +29,31 @@ extern const BgcCotesNumberFP64 BGC_IDLE_COTES_NUMBER_FP64; inline void bgc_cotes_number_reset_fp32(BgcCotesNumberFP32* number) { - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)number; - - twin->cos = 1.0f; - twin->sin = 0.0f; + number->_cos = 1.0f; + number->_sin = 0.0f; } inline void bgc_cotes_number_reset_fp64(BgcCotesNumberFP64* number) { - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)number; - - twin->cos = 1.0; - twin->sin = 0.0; + number->_cos = 1.0; + number->_sin = 0.0; } // ==================== Set ===================== // -void _bgc_cotes_number_normalize_fp32(const float square_modulus, _BgcTwinCotesNumberFP32* twin); +void _bgc_cotes_number_normalize_fp32(const float square_modulus, BgcCotesNumberFP32* twin); -void _bgc_cotes_number_normalize_fp64(const double square_modulus, _BgcTwinCotesNumberFP64* twin); +void _bgc_cotes_number_normalize_fp64(const double square_modulus, BgcCotesNumberFP64* twin); inline void bgc_cotes_number_set_values_fp32(const float x1, const float x2, BgcCotesNumberFP32* number) { const float square_modulus = x1 * x1 + x2 * x2; - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)number; - - twin->cos = x1; - twin->sin = x2; + number->_cos = x1; + number->_sin = x2; if (!bgc_is_sqare_unit_fp32(square_modulus)) { - _bgc_cotes_number_normalize_fp32(square_modulus, twin); + _bgc_cotes_number_normalize_fp32(square_modulus, number); } } @@ -77,13 +61,11 @@ inline void bgc_cotes_number_set_values_fp64(const double x1, const double x2, B { const double square_modulus = x1 * x1 + x2 * x2; - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)number; - - twin->cos = x1; - twin->sin = x2; + number->_cos = x1; + number->_sin = x2; if (!bgc_is_sqare_unit_fp64(square_modulus)) { - _bgc_cotes_number_normalize_fp64(square_modulus, twin); + _bgc_cotes_number_normalize_fp64(square_modulus, number); } } @@ -93,168 +75,148 @@ inline void bgc_cotes_number_set_turn_fp32(const float angle, const BgcAngleUnit { const float radians = bgc_angle_to_radians_fp32(angle, unit); - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)number; - - twin->cos = cosf(radians); - twin->sin = sinf(radians); + number->_cos = cosf(radians); + number->_sin = sinf(radians); } inline void bgc_cotes_number_set_turn_fp64(const double angle, const BgcAngleUnitEnum unit, BgcCotesNumberFP64* number) { const double radians = bgc_angle_to_radians_fp64(angle, unit); - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)number; - - twin->cos = cos(radians); - twin->sin = sin(radians); + number->_cos = cos(radians); + number->_sin = sin(radians); } // =================== Angle =================== // inline float bgc_cotes_number_get_angle_fp32(const BgcCotesNumberFP32* number, const BgcAngleUnitEnum unit) { - if (number->cos >= 1.0f - BGC_EPSYLON_FP32) { - return 0.0f; - } - - if (number->cos <= -1.0f + BGC_EPSYLON_FP32) { - return bgc_angle_get_half_circle_fp32(unit); - } - - if (number->sin >= 1.0f - BGC_EPSYLON_FP32) { - return bgc_angle_get_quater_circle_fp32(unit); - } - - if (number->sin <= -1.0f + BGC_EPSYLON_FP32) { - return 0.75f * bgc_angle_get_full_circle_fp32(unit); - } - - return bgc_radians_to_units_fp32(atan2f(number->sin, number->cos), unit); + return bgc_radians_to_units_fp32(atan2f(number->_sin, number->_cos), unit); } inline double bgc_cotes_number_get_angle_fp64(const BgcCotesNumberFP64* number, const BgcAngleUnitEnum unit) { - if (number->cos >= 1.0 - BGC_EPSYLON_FP64) { - return 0.0; - } - - if (number->cos <= -1.0 + BGC_EPSYLON_FP64) { - return bgc_angle_get_half_circle_fp64(unit); - } - - if (number->sin >= 1.0 - BGC_EPSYLON_FP64) { - return bgc_angle_get_quater_circle_fp64(unit); - } - - if (number->sin <= -1.0 + BGC_EPSYLON_FP64) { - return 0.75 * bgc_angle_get_full_circle_fp64(unit); - } - - return bgc_radians_to_units_fp64(atan2(number->sin, number->cos), unit); + return bgc_radians_to_units_fp64(atan2(number->_sin, number->_cos), unit); } // ==================== Copy ==================== // inline void bgc_cotes_number_copy_fp32(const BgcCotesNumberFP32* source, BgcCotesNumberFP32* destination) { - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)destination; - - twin->cos = source->cos; - twin->sin = source->sin; + destination->_cos = source->_cos; + destination->_sin = source->_sin; } inline void bgc_cotes_number_copy_fp64(const BgcCotesNumberFP64* source, BgcCotesNumberFP64* destination) { - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)destination; - - twin->cos = source->cos; - twin->sin = source->sin; + destination->_cos = source->_cos; + destination->_sin = source->_sin; } // ==================== Swap ==================== // inline void bgc_cotes_number_swap_fp32(BgcCotesNumberFP32* number1, BgcCotesNumberFP32* number2) { - const float cos = number1->cos; - const float sin = number1->sin; + const float cos = number1->_cos; + const float sin = number1->_sin; - _BgcTwinCotesNumberFP32* twin1 = (_BgcTwinCotesNumberFP32*)number1; + number1->_cos = number2->_cos; + number1->_sin = number2->_sin; - twin1->cos = number2->cos; - twin1->sin = number2->sin; - - _BgcTwinCotesNumberFP32* twin2 = (_BgcTwinCotesNumberFP32*)number2; - - twin2->cos = cos; - twin2->sin = sin; + number2->_cos = cos; + number2->_sin = sin; } inline void bgc_cotes_number_swap_fp64(BgcCotesNumberFP64* number1, BgcCotesNumberFP64* number2) { - const double cos = number1->cos; - const double sin = number1->sin; + const double cos = number1->_cos; + const double sin = number1->_sin; - _BgcTwinCotesNumberFP64* twin1 = (_BgcTwinCotesNumberFP64*)number1; + number1->_cos = number2->_cos; + number1->_sin = number2->_sin; - twin1->cos = number2->cos; - twin1->sin = number2->sin; - - _BgcTwinCotesNumberFP64* twin2 = (_BgcTwinCotesNumberFP64*)number2; - - twin2->cos = cos; - twin2->sin = sin; + number2->_cos = cos; + number2->_sin = sin; } // ================== Convert =================== // inline void bgc_cotes_number_convert_fp64_to_fp32(const BgcCotesNumberFP64* source, BgcCotesNumberFP32* destination) { - bgc_cotes_number_set_values_fp32((float)source->cos, (float)source->sin, destination); + bgc_cotes_number_set_values_fp32((float)source->_cos, (float)source->_sin, destination); } inline void bgc_cotes_number_convert_fp32_to_fp64(const BgcCotesNumberFP32* source, BgcCotesNumberFP64* destination) { - bgc_cotes_number_set_values_fp64((double)source->cos, (double)source->sin, destination); + bgc_cotes_number_set_values_fp64((double)source->_cos, (double)source->_sin, destination); +} + +// ================== Negative ================== // + +inline void bgc_cotes_number_make_opposite_fp32(BgcCotesNumberFP32* number) +{ + number->_cos = -number->_cos; + number->_sin = -number->_sin; +} + +inline void bgc_cotes_number_make_opposite_fp64(BgcCotesNumberFP64* number) +{ + number->_cos = -number->_cos; + number->_sin = -number->_sin; +} + +inline void bgc_cotes_number_get_opposite_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* opposite) +{ + opposite->_cos = -number->_cos; + opposite->_sin = -number->_sin; +} + +inline void bgc_cotes_number_get_opposite_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* opposite) +{ + opposite->_cos = -number->_cos; + opposite->_sin = -number->_sin; } // =================== Invert =================== // -inline void bgc_cotes_number_invert_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* inverted) +inline void bgc_cotes_number_invert_fp32(BgcCotesNumberFP32* number) { - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)inverted; - - twin->cos = number->cos; - twin->sin = -number->sin; + number->_sin = -number->_sin; } -inline void bgc_cotes_number_invert_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* inverted) +inline void bgc_cotes_number_invert_fp64(BgcCotesNumberFP64* number) { - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)inverted; + number->_sin = -number->_sin; +} - twin->cos = number->cos; - twin->sin = -number->sin; +inline void bgc_cotes_number_get_inverse_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* inverse) +{ + inverse->_cos = number->_cos; + inverse->_sin = -number->_sin; +} + +inline void bgc_cotes_number_get_inverse_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* inverse) +{ + inverse->_cos = number->_cos; + inverse->_sin = -number->_sin; } // ================= Exponation ================= // inline void bgc_cotes_number_get_exponation_fp32(const BgcCotesNumberFP32* base, const float exponent, BgcCotesNumberFP32* power) { - const float power_angle = exponent * atan2f(base->sin, base->cos); + const float power_angle = exponent * atan2f(base->_sin, base->_cos); - _BgcTwinCotesNumberFP32* twin = (_BgcTwinCotesNumberFP32*)power; - - twin->cos = cosf(power_angle); - twin->sin = sinf(power_angle); + power->_cos = cosf(power_angle); + power->_sin = sinf(power_angle); } inline void bgc_cotes_number_get_exponation_fp64(const BgcCotesNumberFP64* base, const double exponent, BgcCotesNumberFP64* power) { - const double power_angle = exponent * atan2(base->sin, base->cos); + const double power_angle = exponent * atan2(base->_sin, base->_cos); - _BgcTwinCotesNumberFP64* twin = (_BgcTwinCotesNumberFP64*)power; - - twin->cos = cos(power_angle); - twin->sin = sin(power_angle); + power->_cos = cos(power_angle); + power->_sin = sin(power_angle); } // ================ Combination ================= // @@ -262,8 +224,8 @@ inline void bgc_cotes_number_get_exponation_fp64(const BgcCotesNumberFP64* base, inline void bgc_cotes_number_combine_fp32(const BgcCotesNumberFP32* number1, const BgcCotesNumberFP32* number2, BgcCotesNumberFP32* result) { bgc_cotes_number_set_values_fp32( - number1->cos * number2->cos - number1->sin * number2->sin, - number1->cos * number2->sin + number1->sin * number2->cos, + number1->_cos * number2->_cos - number1->_sin * number2->_sin, + number1->_cos * number2->_sin + number1->_sin * number2->_cos, result ); } @@ -271,8 +233,8 @@ inline void bgc_cotes_number_combine_fp32(const BgcCotesNumberFP32* number1, con inline void bgc_cotes_number_combine_fp64(const BgcCotesNumberFP64* number1, const BgcCotesNumberFP64* number2, BgcCotesNumberFP64* result) { bgc_cotes_number_set_values_fp64( - number1->cos * number2->cos - number1->sin * number2->sin, - number1->cos * number2->sin + number1->sin * number2->cos, + number1->_cos * number2->_cos - number1->_sin * number2->_sin, + number1->_cos * number2->_sin + number1->_sin * number2->_cos, result ); } @@ -282,8 +244,8 @@ inline void bgc_cotes_number_combine_fp64(const BgcCotesNumberFP64* number1, con inline void bgc_cotes_number_exclude_fp32(const BgcCotesNumberFP32* base, const BgcCotesNumberFP32* excludant, BgcCotesNumberFP32* difference) { bgc_cotes_number_set_values_fp32( - base->cos * excludant->cos + base->sin * excludant->sin, - base->sin * excludant->cos - base->cos * excludant->sin, + base->_cos * excludant->_cos + base->_sin * excludant->_sin, + base->_sin * excludant->_cos - base->_cos * excludant->_sin, difference ); } @@ -291,8 +253,8 @@ inline void bgc_cotes_number_exclude_fp32(const BgcCotesNumberFP32* base, const inline void bgc_cotes_number_exclude_fp64(const BgcCotesNumberFP64* base, const BgcCotesNumberFP64* excludant, BgcCotesNumberFP64* difference) { bgc_cotes_number_set_values_fp64( - base->cos * excludant->cos + base->sin * excludant->sin, - base->sin * excludant->cos - base->cos * excludant->sin, + base->_cos * excludant->_cos + base->_sin * excludant->_sin, + base->_sin * excludant->_cos - base->_cos * excludant->_sin, difference ); } @@ -301,44 +263,44 @@ inline void bgc_cotes_number_exclude_fp64(const BgcCotesNumberFP64* base, const inline void bgc_cotes_number_get_rotation_matrix_fp32(const BgcCotesNumberFP32* number, BgcMatrix2x2FP32* matrix) { - matrix->r1c1 = number->cos; - matrix->r1c2 = -number->sin; - matrix->r2c1 = number->sin; - matrix->r2c2 = number->cos; + matrix->r1c1 = number->_cos; + matrix->r1c2 = -number->_sin; + matrix->r2c1 = number->_sin; + matrix->r2c2 = number->_cos; } inline void bgc_cotes_number_get_rotation_matrix_fp64(const BgcCotesNumberFP64* number, BgcMatrix2x2FP64* matrix) { - matrix->r1c1 = number->cos; - matrix->r1c2 = -number->sin; - matrix->r2c1 = number->sin; - matrix->r2c2 = number->cos; + matrix->r1c1 = number->_cos; + matrix->r1c2 = -number->_sin; + matrix->r2c1 = number->_sin; + matrix->r2c2 = number->_cos; } // ============== Reverse Matrix ================ // inline void bgc_cotes_number_get_reverse_matrix_fp32(const BgcCotesNumberFP32* number, BgcMatrix2x2FP32* matrix) { - matrix->r1c1 = number->cos; - matrix->r1c2 = number->sin; - matrix->r2c1 = -number->sin; - matrix->r2c2 = number->cos; + matrix->r1c1 = number->_cos; + matrix->r1c2 = number->_sin; + matrix->r2c1 = -number->_sin; + matrix->r2c2 = number->_cos; } inline void bgc_cotes_number_get_reverse_matrix_fp64(const BgcCotesNumberFP64* number, BgcMatrix2x2FP64* matrix) { - matrix->r1c1 = number->cos; - matrix->r1c2 = number->sin; - matrix->r2c1 = -number->sin; - matrix->r2c2 = number->cos; + matrix->r1c1 = number->_cos; + matrix->r1c2 = number->_sin; + matrix->r2c1 = -number->_sin; + matrix->r2c2 = number->_cos; } // ================ Turn Vector ================= // inline void bgc_cotes_number_turn_vector_fp32(const BgcCotesNumberFP32* number, const BgcVector2FP32* vector, BgcVector2FP32* result) { - const float x1 = number->cos * vector->x1 - number->sin * vector->x2; - const float x2 = number->sin * vector->x1 + number->cos * vector->x2; + const float x1 = number->_cos * vector->x1 - number->_sin * vector->x2; + const float x2 = number->_sin * vector->x1 + number->_cos * vector->x2; result->x1 = x1; result->x2 = x2; @@ -346,8 +308,8 @@ inline void bgc_cotes_number_turn_vector_fp32(const BgcCotesNumberFP32* number, inline void bgc_cotes_number_turn_vector_fp64(const BgcCotesNumberFP64* number, const BgcVector2FP64* vector, BgcVector2FP64* result) { - const double x1 = number->cos * vector->x1 - number->sin * vector->x2; - const double x2 = number->sin * vector->x1 + number->cos * vector->x2; + const double x1 = number->_cos * vector->x1 - number->_sin * vector->x2; + const double x2 = number->_sin * vector->x1 + number->_cos * vector->x2; result->x1 = x1; result->x2 = x2; @@ -357,8 +319,8 @@ inline void bgc_cotes_number_turn_vector_fp64(const BgcCotesNumberFP64* number, inline void bgc_cotes_number_turn_vector_back_fp32(const BgcCotesNumberFP32* number, const BgcVector2FP32* vector, BgcVector2FP32* result) { - const float x1 = number->sin * vector->x2 + number->cos * vector->x1; - const float x2 = number->cos * vector->x2 - number->sin * vector->x1; + const float x1 = number->_sin * vector->x2 + number->_cos * vector->x1; + const float x2 = number->_cos * vector->x2 - number->_sin * vector->x1; result->x1 = x1; result->x2 = x2; @@ -366,8 +328,8 @@ inline void bgc_cotes_number_turn_vector_back_fp32(const BgcCotesNumberFP32* num inline void bgc_cotes_number_turn_vector_back_fp64(const BgcCotesNumberFP64* number, const BgcVector2FP64* vector, BgcVector2FP64* result) { - const double x1 = number->sin * vector->x2 + number->cos * vector->x1; - const double x2 = number->cos * vector->x2 - number->sin * vector->x1; + const double x1 = number->_sin * vector->x2 + number->_cos * vector->x1; + const double x2 = number->_cos * vector->x2 - number->_sin * vector->x1; result->x1 = x1; result->x2 = x2; @@ -377,16 +339,16 @@ inline void bgc_cotes_number_turn_vector_back_fp64(const BgcCotesNumberFP64* num inline int bgc_cotes_number_are_close_fp32(const BgcCotesNumberFP32* number1, const BgcCotesNumberFP32* number2) { - const float d_cos = number1->cos - number2->cos; - const float d_sin = number1->sin - number2->sin; + const float d_cos = number1->_cos - number2->_cos; + const float d_sin = number1->_sin - number2->_sin; return d_cos * d_cos + d_sin * d_sin <= BGC_SQUARE_EPSYLON_FP32; } inline int bgc_cotes_number_are_close_fp64(const BgcCotesNumberFP64* number1, const BgcCotesNumberFP64* number2) { - const double d_cos = number1->cos - number2->cos; - const double d_sin = number1->sin - number2->sin; + const double d_cos = number1->_cos - number2->_cos; + const double d_sin = number1->_sin - number2->_sin; return d_cos * d_cos + d_sin * d_sin <= BGC_SQUARE_EPSYLON_FP64; } diff --git a/basic-geometry/matrix3x3.h b/basic-geometry/matrix3x3.h index b8554b0..6a38b04 100644 --- a/basic-geometry/matrix3x3.h +++ b/basic-geometry/matrix3x3.h @@ -274,22 +274,6 @@ inline int bgc_matrix3x3_is_singular_fp64(const BgcMatrix3x3FP64* matrix) // ================ Is Rotation ================= // -inline int bgc_matrix3x3_is_rotation_fp32(const BgcMatrix3x3FP32* matrix) -{ - if (!bgc_is_unit_fp32(bgc_matrix3x3_get_determinant_fp32(matrix))) { - return 0; - } - - BgcMatrix3x3FP32 transposed, product; - - bgc_matrix3x3_transpose_fp32(matrix, &transposed); - bgc_matrix_product_3x3_at_3x3_fp32(matrix, &transposed, &product); - - 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_fp32a(const BgcMatrix3x3FP32* matrix) { if (!bgc_is_unit_fp32(bgc_matrix3x3_get_determinant_fp32(matrix))) { diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index df057b2..8762c9d 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -4,8 +4,8 @@ extern inline void bgc_quaternion_reset_fp32(BgcQuaternionFP32* quaternion); extern inline void bgc_quaternion_reset_fp64(BgcQuaternionFP64* quaternion); -extern inline void bgc_quaternion_set_to_identity_fp32(BgcQuaternionFP32* quaternion); -extern inline void bgc_quaternion_set_to_identity_fp64(BgcQuaternionFP64* quaternion); +extern inline void bgc_quaternion_make_unit_fp32(BgcQuaternionFP32* quaternion); +extern inline void bgc_quaternion_make_unit_fp64(BgcQuaternionFP64* quaternion); extern inline void bgc_quaternion_set_values_fp32(const float s0, const float x1, const float x2, const float x3, BgcQuaternionFP32* quaternion); extern inline void bgc_quaternion_set_values_fp64(const double s0, const double x1, const double x2, const double x3, BgcQuaternionFP64* quaternion); @@ -31,21 +31,6 @@ extern inline void bgc_quaternion_swap_fp64(BgcQuaternionFP64* quarternion1, Bgc extern inline void bgc_quaternion_convert_fp64_to_fp32(const BgcQuaternionFP64* source, BgcQuaternionFP32* destination); extern inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* source, BgcQuaternionFP64* destination); -extern inline void bgc_quaternion_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate); -extern inline void bgc_quaternion_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate); - -extern inline int bgc_quaternion_invert_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverted); -extern inline int bgc_quaternion_invert_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverted); - -extern inline int bgc_quaternion_normalize_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized); -extern inline int bgc_quaternion_normalize_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized); - -extern inline void bgc_quaternion_get_product_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product); -extern inline void bgc_quaternion_get_product_fp64(const BgcQuaternionFP64* left, const BgcQuaternionFP64* right, BgcQuaternionFP64* product); - -extern inline int bgc_quaternion_get_ratio_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient); -extern inline int bgc_quaternion_get_ratio_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient); - extern inline void bgc_quaternion_add_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, BgcQuaternionFP32* sum); extern inline void bgc_quaternion_add_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, BgcQuaternionFP64* sum); @@ -55,17 +40,50 @@ extern inline void bgc_quaternion_add_scaled_fp64(const BgcQuaternionFP64* basic extern inline void bgc_quaternion_subtract_fp32(const BgcQuaternionFP32* minuend, const BgcQuaternionFP32* subtrahend, BgcQuaternionFP32* difference); extern inline void bgc_quaternion_subtract_fp64(const BgcQuaternionFP64* minuend, const BgcQuaternionFP64* subtrahend, BgcQuaternionFP64* difference); -extern inline void bgc_quaternion_subtract_scaled_fp32(const BgcQuaternionFP32* basic_quaternion, const BgcQuaternionFP32* scalable_quaternion, const float scale, BgcQuaternionFP32* difference); -extern inline void bgc_quaternion_subtract_scaled_fp64(const BgcQuaternionFP64* basic_quaternion, const BgcQuaternionFP64* scalable_quaternion, const double scale, BgcQuaternionFP64* difference); +extern inline void bgc_quaternion_multiply_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product); +extern inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* left, const BgcQuaternionFP64* right, BgcQuaternionFP64* product); -extern inline void bgc_quaternion_multiply_fp32(const BgcQuaternionFP32* multiplicand, const float multipier, BgcQuaternionFP32* product); -extern inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multiplicand, const double multipier, BgcQuaternionFP64* product); +extern inline void bgc_quaternion_multiply_by_number_fp32(const BgcQuaternionFP32* multiplicand, const float multipier, BgcQuaternionFP32* product); +extern inline void bgc_quaternion_multiply_by_number_fp64(const BgcQuaternionFP64* multiplicand, const double multipier, BgcQuaternionFP64* product); -extern inline void bgc_quaternion_divide_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient); -extern inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient); +extern inline int bgc_quaternion_divide_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient); +extern inline int bgc_quaternion_divide_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient); -extern inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation); -extern inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation); +extern inline void bgc_quaternion_divide_by_number_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient); +extern inline void bgc_quaternion_divide_by_number_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient); + +extern inline void bgc_quaternion_get_mean_of_two_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, BgcQuaternionFP32* mean); +extern inline void bgc_quaternion_get_mean_of_two_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, BgcQuaternionFP64* mean); + +extern inline void bgc_quaternion_get_mean_of_three_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const BgcQuaternionFP32* vector3, BgcQuaternionFP32* mean); +extern inline void bgc_quaternion_get_mean_of_three_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const BgcQuaternionFP64* vector3, BgcQuaternionFP64* mean); + +extern inline void bgc_quaternion_interpolate_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation); +extern inline void bgc_quaternion_interpolate_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation); + +extern inline void bgc_quaternion_conjugate_fp32(BgcQuaternionFP32* quaternion); +extern inline void bgc_quaternion_conjugate_fp64(BgcQuaternionFP64* quaternion); + +extern inline void bgc_quaternion_get_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate); +extern inline void bgc_quaternion_get_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate); + +extern inline void bgc_quaternion_make_opposite_fp32(BgcQuaternionFP32* quaternion); +extern inline void bgc_quaternion_make_opposite_fp64(BgcQuaternionFP64* quaternion); + +extern inline void bgc_quaternion_get_opposite_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* opposite); +extern inline void bgc_quaternion_get_opposite_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* opposite); + +extern inline int bgc_quaternion_invert_fp32(BgcQuaternionFP32* quaternion); +extern inline int bgc_quaternion_invert_fp64(BgcQuaternionFP64* quaternion); + +extern inline int bgc_quaternion_get_inverse_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverse); +extern inline int bgc_quaternion_get_inverse_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverse); + +extern inline int bgc_quaternion_normalize_fp32(BgcQuaternionFP32* quaternion); +extern inline int bgc_quaternion_normalize_fp64(BgcQuaternionFP64* quaternion); + +extern inline int bgc_quaternion_get_normalized_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized); +extern inline int bgc_quaternion_get_normalized_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized); extern inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation); extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation); @@ -73,8 +91,8 @@ extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP6 extern inline int bgc_quaternion_get_reverse_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* reverse); extern inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* reverse); -extern inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse); -extern inline int bgc_quaternion_get_both_matrixes_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse); +extern inline int bgc_quaternion_get_both_matrices_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse); +extern inline int bgc_quaternion_get_both_matrices_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse); extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index ebf06db..9d8f0f5 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -33,9 +33,9 @@ inline void bgc_quaternion_reset_fp64(BgcQuaternionFP64 * quaternion) quaternion->x3 = 0.0; } -// ================== Set Unit ================== // +// ================= Make Unit ================== // -inline void bgc_quaternion_set_to_identity_fp32(BgcQuaternionFP32 * quaternion) +inline void bgc_quaternion_make_unit_fp32(BgcQuaternionFP32 * quaternion) { quaternion->s0 = 1.0f; quaternion->x1 = 0.0f; @@ -43,7 +43,7 @@ inline void bgc_quaternion_set_to_identity_fp32(BgcQuaternionFP32 * quaternion) quaternion->x3 = 0.0f; } -inline void bgc_quaternion_set_to_identity_fp64(BgcQuaternionFP64 * quaternion) +inline void bgc_quaternion_make_unit_fp64(BgcQuaternionFP64 * quaternion) { quaternion->s0 = 1.0; quaternion->x1 = 0.0; @@ -191,186 +191,6 @@ inline void bgc_quaternion_convert_fp32_to_fp64(const BgcQuaternionFP32* source, destination->x3 = source->x3; } -// ================= Conjugate ================== // - -inline void bgc_quaternion_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate) -{ - conjugate->s0 = quaternion->s0; - conjugate->x1 = -quaternion->x1; - conjugate->x2 = -quaternion->x2; - conjugate->x3 = -quaternion->x3; -} - -inline void bgc_quaternion_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate) -{ - conjugate->s0 = quaternion->s0; - conjugate->x1 = -quaternion->x1; - conjugate->x2 = -quaternion->x2; - conjugate->x3 = -quaternion->x3; -} - -// =================== Invert =================== // - -inline int bgc_quaternion_invert_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverted) -{ - const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float multiplicand = 1.0f / square_modulus; - - inverted->s0 = quaternion->s0 * multiplicand; - inverted->x1 = -quaternion->x1 * multiplicand; - inverted->x2 = -quaternion->x2 * multiplicand; - inverted->x3 = -quaternion->x3 * multiplicand; - - return 1; -} - -inline int bgc_quaternion_invert_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverted) -{ - const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double multiplicand = 1.0 / square_modulus; - - inverted->s0 = quaternion->s0 * multiplicand; - inverted->x1 = -quaternion->x1 * multiplicand; - inverted->x2 = -quaternion->x2 * multiplicand; - inverted->x3 = -quaternion->x3 * multiplicand; - - return 1; -} - -// ================= Normalize ================== // - -inline int bgc_quaternion_normalize_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized) -{ - const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion); - - if (bgc_is_sqare_unit_fp32(square_modulus)) { - bgc_quaternion_copy_fp32(quaternion, normalized); - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float multiplier = sqrtf(1.0f / square_modulus); - - normalized->s0 = quaternion->s0 * multiplier; - normalized->x1 = quaternion->x1 * multiplier; - normalized->x2 = quaternion->x2 * multiplier; - normalized->x3 = quaternion->x3 * multiplier; - - return 1; -} - -inline int bgc_quaternion_normalize_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized) -{ - const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion); - - if (bgc_is_sqare_unit_fp64(square_modulus)) { - bgc_quaternion_copy_fp64(quaternion, normalized); - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double multiplier = sqrt(1.0 / square_modulus); - - normalized->s0 *= multiplier; - normalized->x1 *= multiplier; - normalized->x2 *= multiplier; - normalized->x3 *= multiplier; - - return 1; -} - -// ================ Get Product ================= // - -inline void bgc_quaternion_get_product_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product) -{ - const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); - const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); - const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); - const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); - - product->s0 = s0; - product->x1 = x1; - product->x2 = x2; - product->x3 = x3; -} - -inline void bgc_quaternion_get_product_fp64(const BgcQuaternionFP64* left, const BgcQuaternionFP64* right, BgcQuaternionFP64* product) -{ - const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); - const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); - const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); - const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); - - product->s0 = s0; - product->x1 = x1; - product->x2 = x2; - product->x3 = x3; -} - -// ================= Get Ratio ================== // - -inline int bgc_quaternion_get_ratio_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient) -{ - const float square_modulus = bgc_quaternion_get_square_modulus_fp32(divisor); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3); - const float x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3); - const float x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1); - const float x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2); - - const float multiplicand = 1.0f / square_modulus; - - quotient->s0 = s0 * multiplicand; - quotient->x1 = x1 * multiplicand; - quotient->x2 = x2 * multiplicand; - quotient->x3 = x3 * multiplicand; - - return 1; -} - -inline int bgc_quaternion_get_ratio_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient) -{ - const double square_modulus = bgc_quaternion_get_square_modulus_fp64(divisor); - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3); - const double x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3); - const double x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1); - const double x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2); - - const double multiplicand = 1.0 / square_modulus; - - quotient->s0 = s0 * multiplicand; - quotient->x1 = x1 * multiplicand; - quotient->x2 = x2 * multiplicand; - quotient->x3 = x3 * multiplicand; - - return 1; -} - // ==================== Add ===================== // inline void bgc_quaternion_add_fp32(const BgcQuaternionFP32 * quaternion1, const BgcQuaternionFP32 * quaternion2, BgcQuaternionFP32 * sum) @@ -425,27 +245,35 @@ inline void bgc_quaternion_subtract_fp64(const BgcQuaternionFP64 * minuend, cons difference->x3 = minuend->x3 - subtrahend->x3; } -// ============== Subtract scaled =============== // - -inline void bgc_quaternion_subtract_scaled_fp32(const BgcQuaternionFP32 * basic_quaternion, const BgcQuaternionFP32 * scalable_quaternion, const float scale, BgcQuaternionFP32 * difference) -{ - difference->s0 = basic_quaternion->s0 - scalable_quaternion->s0 * scale; - difference->x1 = basic_quaternion->x1 - scalable_quaternion->x1 * scale; - difference->x2 = basic_quaternion->x2 - scalable_quaternion->x2 * scale; - difference->x3 = basic_quaternion->x3 - scalable_quaternion->x3 * scale; -} - -inline void bgc_quaternion_subtract_scaled_fp64(const BgcQuaternionFP64 * basic_quaternion, const BgcQuaternionFP64 * scalable_quaternion, const double scale, BgcQuaternionFP64 * difference) -{ - difference->s0 = basic_quaternion->s0 - scalable_quaternion->s0 * scale; - difference->x1 = basic_quaternion->x1 - scalable_quaternion->x1 * scale; - difference->x2 = basic_quaternion->x2 - scalable_quaternion->x2 * scale; - difference->x3 = basic_quaternion->x3 - scalable_quaternion->x3 * scale; -} - // ================== Multiply ================== // -inline void bgc_quaternion_multiply_fp32(const BgcQuaternionFP32* multiplicand, const float multipier, BgcQuaternionFP32* product) +inline void bgc_quaternion_multiply_fp32(const BgcQuaternionFP32* left, const BgcQuaternionFP32* right, BgcQuaternionFP32* product) +{ + const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); + + product->s0 = s0; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; +} + +inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* left, const BgcQuaternionFP64* right, BgcQuaternionFP64* product) +{ + const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); + + product->s0 = s0; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; +} + +inline void bgc_quaternion_multiply_by_number_fp32(const BgcQuaternionFP32* multiplicand, const float multipier, BgcQuaternionFP32* product) { product->s0 = multiplicand->s0 * multipier; product->x1 = multiplicand->x1 * multipier; @@ -453,7 +281,7 @@ inline void bgc_quaternion_multiply_fp32(const BgcQuaternionFP32* multiplicand, product->x3 = multiplicand->x3 * multipier; } -inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multiplicand, const double multipier, BgcQuaternionFP64* product) +inline void bgc_quaternion_multiply_by_number_fp64(const BgcQuaternionFP64* multiplicand, const double multipier, BgcQuaternionFP64* product) { product->s0 = multiplicand->s0 * multipier; product->x1 = multiplicand->x1 * multipier; @@ -463,19 +291,101 @@ inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multiplicand, // =================== Divide =================== // -inline void bgc_quaternion_divide_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient) +inline int bgc_quaternion_divide_fp32(const BgcQuaternionFP32* divident, const BgcQuaternionFP32* divisor, BgcQuaternionFP32* quotient) { - bgc_quaternion_multiply_fp32(dividend, 1.0f / divisor, quotient); + const float square_modulus = bgc_quaternion_get_square_modulus_fp32(divisor); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; + } + + const float s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3); + const float x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3); + const float x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1); + const float x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2); + + const float multiplicand = 1.0f / square_modulus; + + quotient->s0 = s0 * multiplicand; + quotient->x1 = x1 * multiplicand; + quotient->x2 = x2 * multiplicand; + quotient->x3 = x3 * multiplicand; + + return 1; } -inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient) +inline int bgc_quaternion_divide_fp64(const BgcQuaternionFP64* divident, const BgcQuaternionFP64* divisor, BgcQuaternionFP64* quotient) { - bgc_quaternion_multiply_fp64(dividend, 1.0 / divisor, quotient); + const double square_modulus = bgc_quaternion_get_square_modulus_fp64(divisor); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double s0 = (divident->s0 * divisor->s0 + divident->x1 * divisor->x1) + (divident->x2 * divisor->x2 + divident->x3 * divisor->x3); + const double x1 = (divident->x1 * divisor->s0 + divident->x3 * divisor->x2) - (divident->s0 * divisor->x1 + divident->x2 * divisor->x3); + const double x2 = (divident->x2 * divisor->s0 + divident->x1 * divisor->x3) - (divident->s0 * divisor->x2 + divident->x3 * divisor->x1); + const double x3 = (divident->x3 * divisor->s0 + divident->x2 * divisor->x1) - (divident->s0 * divisor->x3 + divident->x1 * divisor->x2); + + const double multiplicand = 1.0 / square_modulus; + + quotient->s0 = s0 * multiplicand; + quotient->x1 = x1 * multiplicand; + quotient->x2 = x2 * multiplicand; + quotient->x3 = x3 * multiplicand; + + return 1; +} + +inline void bgc_quaternion_divide_by_number_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient) +{ + bgc_quaternion_multiply_by_number_fp32(dividend, 1.0f / divisor, quotient); +} + +inline void bgc_quaternion_divide_by_number_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient) +{ + bgc_quaternion_multiply_by_number_fp64(dividend, 1.0 / divisor, quotient); +} + +// ================ Mean of Two ================= // + +inline void bgc_quaternion_get_mean_of_two_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, BgcQuaternionFP32* mean) +{ + mean->s0 = (vector1->s0 + vector2->s0) * 0.5f; + mean->x1 = (vector1->x1 + vector2->x1) * 0.5f; + mean->x2 = (vector1->x2 + vector2->x2) * 0.5f; + mean->x3 = (vector1->x3 + vector2->x3) * 0.5f; +} + +inline void bgc_quaternion_get_mean_of_two_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, BgcQuaternionFP64* mean) +{ + mean->s0 = (vector1->s0 + vector2->s0) * 0.5f; + mean->x1 = (vector1->x1 + vector2->x1) * 0.5f; + mean->x2 = (vector1->x2 + vector2->x2) * 0.5f; + mean->x3 = (vector1->x3 + vector2->x3) * 0.5f; +} + +// =============== Mean of Three ================ // + +inline void bgc_quaternion_get_mean_of_three_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const BgcQuaternionFP32* vector3, BgcQuaternionFP32* mean) +{ + mean->s0 = (vector1->s0 + vector2->s0 + vector3->s0) * BGC_ONE_THIRD_FP32; + mean->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP32; + mean->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP32; + mean->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP32; +} + +inline void bgc_quaternion_get_mean_of_three_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const BgcQuaternionFP64* vector3, BgcQuaternionFP64* mean) +{ + mean->s0 = (vector1->s0 + vector2->s0 + vector3->s0) * BGC_ONE_THIRD_FP64; + mean->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP64; + mean->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP64; + mean->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP64; } // ============ Linear Interpolation ============ // -inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation) +inline void bgc_quaternion_interpolate_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation) { const float counterphase = 1.0f - phase; @@ -485,7 +395,7 @@ inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* qu interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase; } -inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation) +inline void bgc_quaternion_interpolate_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation) { const double counterphase = 1.0 - phase; @@ -495,6 +405,202 @@ inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* qu interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase; } +// ================= Conjugate ================== // + +inline void bgc_quaternion_conjugate_fp32(BgcQuaternionFP32* quaternion) +{ + quaternion->x1 = -quaternion->x1; + quaternion->x2 = -quaternion->x2; + quaternion->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_conjugate_fp64(BgcQuaternionFP64* quaternion) +{ + quaternion->x1 = -quaternion->x1; + quaternion->x2 = -quaternion->x2; + quaternion->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_get_conjugate_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* conjugate) +{ + conjugate->s0 = quaternion->s0; + conjugate->x1 = -quaternion->x1; + conjugate->x2 = -quaternion->x2; + conjugate->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_get_conjugate_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* conjugate) +{ + conjugate->s0 = quaternion->s0; + conjugate->x1 = -quaternion->x1; + conjugate->x2 = -quaternion->x2; + conjugate->x3 = -quaternion->x3; +} + +// ================== Negative ================== // + +inline void bgc_quaternion_make_opposite_fp32(BgcQuaternionFP32* quaternion) +{ + quaternion->s0 = -quaternion->s0; + quaternion->x1 = -quaternion->x1; + quaternion->x2 = -quaternion->x2; + quaternion->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_make_opposite_fp64(BgcQuaternionFP64* quaternion) +{ + quaternion->s0 = -quaternion->s0; + quaternion->x1 = -quaternion->x1; + quaternion->x2 = -quaternion->x2; + quaternion->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_get_opposite_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* opposite) +{ + opposite->s0 = -quaternion->s0; + opposite->x1 = -quaternion->x1; + opposite->x2 = -quaternion->x2; + opposite->x3 = -quaternion->x3; +} + +inline void bgc_quaternion_get_opposite_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* opposite) +{ + opposite->s0 = -quaternion->s0; + opposite->x1 = -quaternion->x1; + opposite->x2 = -quaternion->x2; + opposite->x3 = -quaternion->x3; +} + +// =================== Invert =================== // + +inline int bgc_quaternion_get_inverse_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* inverse) +{ + const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; + } + + const float multiplicand = 1.0f / square_modulus; + + inverse->s0 = quaternion->s0 * multiplicand; + inverse->x1 = -quaternion->x1 * multiplicand; + inverse->x2 = -quaternion->x2 * multiplicand; + inverse->x3 = -quaternion->x3 * multiplicand; + + return 1; +} + +inline int bgc_quaternion_get_inverse_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* inverse) +{ + const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion); + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double multiplicand = 1.0 / square_modulus; + + inverse->s0 = quaternion->s0 * multiplicand; + inverse->x1 = -quaternion->x1 * multiplicand; + inverse->x2 = -quaternion->x2 * multiplicand; + inverse->x3 = -quaternion->x3 * multiplicand; + + return 1; +} + +inline int bgc_quaternion_invert_fp32(BgcQuaternionFP32* quaternion) +{ + return bgc_quaternion_get_inverse_fp32(quaternion, quaternion); +} + +inline int bgc_quaternion_invert_fp64(BgcQuaternionFP64* quaternion) +{ + return bgc_quaternion_get_inverse_fp64(quaternion, quaternion); +} + +// ================= Normalize ================== // + +inline int bgc_quaternion_normalize_fp32(BgcQuaternionFP32* quaternion) +{ + const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + quaternion->s0 *= multiplier; + quaternion->x1 *= multiplier; + quaternion->x2 *= multiplier; + quaternion->x3 *= multiplier; + + return 1; +} + +inline int bgc_quaternion_normalize_fp64(BgcQuaternionFP64* quaternion) +{ + const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + quaternion->s0 *= multiplier; + quaternion->x1 *= multiplier; + quaternion->x2 *= multiplier; + quaternion->x3 *= multiplier; + + return 1; +} + +inline int bgc_quaternion_get_normalized_fp32(const BgcQuaternionFP32* quaternion, BgcQuaternionFP32* normalized) +{ + const float square_modulus = bgc_quaternion_get_square_modulus_fp32(quaternion); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + bgc_quaternion_copy_fp32(quaternion, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + bgc_quaternion_reset_fp32(normalized); + return 0; + } + + bgc_quaternion_multiply_by_number_fp32(quaternion, sqrtf(1.0f / square_modulus), normalized); + return 1; +} + +inline int bgc_quaternion_get_normalized_fp64(const BgcQuaternionFP64* quaternion, BgcQuaternionFP64* normalized) +{ + const double square_modulus = bgc_quaternion_get_square_modulus_fp64(quaternion); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + bgc_quaternion_copy_fp64(quaternion, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + bgc_quaternion_reset_fp64(normalized); + return 0; + } + + bgc_quaternion_multiply_by_number_fp64(quaternion, sqrt(1.0 / square_modulus), normalized); + return 1; +} + // =============== Get Exponation =============== // int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power); @@ -514,6 +620,7 @@ inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quat if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + bgc_matrix3x3_set_to_identity_fp32(rotation); return 0; } @@ -554,6 +661,7 @@ inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quat if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + bgc_matrix3x3_set_to_identity_fp64(rotation); return 0; } @@ -596,6 +704,7 @@ inline int bgc_quaternion_get_reverse_matrix_fp32(const BgcQuaternionFP32* quate if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + bgc_matrix3x3_set_to_identity_fp32(reverse); return 0; } @@ -636,6 +745,7 @@ inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quate if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + bgc_matrix3x3_set_to_identity_fp64(reverse); return 0; } @@ -667,7 +777,7 @@ inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64* quate // ============= Get Both Matrixes ============== // -inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse) +inline int bgc_quaternion_get_both_matrices_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse) { if (bgc_quaternion_get_reverse_matrix_fp32(quaternion, reverse)) { bgc_matrix3x3_transpose_fp32(reverse, rotation); @@ -677,7 +787,7 @@ inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quater return 0; } -inline int bgc_quaternion_get_both_matrixes_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse) +inline int bgc_quaternion_get_both_matrices_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse) { if (bgc_quaternion_get_reverse_matrix_fp64(quaternion, reverse)) { bgc_matrix3x3_transpose_fp64(reverse, rotation); diff --git a/basic-geometry/rotation3.h b/basic-geometry/rotation3.h index d55cdc3..c382316 100644 --- a/basic-geometry/rotation3.h +++ b/basic-geometry/rotation3.h @@ -72,9 +72,7 @@ inline void bgc_rotation3_set_values_fp64(const double x1, const double x2, cons inline void bgc_rotation3_set_with_axis_fp32(const BgcVector3FP32* axis, const float angle, const BgcAngleUnitEnum unit, BgcRotation3FP32* rotation) { - bgc_vector3_copy_fp32(axis, &rotation->axis); - - if (bgc_vector3_normalize_fp32(&rotation->axis)) { + if (bgc_vector3_get_normalized_fp32(axis, &rotation->axis)) { rotation->radians = bgc_angle_to_radians_fp32(angle, unit); } else { @@ -84,9 +82,7 @@ inline void bgc_rotation3_set_with_axis_fp32(const BgcVector3FP32* axis, const f inline void bgc_rotation3_set_with_axis_fp64(const BgcVector3FP64* axis, const double angle, const BgcAngleUnitEnum unit, BgcRotation3FP64* rotation) { - bgc_vector3_copy_fp64(axis, &rotation->axis); - - if (bgc_vector3_normalize_fp64(&rotation->axis)) { + if (bgc_vector3_get_normalized_fp64(axis, &rotation->axis)) { rotation->radians = bgc_angle_to_radians_fp64(angle, unit); } else { diff --git a/basic-geometry/slerp.c b/basic-geometry/slerp.c index 40753eb..9ad91df 100644 --- a/basic-geometry/slerp.c +++ b/basic-geometry/slerp.c @@ -14,7 +14,7 @@ extern inline void bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, void bgc_slerp_make_fp32(const BgcVersorFP32* start, const BgcVersorFP32* augment, BgcSlerpFP32* slerp) { - const float square_vector = augment->x1 * augment->x1 + augment->x2 * augment->x2 + augment->x3 * augment->x3; + const float square_vector = augment->_x1 * augment->_x1 + augment->_x2 * augment->_x2 + augment->_x3 * augment->_x3; if (square_vector != square_vector) { bgc_slerp_reset_fp32(slerp); @@ -22,10 +22,10 @@ void bgc_slerp_make_fp32(const BgcVersorFP32* start, const BgcVersorFP32* augmen } if (square_vector <= BGC_SQUARE_EPSYLON_FP32) { - slerp->s0_cos_weight = start->s0; - slerp->x1_cos_weight = start->x1; - slerp->x2_cos_weight = start->x2; - slerp->x3_cos_weight = start->x3; + slerp->s0_cos_weight = start->_s0; + slerp->x1_cos_weight = start->_x1; + slerp->x2_cos_weight = start->_x2; + slerp->x3_cos_weight = start->_x3; slerp->s0_sin_weight = 0.0f; slerp->x1_sin_weight = 0.0f; @@ -38,24 +38,24 @@ void bgc_slerp_make_fp32(const BgcVersorFP32* start, const BgcVersorFP32* augmen const float vector_modulus = sqrtf(square_vector); - slerp->radians = atan2f(vector_modulus, augment->s0); + slerp->radians = atan2f(vector_modulus, augment->_s0); const float multiplier = 1.0f / vector_modulus; - slerp->s0_cos_weight = start->s0; - slerp->x1_cos_weight = start->x1; - slerp->x2_cos_weight = start->x2; - slerp->x3_cos_weight = start->x3; + slerp->s0_cos_weight = start->_s0; + slerp->x1_cos_weight = start->_x1; + slerp->x2_cos_weight = start->_x2; + slerp->x3_cos_weight = start->_x3; - slerp->s0_sin_weight = -multiplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3); - slerp->x1_sin_weight = multiplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2); - slerp->x2_sin_weight = multiplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1); - slerp->x3_sin_weight = multiplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2); + slerp->s0_sin_weight = -multiplier * (augment->_x1 * start->_x1 + augment->_x2 * start->_x2 + augment->_x3 * start->_x3); + slerp->x1_sin_weight = multiplier * (augment->_x1 * start->_s0 + augment->_x2 * start->_x3 - augment->_x3 * start->_x2); + slerp->x2_sin_weight = multiplier * (augment->_x2 * start->_s0 - augment->_x1 * start->_x3 + augment->_x3 * start->_x1); + slerp->x3_sin_weight = multiplier * (augment->_x3 * start->_s0 - augment->_x2 * start->_x1 + augment->_x1 * start->_x2); } void bgc_slerp_make_fp64(const BgcVersorFP64* start, const BgcVersorFP64* augment, BgcSlerpFP64* slerp) { - const double square_vector = augment->x1 * augment->x1 + augment->x2 * augment->x2 + augment->x3 * augment->x3; + const double square_vector = augment->_x1 * augment->_x1 + augment->_x2 * augment->_x2 + augment->_x3 * augment->_x3; if (square_vector != square_vector) { bgc_slerp_reset_fp64(slerp); @@ -63,10 +63,10 @@ void bgc_slerp_make_fp64(const BgcVersorFP64* start, const BgcVersorFP64* augmen } if (square_vector <= BGC_SQUARE_EPSYLON_FP64) { - slerp->s0_cos_weight = start->s0; - slerp->x1_cos_weight = start->x1; - slerp->x2_cos_weight = start->x2; - slerp->x3_cos_weight = start->x3; + slerp->s0_cos_weight = start->_s0; + slerp->x1_cos_weight = start->_x1; + slerp->x2_cos_weight = start->_x2; + slerp->x3_cos_weight = start->_x3; slerp->s0_sin_weight = 0.0; slerp->x1_sin_weight = 0.0; @@ -79,17 +79,17 @@ void bgc_slerp_make_fp64(const BgcVersorFP64* start, const BgcVersorFP64* augmen const double vector_modulus = sqrt(square_vector); - slerp->radians = atan2(vector_modulus, augment->s0); + slerp->radians = atan2(vector_modulus, augment->_s0); const double multiplier = 1.0 / vector_modulus; - slerp->s0_cos_weight = start->s0; - slerp->x1_cos_weight = start->x1; - slerp->x2_cos_weight = start->x2; - slerp->x3_cos_weight = start->x3; + slerp->s0_cos_weight = start->_s0; + slerp->x1_cos_weight = start->_x1; + slerp->x2_cos_weight = start->_x2; + slerp->x3_cos_weight = start->_x3; - slerp->s0_sin_weight = -multiplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3); - slerp->x1_sin_weight = multiplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2); - slerp->x2_sin_weight = multiplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1); - slerp->x3_sin_weight = multiplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2); + slerp->s0_sin_weight = -multiplier * (augment->_x1 * start->_x1 + augment->_x2 * start->_x2 + augment->_x3 * start->_x3); + slerp->x1_sin_weight = multiplier * (augment->_x1 * start->_s0 + augment->_x2 * start->_x3 - augment->_x3 * start->_x2); + slerp->x2_sin_weight = multiplier * (augment->_x2 * start->_s0 - augment->_x1 * start->_x3 + augment->_x3 * start->_x1); + slerp->x3_sin_weight = multiplier * (augment->_x3 * start->_s0 - augment->_x2 * start->_x1 + augment->_x1 * start->_x2); } diff --git a/basic-geometry/slerp.h b/basic-geometry/slerp.h index c2f02a5..5760c2c 100644 --- a/basic-geometry/slerp.h +++ b/basic-geometry/slerp.h @@ -80,7 +80,7 @@ inline void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcV BgcVersorFP32 augment; bgc_versor_exclude_fp32(end, start, &augment); - bgc_versor_shorten_fp32(&augment, &augment); + bgc_versor_shorten_fp32(&augment); bgc_slerp_make_fp32(start, &augment, slerp); } @@ -90,7 +90,7 @@ inline void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcV BgcVersorFP64 augment; bgc_versor_exclude_fp64(end, start, &augment); - bgc_versor_shorten_fp64(&augment, &augment); + bgc_versor_shorten_fp64(&augment); bgc_slerp_make_fp64(start, &augment, slerp); } diff --git a/basic-geometry/types.h b/basic-geometry/types.h new file mode 100644 index 0000000..7ad6d7f --- /dev/null +++ b/basic-geometry/types.h @@ -0,0 +1,16 @@ +#ifndef _BGC_TYPES_H_ +#define _BGC_TYPES_H_ + +// ================== Complex =================== // + +typedef struct +{ + float real, imaginary; +} BgcComplexFP32; + +typedef struct +{ + double real, imaginary; +} BgcComplexFP64; + +#endif diff --git a/basic-geometry/utilities.c b/basic-geometry/utilities.c index ec89f4f..a656162 100644 --- a/basic-geometry/utilities.c +++ b/basic-geometry/utilities.c @@ -1,6 +1,6 @@ #include "utilities.h" -extern inline int bgc_is_correct_direction(const int direction); +extern inline int bgc_is_correct_axis(const int axis); extern inline int bgc_is_zero_fp32(const float square_value); extern inline int bgc_is_zero_fp64(const double square_value); diff --git a/basic-geometry/utilities.h b/basic-geometry/utilities.h index aadca7f..25ae143 100644 --- a/basic-geometry/utilities.h +++ b/basic-geometry/utilities.h @@ -32,15 +32,25 @@ #define BGC_SUCCESS 0 #define BGC_FAILED -1 -#define BGC_DIRECTION_X1 1 -#define BGC_DIRECTION_X2 2 -#define BGC_DIRECTION_X3 3 +#define BGC_ATTITUDE_ANY 0 +#define BGC_ATTITUDE_ZERO 1 +#define BGC_ATTITUDE_ORTHOGONAL 2 +#define BGC_ATTITUDE_CO_DIRECTIONAL 3 +#define BGC_ATTITUDE_COUNTER_DIRECTIONAL 4 -inline int bgc_is_correct_direction(const int direction) +#define BGC_AXIS_X1 1 +#define BGC_AXIS_X2 2 +#define BGC_AXIS_X3 3 + +#define BGC_AXIS_REVERSE_X1 -1 +#define BGC_AXIS_REVERSE_X2 -2 +#define BGC_AXIS_REVERSE_X3 -3 + +inline int bgc_is_correct_axis(const int axis) { - return direction == BGC_DIRECTION_X1 || direction == -BGC_DIRECTION_X1 - || direction == BGC_DIRECTION_X2 || direction == -BGC_DIRECTION_X2 - || direction == BGC_DIRECTION_X3 || direction == -BGC_DIRECTION_X3; + return axis == BGC_AXIS_X1 || axis == BGC_AXIS_REVERSE_X1 + || axis == BGC_AXIS_X2 || axis == BGC_AXIS_REVERSE_X2 + || axis == BGC_AXIS_X3 || axis == BGC_AXIS_REVERSE_X3; } inline int bgc_is_zero_fp32(const float value) diff --git a/basic-geometry/vector2.c b/basic-geometry/vector2.c index bf59117..64f2521 100644 --- a/basic-geometry/vector2.c +++ b/basic-geometry/vector2.c @@ -6,9 +6,6 @@ extern inline void bgc_vector2_reset_fp64(BgcVector2FP64* vector); extern inline void bgc_vector2_set_values_fp32(const float x1, const float x2, BgcVector2FP32* destination); extern inline void bgc_vector2_set_values_fp64(const double x1, const double x2, BgcVector2FP64* destination); -extern inline int bgc_vector2_get_direction_fp32(const int direction, BgcVector2FP32* vector); -extern inline int bgc_vector2_get_direction_fp64(const int direction, BgcVector2FP64* vector); - extern inline float bgc_vector2_get_square_modulus_fp32(const BgcVector2FP32* vector); extern inline double bgc_vector2_get_square_modulus_fp64(const BgcVector2FP64* vector); @@ -30,12 +27,6 @@ extern inline void bgc_vector2_swap_fp64(BgcVector2FP64* vector1, BgcVector2FP64 extern inline void bgc_vector2_convert_fp64_to_fp32(const BgcVector2FP64* source, BgcVector2FP32* destination); extern inline void bgc_vector2_convert_fp32_to_fp64(const BgcVector2FP32* source, BgcVector2FP64* destination); -extern inline void bgc_vector2_reverse_fp32(const BgcVector2FP32* vector, BgcVector2FP32* reverse); -extern inline void bgc_vector2_reverse_fp64(const BgcVector2FP64* vector, BgcVector2FP64* reverse); - -extern inline int bgc_vector2_normalize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* normalized); -extern inline int bgc_vector2_normalize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* normalized); - extern inline void bgc_vector2_add_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, BgcVector2FP32* sum); extern inline void bgc_vector2_add_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, BgcVector2FP64* sum); @@ -45,9 +36,6 @@ extern inline void bgc_vector2_add_scaled_fp64(const BgcVector2FP64* basic_vecto extern inline void bgc_vector2_subtract_fp32(const BgcVector2FP32* minuend, const BgcVector2FP32* subtrahend, BgcVector2FP32* difference); extern inline void bgc_vector2_subtract_fp64(const BgcVector2FP64* minuend, const BgcVector2FP64* subtrahend, BgcVector2FP64* difference); -extern inline void bgc_vector2_subtract_scaled_fp32(const BgcVector2FP32* basic_vector, const BgcVector2FP32* scalable_vector, const float scale, BgcVector2FP32* difference); -extern inline void bgc_vector2_subtract_scaled_fp64(const BgcVector2FP64* basic_vector, const BgcVector2FP64* scalable_vector, const double scale, BgcVector2FP64* difference); - extern inline void bgc_vector2_multiply_fp32(const BgcVector2FP32* multiplicand, const float multiplier, BgcVector2FP32* product); extern inline void bgc_vector2_multiply_fp64(const BgcVector2FP64* multiplicand, const double multiplier, BgcVector2FP64* product); @@ -60,14 +48,20 @@ extern inline void bgc_vector2_get_mean_of_two_fp64(const BgcVector2FP64* vector extern inline void bgc_vector2_get_mean_of_three_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const BgcVector2FP32* vector3, BgcVector2FP32* mean); extern inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcVector2FP64* vector3, BgcVector2FP64* mean); -extern inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation); -extern inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation); +extern inline void bgc_vector2_interpolate_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation); +extern inline void bgc_vector2_interpolate_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation); -extern inline void bgc_vector2_minimize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* minimal); -extern inline void bgc_vector2_minimize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* minimal); +extern inline void bgc_vector2_make_opposite_fp32(BgcVector2FP32* vector); +extern inline void bgc_vector2_make_opposite_fp64(BgcVector2FP64* vector); -extern inline void bgc_vector2_maximize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* maximal); -extern inline void bgc_vector2_maximize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* maximal); +extern inline void bgc_vector2_get_opposite_fp32(const BgcVector2FP32* vector, BgcVector2FP32* opposite); +extern inline void bgc_vector2_get_opposite_fp64(const BgcVector2FP64* vector, BgcVector2FP64* opposite); + +extern inline int bgc_vector2_normalize_fp32(BgcVector2FP32* vector); +extern inline int bgc_vector2_normalize_fp64(BgcVector2FP64* vector); + +extern inline int bgc_vector2_get_normalized_fp32(const BgcVector2FP32* vector, BgcVector2FP32* normalized); +extern inline int bgc_vector2_get_normalized_fp64(const BgcVector2FP64* vector, BgcVector2FP64* normalized); extern inline float bgc_vector2_get_scalar_product_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2); extern inline double bgc_vector2_get_scalar_product_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2); @@ -87,6 +81,15 @@ extern inline int bgc_vector2_are_close_enough_fp64(const BgcVector2FP64* vector extern inline int bgc_vector2_are_close_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2); extern inline int bgc_vector2_are_close_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2); +extern inline int bgc_vector2_are_parallel_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2); +extern inline int bgc_vector2_are_parallel_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2); + +extern inline int bgc_vector2_are_orthogonal_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2); +extern inline int bgc_vector2_are_orthogonal_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2); + +extern inline int bgc_vector2_get_attitude_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2); +extern inline int bgc_vector2_get_attitude_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2); + // =================== Angle ==================== // float bgc_vector2_get_angle_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const BgcAngleUnitEnum unit) @@ -105,11 +108,13 @@ float bgc_vector2_get_angle_fp32(const BgcVector2FP32* vector1, const BgcVector2 return 0.0f; } - const float scalar = bgc_vector2_get_scalar_product_fp32(vector1, vector2); + const float multiplier = sqrtf(1.0f / (square_modulus1 * square_modulus2)); - const float cross = bgc_vector2_get_cross_product_fp32(vector1, vector2); + const float x = bgc_vector2_get_scalar_product_fp32(vector1, vector2); - return bgc_radians_to_units_fp32(atan2f(cross >= 0 ? cross : -cross, scalar), unit); + const float y = fabsf(bgc_vector2_get_cross_product_fp32(vector1, vector2)); + + return bgc_radians_to_units_fp32(atan2f(y * multiplier, x * multiplier), unit); } double bgc_vector2_get_angle_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcAngleUnitEnum unit) @@ -128,9 +133,11 @@ double bgc_vector2_get_angle_fp64(const BgcVector2FP64* vector1, const BgcVector return 0.0; } - const double scalar = bgc_vector2_get_scalar_product_fp64(vector1, vector2); + const double multiplier = sqrt(1.0 / (square_modulus1 * square_modulus2)); - const double cross = bgc_vector2_get_cross_product_fp64(vector1, vector2); + const double x = bgc_vector2_get_scalar_product_fp64(vector1, vector2); - return bgc_radians_to_units_fp64(atan2(cross >= 0 ? cross : -cross, scalar), unit); + const double y = bgc_vector2_get_cross_product_fp64(vector1, vector2); + + return bgc_radians_to_units_fp64(atan2(y * multiplier, x * multiplier), unit); } diff --git a/basic-geometry/vector2.h b/basic-geometry/vector2.h index 9c9c508..d5e5bbb 100644 --- a/basic-geometry/vector2.h +++ b/basic-geometry/vector2.h @@ -44,62 +44,6 @@ inline void bgc_vector2_set_values_fp64(const double x1, const double x2, BgcVec destination->x2 = x2; } -// ================= Directions ================= // - -inline int bgc_vector2_get_direction_fp32(const int direction, BgcVector2FP32* vector) -{ - switch (direction) { - case BGC_DIRECTION_X1: - vector->x1 = 1.0f; - vector->x2 = 0.0f; - return 1; - - case BGC_DIRECTION_X2: - vector->x1 = 0.0f; - vector->x2 = 1.0f; - return 1; - - case -BGC_DIRECTION_X1: - vector->x1 = -1.0f; - vector->x2 = 0.0f; - return 1; - - case -BGC_DIRECTION_X2: - vector->x1 = 0.0f; - vector->x2 = -1.0f; - return 1; - } - - return 0; -} - -inline int bgc_vector2_get_direction_fp64(const int direction, BgcVector2FP64* vector) -{ - switch (direction) { - case BGC_DIRECTION_X1: - vector->x1 = 1.0; - vector->x2 = 0.0; - return 1; - - case BGC_DIRECTION_X2: - vector->x1 = 0.0; - vector->x2 = 1.0; - return 1; - - case -BGC_DIRECTION_X1: - vector->x1 = -1.0; - vector->x2 = 0.0; - return 1; - - case -BGC_DIRECTION_X2: - vector->x1 = 0.0; - vector->x2 = -1.0; - return 1; - } - - return 0; -} - // ================== Modulus =================== // inline float bgc_vector2_get_square_modulus_fp32(const BgcVector2FP32* vector) @@ -198,66 +142,6 @@ inline void bgc_vector2_convert_fp32_to_fp64(const BgcVector2FP32* source, BgcVe destination->x2 = source->x2; } -// ================== Reverse =================== // - -inline void bgc_vector2_reverse_fp32(const BgcVector2FP32* vector, BgcVector2FP32* reverse) -{ - reverse->x1 = -vector->x1; - reverse->x2 = -vector->x2; -} - -inline void bgc_vector2_reverse_fp64(const BgcVector2FP64* vector, BgcVector2FP64* reverse) -{ - reverse->x1 = -vector->x1; - reverse->x2 = -vector->x2; -} - -// ================= Normalize ================== // - -inline int bgc_vector2_normalize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* normalized) -{ - const float square_modulus = bgc_vector2_get_square_modulus_fp32(vector); - - if (bgc_is_sqare_unit_fp32(square_modulus)) { - normalized->x1 = vector->x1; - normalized->x2 = vector->x2; - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float multiplicand = sqrtf(1.0f / square_modulus); - - normalized->x1 = vector->x1 * multiplicand; - normalized->x2 = vector->x2 * multiplicand; - - return 1; -} - -inline int bgc_vector2_normalize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* normalized) -{ - const double square_modulus = bgc_vector2_get_square_modulus_fp64(vector); - - if (bgc_is_sqare_unit_fp64(square_modulus)) { - normalized->x1 = vector->x1; - normalized->x2 = vector->x2; - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double multiplicand = sqrt(1.0 / square_modulus); - - normalized->x1 = vector->x1 * multiplicand; - normalized->x2 = vector->x2 * multiplicand; - - return 1; -} - // ==================== Add ===================== // inline void bgc_vector2_add_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, BgcVector2FP32* sum) @@ -300,20 +184,6 @@ inline void bgc_vector2_subtract_fp64(const BgcVector2FP64* minuend, const BgcVe difference->x2 = minuend->x2 - subtrahend->x2; } -// ============== Subtract scaled =============== // - -inline void bgc_vector2_subtract_scaled_fp32(const BgcVector2FP32* basic_vector, const BgcVector2FP32* scalable_vector, const float scale, BgcVector2FP32* difference) -{ - difference->x1 = basic_vector->x1 - scalable_vector->x1 * scale; - difference->x2 = basic_vector->x2 - scalable_vector->x2 * scale; -} - -inline void bgc_vector2_subtract_scaled_fp64(const BgcVector2FP64* basic_vector, const BgcVector2FP64* scalable_vector, const double scale, BgcVector2FP64* difference) -{ - difference->x1 = basic_vector->x1 - scalable_vector->x1 * scale; - difference->x2 = basic_vector->x2 - scalable_vector->x2 * scale; -} - // ================== Multiply ================== // inline void bgc_vector2_multiply_fp32(const BgcVector2FP32* multiplicand, const float multiplier, BgcVector2FP32* product) @@ -340,7 +210,7 @@ inline void bgc_vector2_divide_fp64(const BgcVector2FP64* dividend, const double bgc_vector2_multiply_fp64(dividend, 1.0 / divisor, quotient); } -// ================== Average2 ================== // +// ================ Mean of Two ================= // inline void bgc_vector2_get_mean_of_two_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, BgcVector2FP32* mean) { @@ -354,7 +224,7 @@ inline void bgc_vector2_get_mean_of_two_fp64(const BgcVector2FP64* vector1, cons mean->x2 = (vector1->x2 + vector2->x2) * 0.5; } -// ================== Average3 ================== // +// =============== Mean of Three ================ // inline void bgc_vector2_get_mean_of_three_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const BgcVector2FP32* vector3, BgcVector2FP32* mean) { @@ -370,7 +240,7 @@ inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, co // =================== Linear =================== // -inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation) +inline void bgc_vector2_interpolate_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation) { const float counterphase = 1.0f - phase; @@ -378,7 +248,7 @@ inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, interpolation->x2 = vector1->x2 * counterphase + vector2->x2 * phase; } -inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation) +inline void bgc_vector2_interpolate_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation) { const double counterphase = 1.0 - phase; @@ -386,52 +256,108 @@ inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, interpolation->x2 = vector1->x2 * counterphase + vector2->x2 * phase; } -// ================== Minimal =================== // +// ================== Negative ================== // -inline void bgc_vector2_minimize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* minimal) +inline void bgc_vector2_make_opposite_fp32(BgcVector2FP32* vector) { - if (vector->x1 < minimal->x1) { - minimal->x1 = vector->x1; - } - - if (vector->x2 < minimal->x2) { - minimal->x2 = vector->x2; - } + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; } -inline void bgc_vector2_minimize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* minimal) +inline void bgc_vector2_make_opposite_fp64(BgcVector2FP64* vector) { - if (vector->x1 < minimal->x1) { - minimal->x1 = vector->x1; - } - - if (vector->x2 < minimal->x2) { - minimal->x2 = vector->x2; - } + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; } -// ================== Maximal =================== // - -inline void bgc_vector2_maximize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* maximal) +inline void bgc_vector2_get_opposite_fp32(const BgcVector2FP32* vector, BgcVector2FP32* opposite) { - if (vector->x1 > maximal->x1) { - maximal->x1 = vector->x1; - } - - if (vector->x2 > maximal->x2) { - maximal->x2 = vector->x2; - } + opposite->x1 = -vector->x1; + opposite->x2 = -vector->x2; } -inline void bgc_vector2_maximize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* maximal) +inline void bgc_vector2_get_opposite_fp64(const BgcVector2FP64* vector, BgcVector2FP64* opposite) { - if (vector->x1 > maximal->x1) { - maximal->x1 = vector->x1; + opposite->x1 = -vector->x1; + opposite->x2 = -vector->x2; +} + +// ================= Normalize ================== // + +inline int bgc_vector2_normalize_fp32(BgcVector2FP32* vector) +{ + const float square_modulus = bgc_vector2_get_square_modulus_fp32(vector); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + return 1; } - if (vector->x2 > maximal->x2) { - maximal->x2 = vector->x2; + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; } + + const float multiplier = sqrtf(1.0f / square_modulus); + + vector->x1 *= multiplier; + vector->x2 *= multiplier; + + return 1; +} + +inline int bgc_vector2_normalize_fp64(BgcVector2FP64* vector) +{ + const double square_modulus = bgc_vector2_get_square_modulus_fp64(vector); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + vector->x1 *= multiplier; + vector->x2 *= multiplier; + + return 1; +} + +inline int bgc_vector2_get_normalized_fp32(const BgcVector2FP32* vector, BgcVector2FP32* normalized) +{ + const float square_modulus = bgc_vector2_get_square_modulus_fp32(vector); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + bgc_vector2_copy_fp32(vector, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + bgc_vector2_reset_fp32(normalized); + return 0; + } + + bgc_vector2_multiply_fp32(vector, sqrtf(1.0f / square_modulus), normalized); + return 1; +} + +inline int bgc_vector2_get_normalized_fp64(const BgcVector2FP64* vector, BgcVector2FP64* normalized) +{ + const double square_modulus = bgc_vector2_get_square_modulus_fp64(vector); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + bgc_vector2_copy_fp64(vector, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + bgc_vector2_reset_fp64(normalized); + return 0; + } + + bgc_vector2_multiply_fp64(vector, sqrt(1.0 / square_modulus), normalized); + return 1; } // ============= Get Scalar Product ============= // @@ -468,16 +394,16 @@ double bgc_vector2_get_angle_fp64(const BgcVector2FP64* vector1, const BgcVector inline float bgc_vector2_get_square_distance_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2) { - const float dx1 = (vector1->x1 - vector2->x1); - const float dx2 = (vector1->x2 - vector2->x2); + const float dx1 = vector1->x1 - vector2->x1; + const float dx2 = vector1->x2 - vector2->x2; return dx1 * dx1 + dx2 * dx2; } inline double bgc_vector2_get_square_distance_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2) { - const double dx1 = (vector1->x1 - vector2->x1); - const double dx2 = (vector1->x2 - vector2->x2); + const double dx1 = vector1->x1 - vector2->x1; + const double dx2 = vector1->x2 - vector2->x2; return dx1 * dx1 + dx2 * dx2; } @@ -496,14 +422,14 @@ inline double bgc_vector2_get_distance_fp64(const BgcVector2FP64* vector1, const // ============== Are Close Enough ============== // -inline int bgc_vector2_are_close_enough_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float distance) +inline int bgc_vector2_are_close_enough_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float distance_limit) { - return bgc_vector2_get_square_distance_fp32(vector1, vector2) <= distance * distance; + return bgc_vector2_get_square_distance_fp32(vector1, vector2) <= distance_limit * distance_limit; } -inline int bgc_vector2_are_close_enough_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double distance) +inline int bgc_vector2_are_close_enough_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double distance_limit) { - return bgc_vector2_get_square_distance_fp64(vector1, vector2) <= distance * distance; + return bgc_vector2_get_square_distance_fp64(vector1, vector2) <= distance_limit * distance_limit; } // ================== Are Close ================= // @@ -531,7 +457,143 @@ inline int bgc_vector2_are_close_fp64(const BgcVector2FP64* vector1, const BgcVe return square_distance <= BGC_SQUARE_EPSYLON_FP64; } - return square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus2; + return square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus2; +} + + +// ================== Parallel ================== // + +inline int bgc_vector2_are_parallel_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2) +{ + const float square_modulus1 = bgc_vector2_get_square_modulus_fp32(vector1); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + const float square_modulus2 = bgc_vector2_get_square_modulus_fp32(vector2); + + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + const float cross_product = bgc_vector2_get_cross_product_fp32(vector1, vector2); + + return cross_product * cross_product <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; +} + +inline int bgc_vector2_are_parallel_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2) +{ + const double square_modulus1 = bgc_vector2_get_square_modulus_fp64(vector1); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + const double square_modulus2 = bgc_vector2_get_square_modulus_fp64(vector2); + + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + const double cross_product = bgc_vector2_get_cross_product_fp64(vector1, vector2); + + return cross_product * cross_product <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; +} + +// ================= Orthogonal ================= // + +inline int bgc_vector2_are_orthogonal_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2) +{ + const float square_modulus1 = bgc_vector2_get_square_modulus_fp32(vector1); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + const float square_modulus2 = bgc_vector2_get_square_modulus_fp32(vector2); + + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + const float scalar_product = bgc_vector2_get_scalar_product_fp32(vector1, vector2); + + return scalar_product * scalar_product <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; +} + +inline int bgc_vector2_are_orthogonal_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2) +{ + const double square_modulus1 = bgc_vector2_get_square_modulus_fp64(vector1); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + const double square_modulus2 = bgc_vector2_get_square_modulus_fp64(vector2); + + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + const double scalar_product = bgc_vector2_get_scalar_product_fp64(vector1, vector2); + + return scalar_product * scalar_product <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; +} + + +// ================== Attitude ================== // + +inline int bgc_vector2_get_attitude_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2) +{ + const float square_modulus1 = bgc_vector2_get_square_modulus_fp32(vector1); + const float square_modulus2 = bgc_vector2_get_square_modulus_fp32(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return BGC_ATTITUDE_ZERO; + } + + const float square_limit = BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; + + const float scalar_product = bgc_vector2_get_scalar_product_fp32(vector1, vector2); + + if (scalar_product * scalar_product <= square_limit) { + return BGC_ATTITUDE_ORTHOGONAL; + } + + const float cross_product = bgc_vector2_get_cross_product_fp32(vector1, vector2); + + if (cross_product * cross_product > square_limit) { + return BGC_ATTITUDE_ANY; + } + + return scalar_product > 0.0f ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL; +} + +inline int bgc_vector2_get_attitude_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2) +{ + const double square_modulus1 = bgc_vector2_get_square_modulus_fp64(vector1); + const double square_modulus2 = bgc_vector2_get_square_modulus_fp64(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return BGC_ATTITUDE_ZERO; + } + + const double square_limit = BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; + + const double scalar_product = bgc_vector2_get_scalar_product_fp64(vector1, vector2); + + if (scalar_product * scalar_product <= square_limit) { + return BGC_ATTITUDE_ORTHOGONAL; + } + + const double cross_product = bgc_vector2_get_cross_product_fp64(vector1, vector2); + + if (cross_product * cross_product > square_limit) { + return BGC_ATTITUDE_ANY; + } + + return scalar_product > 0.0 ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL; } #endif diff --git a/basic-geometry/vector3.c b/basic-geometry/vector3.c index 287f0f5..95f1bac 100644 --- a/basic-geometry/vector3.c +++ b/basic-geometry/vector3.c @@ -6,9 +6,6 @@ extern inline void bgc_vector3_reset_fp64(BgcVector3FP64* vector); extern inline void bgc_vector3_set_values_fp32(const float x1, const float x2, const float x3, BgcVector3FP32* destination); extern inline void bgc_vector3_set_values_fp64(const double x1, const double x2, const double x3, BgcVector3FP64* destination); -inline int bgc_vector3_get_direction_fp32(const int direction, BgcVector3FP32* vector); -inline int bgc_vector3_get_direction_fp64(const int direction, BgcVector3FP64* vector); - extern inline float bgc_vector3_get_square_modulus_fp32(const BgcVector3FP32* vector); extern inline double bgc_vector3_get_square_modulus_fp64(const BgcVector3FP64* vector); @@ -30,15 +27,6 @@ extern inline void bgc_vector3_convert_fp32_to_fp64(const BgcVector3FP32* source extern inline void bgc_vector3_swap_fp32(BgcVector3FP32* vector1, BgcVector3FP32* vector2); extern inline void bgc_vector3_swap_fp64(BgcVector3FP64* vector1, BgcVector3FP64* vector2); -extern inline void bgc_vector3_reverse_fp32(const BgcVector3FP32* vector, BgcVector3FP32* reverse); -extern inline void bgc_vector3_reverse_fp64(const BgcVector3FP64* vector, BgcVector3FP64* reverse); - -extern inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector); -extern inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector); - -extern inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized); -extern inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized); - extern inline void bgc_vector3_add_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* sum); extern inline void bgc_vector3_add_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* sum); @@ -48,9 +36,6 @@ extern inline void bgc_vector3_add_scaled_fp64(const BgcVector3FP64* basic_vecto extern inline void bgc_vector3_subtract_fp32(const BgcVector3FP32* minuend, const BgcVector3FP32* subtrahend, BgcVector3FP32* difference); extern inline void bgc_vector3_subtract_fp64(const BgcVector3FP64* minuend, const BgcVector3FP64* subtrahend, BgcVector3FP64* difference); -extern inline void bgc_vector3_subtract_scaled_fp32(const BgcVector3FP32* basic_vector, const BgcVector3FP32* scalable_vector, const float scale, BgcVector3FP32* difference); -extern inline void bgc_vector3_subtract_scaled_fp64(const BgcVector3FP64* basic_vector, const BgcVector3FP64* scalable_vector, const double scale, BgcVector3FP64* difference); - extern inline void bgc_vector3_multiply_fp32(const BgcVector3FP32* multiplicand, const float multiplier, BgcVector3FP32* product); extern inline void bgc_vector3_multiply_fp64(const BgcVector3FP64* multiplicand, const double multiplier, BgcVector3FP64* product); @@ -63,14 +48,20 @@ extern inline void bgc_vector3_get_mean_of_two_fp64(const BgcVector3FP64* vector extern inline void bgc_vector3_get_mean_of_three_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* result); extern inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* result); -extern inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation); -extern inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation); +extern inline void bgc_vector3_interpolate_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation); +extern inline void bgc_vector3_interpolate_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation); -extern inline void bgc_vector3_minimize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* minimal); -extern inline void bgc_vector3_minimize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* minimal); +extern inline void bgc_vector3_make_opposite_fp32(BgcVector3FP32* vector); +extern inline void bgc_vector3_make_opposite_fp64(BgcVector3FP64* vector); -extern inline void bgc_vector3_maximize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* maximal); -extern inline void bgc_vector3_maximize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* maximal); +extern inline void bgc_vector3_get_opposite_fp32(const BgcVector3FP32* vector, BgcVector3FP32* opposite); +extern inline void bgc_vector3_get_opposite_fp64(const BgcVector3FP64* vector, BgcVector3FP64* opposite); + +extern inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector); +extern inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector); + +extern inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized); +extern inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized); extern inline float bgc_vector3_get_scalar_product_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2); extern inline double bgc_vector3_get_scalar_product_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2); @@ -96,9 +87,18 @@ extern inline int bgc_vector3_are_close_enough_fp64(const BgcVector3FP64* vector extern inline int bgc_vector3_are_close_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2); extern inline int bgc_vector3_are_close_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2); +extern inline int bgc_vector3_are_parallel_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2); +extern inline int bgc_vector3_are_parallel_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2); + +extern inline int bgc_vector3_are_orthogonal_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2); +extern inline int bgc_vector3_are_orthogonal_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2); + +extern inline int bgc_vector3_get_attitude_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2); +extern inline int bgc_vector3_get_attitude_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2); + // =================== Angle ==================== // -float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcAngleUnitEnum unit) +float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcAngleUnitEnum angle_unit) { const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1); @@ -122,10 +122,10 @@ float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3 const float cross = bgc_vector3_get_modulus_fp32(&cross_product); - return bgc_radians_to_units_fp32(atan2f(cross, scalar), unit); + return bgc_radians_to_units_fp32(atan2f(cross, scalar), angle_unit); } -double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum unit) +double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum angle_unit) { const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1); @@ -149,5 +149,5 @@ double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector const double cross = bgc_vector3_get_modulus_fp64(&cross_product); - return bgc_radians_to_units_fp64(atan2(cross, scalar), unit); + return bgc_radians_to_units_fp64(atan2(cross, scalar), angle_unit); } diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h index 3f828f8..a5e42a7 100644 --- a/basic-geometry/vector3.h +++ b/basic-geometry/vector3.h @@ -50,94 +50,6 @@ inline void bgc_vector3_set_values_fp64(const double x1, const double x2, const destination->x3 = x3; } -// ================= Directions ================= // - -inline int bgc_vector3_get_direction_fp32(const int direction, BgcVector3FP32* vector) -{ - switch (direction) { - case BGC_DIRECTION_X1: - vector->x1 = 1.0f; - vector->x2 = 0.0f; - vector->x3 = 0.0f; - return 1; - - case BGC_DIRECTION_X2: - vector->x1 = 0.0f; - vector->x2 = 1.0f; - vector->x3 = 0.0f; - return 1; - - case BGC_DIRECTION_X3: - vector->x1 = 0.0f; - vector->x2 = 0.0f; - vector->x3 = 1.0f; - return 1; - - case -BGC_DIRECTION_X1: - vector->x1 = -1.0f; - vector->x2 = 0.0f; - vector->x3 = 0.0f; - return 1; - - case -BGC_DIRECTION_X2: - vector->x1 = 0.0f; - vector->x2 = -1.0f; - vector->x3 = 0.0f; - return 1; - - case -BGC_DIRECTION_X3: - vector->x1 = 0.0f; - vector->x2 = 0.0f; - vector->x3 = -1.0f; - return 1; - } - - return 0; -} - -inline int bgc_vector3_get_direction_fp64(const int direction, BgcVector3FP64* vector) -{ - switch (direction) { - case BGC_DIRECTION_X1: - vector->x1 = 1.0; - vector->x2 = 0.0; - vector->x3 = 0.0; - return 1; - - case BGC_DIRECTION_X2: - vector->x1 = 0.0; - vector->x2 = 1.0; - vector->x3 = 0.0; - return 1; - - case BGC_DIRECTION_X3: - vector->x1 = 0.0; - vector->x2 = 0.0; - vector->x3 = 1.0; - return 1; - - case -BGC_DIRECTION_X1: - vector->x1 = -1.0; - vector->x2 = 0.0; - vector->x3 = 0.0; - return 1; - - case -BGC_DIRECTION_X2: - vector->x1 = 0.0; - vector->x2 = -1.0; - vector->x3 = 0.0; - return 1; - - case -BGC_DIRECTION_X3: - vector->x1 = 0.0; - vector->x2 = 0.0; - vector->x3 = -1.0; - return 1; - } - - return 0; -} - // ================== Modulus =================== // inline float bgc_vector3_get_square_modulus_fp32(const BgcVector3FP32* vector) @@ -246,114 +158,6 @@ inline void bgc_vector3_convert_fp32_to_fp64(const BgcVector3FP32* source, BgcVe destination->x3 = source->x3; } -// ================== Reverse =================== // - -inline void bgc_vector3_reverse_fp32(const BgcVector3FP32* vector, BgcVector3FP32* reverse) -{ - reverse->x1 = -vector->x1; - reverse->x2 = -vector->x2; - reverse->x3 = -vector->x3; -} - -inline void bgc_vector3_reverse_fp64(const BgcVector3FP64* vector, BgcVector3FP64* reverse) -{ - reverse->x1 = -vector->x1; - reverse->x2 = -vector->x2; - reverse->x3 = -vector->x3; -} - -// ================= Normalize ================== // - -inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector) -{ - const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector); - - if (bgc_is_sqare_unit_fp32(square_modulus)) { - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float multiplicand = sqrtf(1.0f / square_modulus); - - vector->x1 *= multiplicand; - vector->x2 *= multiplicand; - vector->x3 *= multiplicand; - - return 1; -} - -inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector) -{ - const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector); - - if (bgc_is_sqare_unit_fp64(square_modulus)) { - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double multiplicand = sqrt(1.0 / square_modulus); - - vector->x1 *= multiplicand; - vector->x2 *= multiplicand; - vector->x3 *= multiplicand; - - return 1; -} - -inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized) -{ - const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector); - - if (bgc_is_sqare_unit_fp32(square_modulus)) { - normalized->x1 = vector->x1; - normalized->x2 = vector->x2; - normalized->x3 = vector->x3; - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - return 0; - } - - const float multiplicand = sqrtf(1.0f / square_modulus); - - normalized->x1 = vector->x1 * multiplicand; - normalized->x2 = vector->x2 * multiplicand; - normalized->x3 = vector->x3 * multiplicand; - - return 1; -} - -inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized) -{ - const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector); - - if (bgc_is_sqare_unit_fp64(square_modulus)) { - normalized->x1 = vector->x1; - normalized->x2 = vector->x2; - normalized->x3 = vector->x3; - return 1; - } - - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - return 0; - } - - const double multiplicand = sqrt(1.0 / square_modulus); - - normalized->x1 = vector->x1 * multiplicand; - normalized->x2 = vector->x2 * multiplicand; - normalized->x3 = vector->x3 * multiplicand; - - return 1; -} - // ==================== Add ===================== // inline void bgc_vector3_add_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* sum) @@ -372,18 +176,18 @@ inline void bgc_vector3_add_fp64(const BgcVector3FP64* vector1, const BgcVector3 // ================= Add scaled ================= // -inline void bgc_vector3_add_scaled_fp32(const BgcVector3FP32* basic_vector, const BgcVector3FP32* scalable_vector, const float scale, BgcVector3FP32* result) +inline void bgc_vector3_add_scaled_fp32(const BgcVector3FP32* basic_vector, const BgcVector3FP32* scalable_vector, const float scale, BgcVector3FP32* sum) { - result->x1 = basic_vector->x1 + scalable_vector->x1 * scale; - result->x2 = basic_vector->x2 + scalable_vector->x2 * scale; - result->x3 = basic_vector->x3 + scalable_vector->x3 * scale; + sum->x1 = basic_vector->x1 + scalable_vector->x1 * scale; + sum->x2 = basic_vector->x2 + scalable_vector->x2 * scale; + sum->x3 = basic_vector->x3 + scalable_vector->x3 * scale; } -inline void bgc_vector3_add_scaled_fp64(const BgcVector3FP64* basic_vector, const BgcVector3FP64* scalable_vector, const double scale, BgcVector3FP64* result) +inline void bgc_vector3_add_scaled_fp64(const BgcVector3FP64* basic_vector, const BgcVector3FP64* scalable_vector, const double scale, BgcVector3FP64* sum) { - result->x1 = basic_vector->x1 + scalable_vector->x1 * scale; - result->x2 = basic_vector->x2 + scalable_vector->x2 * scale; - result->x3 = basic_vector->x3 + scalable_vector->x3 * scale; + sum->x1 = basic_vector->x1 + scalable_vector->x1 * scale; + sum->x2 = basic_vector->x2 + scalable_vector->x2 * scale; + sum->x3 = basic_vector->x3 + scalable_vector->x3 * scale; } // ================== Subtract ================== // @@ -402,22 +206,6 @@ inline void bgc_vector3_subtract_fp64(const BgcVector3FP64* minuend, const BgcVe difference->x3 = minuend->x3 - subtrahend->x3; } -// ============== Subtract scaled =============== // - -inline void bgc_vector3_subtract_scaled_fp32(const BgcVector3FP32* basic_vector, const BgcVector3FP32* scalable_vector, const float scale, BgcVector3FP32* result) -{ - result->x1 = basic_vector->x1 - scalable_vector->x1 * scale; - result->x2 = basic_vector->x2 - scalable_vector->x2 * scale; - result->x3 = basic_vector->x3 - scalable_vector->x3 * scale; -} - -inline void bgc_vector3_subtract_scaled_fp64(const BgcVector3FP64* basic_vector, const BgcVector3FP64* scalable_vector, const double scale, BgcVector3FP64* result) -{ - result->x1 = basic_vector->x1 - scalable_vector->x1 * scale; - result->x2 = basic_vector->x2 - scalable_vector->x2 * scale; - result->x3 = basic_vector->x3 - scalable_vector->x3 * scale; -} - // ================== Multiply ================== // inline void bgc_vector3_multiply_fp32(const BgcVector3FP32* multiplicand, const float multiplier, BgcVector3FP32* product) @@ -448,39 +236,39 @@ inline void bgc_vector3_divide_fp64(const BgcVector3FP64* dividend, const double // ================== Average2 ================== // -inline void bgc_vector3_get_mean_of_two_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* result) +inline void bgc_vector3_get_mean_of_two_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* mean) { - result->x1 = (vector1->x1 + vector2->x1) * 0.5f; - result->x2 = (vector1->x2 + vector2->x2) * 0.5f; - result->x3 = (vector1->x3 + vector2->x3) * 0.5f; + mean->x1 = (vector1->x1 + vector2->x1) * 0.5f; + mean->x2 = (vector1->x2 + vector2->x2) * 0.5f; + mean->x3 = (vector1->x3 + vector2->x3) * 0.5f; } -inline void bgc_vector3_get_mean_of_two_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* result) +inline void bgc_vector3_get_mean_of_two_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* mean) { - result->x1 = (vector1->x1 + vector2->x1) * 0.5; - result->x2 = (vector1->x2 + vector2->x2) * 0.5; - result->x3 = (vector1->x3 + vector2->x3) * 0.5; + mean->x1 = (vector1->x1 + vector2->x1) * 0.5; + mean->x2 = (vector1->x2 + vector2->x2) * 0.5; + mean->x3 = (vector1->x3 + vector2->x3) * 0.5; } // ================== Average3 ================== // -inline void bgc_vector3_get_mean_of_three_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* result) +inline void bgc_vector3_get_mean_of_three_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* mean) { - result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP32; - result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP32; - result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP32; + mean->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP32; + mean->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP32; + mean->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP32; } -inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* result) +inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* mean) { - result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP64; - result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP64; - result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP64; + mean->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BGC_ONE_THIRD_FP64; + mean->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BGC_ONE_THIRD_FP64; + mean->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BGC_ONE_THIRD_FP64; } // =================== Linear =================== // -inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation) +inline void bgc_vector3_interpolate_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation) { const float counterphase = 1.0f - phase; @@ -489,7 +277,7 @@ inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, interpolation->x3 = vector1->x3 * counterphase + vector2->x3 * phase; } -inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation) +inline void bgc_vector3_interpolate_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation) { const double counterphase = 1.0 - phase; @@ -498,68 +286,114 @@ inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, interpolation->x3 = vector1->x3 * counterphase + vector2->x3 * phase; } -// ================== Minimal =================== // +// ================== Negative ================== // -inline void bgc_vector3_minimize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* minimal) +inline void bgc_vector3_make_opposite_fp32(BgcVector3FP32* vector) { - if (vector->x1 < minimal->x1) { - minimal->x1 = vector->x1; - } - - if (vector->x2 < minimal->x2) { - minimal->x2 = vector->x2; - } - - if (vector->x3 < minimal->x3) { - minimal->x3 = vector->x3; - } + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; + vector->x3 = -vector->x3; } -inline void bgc_vector3_minimize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* minimal) +inline void bgc_vector3_make_opposite_fp64(BgcVector3FP64* vector) { - if (vector->x1 < minimal->x1) { - minimal->x1 = vector->x1; - } - - if (vector->x2 < minimal->x2) { - minimal->x2 = vector->x2; - } - - if (vector->x3 < minimal->x3) { - minimal->x3 = vector->x3; - } + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; + vector->x3 = -vector->x3; } -// ================== Maximal =================== // - -inline void bgc_vector3_maximize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* maximal) +inline void bgc_vector3_get_opposite_fp32(const BgcVector3FP32* vector, BgcVector3FP32* opposite) { - if (vector->x1 > maximal->x1) { - maximal->x1 = vector->x1; - } - - if (vector->x2 > maximal->x2) { - maximal->x2 = vector->x2; - } - - if (vector->x3 > maximal->x3) { - maximal->x3 = vector->x3; - } + opposite->x1 = -vector->x1; + opposite->x2 = -vector->x2; + opposite->x3 = -vector->x3; } -inline void bgc_vector3_maximize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* maximal) +inline void bgc_vector3_get_opposite_fp64(const BgcVector3FP64* vector, BgcVector3FP64* opposite) { - if (vector->x1 > maximal->x1) { - maximal->x1 = vector->x1; + opposite->x1 = -vector->x1; + opposite->x2 = -vector->x2; + opposite->x3 = -vector->x3; +} + +// ================= Normalize ================== // + +inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector) +{ + const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + return 1; } - if (vector->x2 > maximal->x2) { - maximal->x2 = vector->x2; + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + return 0; } - if (vector->x3 > maximal->x3) { - maximal->x3 = vector->x3; + const float multiplier = sqrtf(1.0f / square_modulus); + + vector->x1 *= multiplier; + vector->x2 *= multiplier; + vector->x3 *= multiplier; + + return 1; +} + +inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector) +{ + const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + return 1; } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + return 0; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + vector->x1 *= multiplier; + vector->x2 *= multiplier; + vector->x3 *= multiplier; + + return 1; +} + +inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized) +{ + const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector); + + if (bgc_is_sqare_unit_fp32(square_modulus)) { + bgc_vector3_copy_fp32(vector, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + bgc_vector3_reset_fp32(normalized); + return 0; + } + + bgc_vector3_multiply_fp32(vector, sqrtf(1.0f / square_modulus), normalized); + return 1; +} + +inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized) +{ + const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector); + + if (bgc_is_sqare_unit_fp64(square_modulus)) { + bgc_vector3_copy_fp64(vector, normalized); + return 1; + } + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + bgc_vector3_reset_fp64(normalized); + return 0; + } + + bgc_vector3_multiply_fp64(vector, sqrt(1.0 / square_modulus), normalized); + return 1; } // =============== Scalar Product =============== // @@ -592,55 +426,55 @@ inline double bgc_vector3_get_triple_product_fp64(const BgcVector3FP64* vector1, // =============== Cross Product ================ // -inline void bgc_vector3_get_cross_product_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* result) +inline void bgc_vector3_get_cross_product_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* product) { const float x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2; const float x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3; const float x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; } -inline void bgc_vector3_get_cross_product_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* result) +inline void bgc_vector3_get_cross_product_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* product) { const double x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2; const double x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3; const double x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; } // ============ Double Cross Product ============ // -inline void bgc_vector3_get_double_cross_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* result) +inline void bgc_vector3_get_double_cross_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* product) { const float ac = bgc_vector3_get_scalar_product_fp32(vector1, vector3); const float ab = bgc_vector3_get_scalar_product_fp32(vector1, vector2); - result->x1 = vector2->x1 * ac - vector3->x1 * ab; - result->x2 = vector2->x2 * ac - vector3->x2 * ab; - result->x3 = vector2->x3 * ac - vector3->x3 * ab; + product->x1 = vector2->x1 * ac - vector3->x1 * ab; + product->x2 = vector2->x2 * ac - vector3->x2 * ab; + product->x3 = vector2->x3 * ac - vector3->x3 * ab; } -inline void bgc_vector3_get_double_cross_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* result) +inline void bgc_vector3_get_double_cross_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* product) { const double ac = bgc_vector3_get_scalar_product_fp64(vector1, vector3); const double ab = bgc_vector3_get_scalar_product_fp64(vector1, vector2); - result->x1 = vector2->x1 * ac - vector3->x1 * ab; - result->x2 = vector2->x2 * ac - vector3->x2 * ab; - result->x3 = vector2->x3 * ac - vector3->x3 * ab; + product->x1 = vector2->x1 * ac - vector3->x1 * ab; + product->x2 = vector2->x2 * ac - vector3->x2 * ab; + product->x3 = vector2->x3 * ac - vector3->x3 * ab; } // =================== Angle ==================== // -float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcAngleUnitEnum unit); +float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcAngleUnitEnum angle_unit); -double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum unit); +double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum angle_unit); // =============== Square Distance ============== // @@ -676,14 +510,14 @@ inline double bgc_vector3_get_distance_fp64(const BgcVector3FP64* vector1, const // ============== Are Close Enough ============== // -inline int bgc_vector3_are_close_enough_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float distance) +inline int bgc_vector3_are_close_enough_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float distance_limit) { - return bgc_vector3_get_square_distance_fp32(vector1, vector2) <= distance * distance; + return bgc_vector3_get_square_distance_fp32(vector1, vector2) <= distance_limit * distance_limit; } -inline int bgc_vector3_are_close_enough_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double distance) +inline int bgc_vector3_are_close_enough_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double distance_limit) { - return bgc_vector3_get_square_distance_fp64(vector1, vector2) <= distance * distance; + return bgc_vector3_get_square_distance_fp64(vector1, vector2) <= distance_limit * distance_limit; } // ================== Are Close ================= // @@ -711,7 +545,129 @@ inline int bgc_vector3_are_close_fp64(const BgcVector3FP64* vector1, const BgcVe return square_distance <= BGC_SQUARE_EPSYLON_FP64; } - return square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP32 * square_modulus2; + return square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 && square_distance <= BGC_SQUARE_EPSYLON_FP64 * square_modulus2; +} + +// ================== Parallel ================== // + +inline int bgc_vector3_are_parallel_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2) +{ + const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1); + const float square_modulus2 = bgc_vector3_get_square_modulus_fp32(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + BgcVector3FP32 product; + + bgc_vector3_get_cross_product_fp32(vector1, vector2, &product); + + return bgc_vector3_get_square_modulus_fp32(&product) <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; +} + +inline int bgc_vector3_are_parallel_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2) +{ + const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1); + const double square_modulus2 = bgc_vector3_get_square_modulus_fp64(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + BgcVector3FP64 product; + + bgc_vector3_get_cross_product_fp64(vector1, vector2, &product); + + return bgc_vector3_get_square_modulus_fp64(&product) <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; +} + +// ================= Orthogonal ================= // + +inline int bgc_vector3_are_orthogonal_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2) +{ + const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1); + const float square_modulus2 = bgc_vector3_get_square_modulus_fp32(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return 1; + } + + const float scalar_product = bgc_vector3_get_scalar_product_fp32(vector1, vector2); + + return scalar_product * scalar_product <= BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; +} + +inline int bgc_vector3_are_orthogonal_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2) +{ + const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1); + const double square_modulus2 = bgc_vector3_get_square_modulus_fp64(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return 1; + } + + const double scalar_product = bgc_vector3_get_scalar_product_fp64(vector1, vector2); + + return scalar_product * scalar_product <= BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; +} + +// ================== Attitude ================== // + +inline int bgc_vector3_get_attitude_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2) +{ + const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1); + const float square_modulus2 = bgc_vector3_get_square_modulus_fp32(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + return BGC_ATTITUDE_ZERO; + } + + const float square_limit = BGC_SQUARE_EPSYLON_FP32 * square_modulus1 * square_modulus2; + + const float scalar_product = bgc_vector3_get_scalar_product_fp32(vector1, vector2); + + if (scalar_product * scalar_product <= square_limit) { + return BGC_ATTITUDE_ORTHOGONAL; + } + + BgcVector3FP32 product; + + bgc_vector3_get_cross_product_fp32(vector1, vector2, &product); + + if (bgc_vector3_get_square_modulus_fp32(&product) > square_limit) { + return BGC_ATTITUDE_ANY; + } + + return scalar_product > 0.0f ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL; +} + +inline int bgc_vector3_get_attitude_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2) +{ + const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1); + const double square_modulus2 = bgc_vector3_get_square_modulus_fp64(vector2); + + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + return BGC_ATTITUDE_ZERO; + } + + const double square_limit = BGC_SQUARE_EPSYLON_FP64 * square_modulus1 * square_modulus2; + + const double scalar_product = bgc_vector3_get_scalar_product_fp64(vector1, vector2); + + if (scalar_product * scalar_product <= square_limit) { + return BGC_ATTITUDE_ORTHOGONAL; + } + + BgcVector3FP64 product; + + bgc_vector3_get_cross_product_fp64(vector1, vector2, &product); + + if (bgc_vector3_get_square_modulus_fp64(&product) > square_limit) { + return BGC_ATTITUDE_ANY; + } + + return scalar_product > 0.0 ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL; } #endif diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c index 2d84a73..5c5b53d 100644 --- a/basic-geometry/versor.c +++ b/basic-geometry/versor.c @@ -28,11 +28,23 @@ extern inline int bgc_versor_is_identity_fp64(const BgcVersorFP64* versor); extern inline void bgc_versor_convert_fp64_to_fp32(const BgcVersorFP64* source, BgcVersorFP32* destination); extern inline void bgc_versor_convert_fp32_to_fp64(const BgcVersorFP32* source, BgcVersorFP64* destination); -extern inline void bgc_versor_shorten_fp32(const BgcVersorFP32* versor, BgcVersorFP32* shortened); -extern inline void bgc_versor_shorten_fp64(const BgcVersorFP64* versor, BgcVersorFP64* shortened); +extern inline void bgc_versor_shorten_fp32(BgcVersorFP32* versor); +extern inline void bgc_versor_shorten_fp64(BgcVersorFP64* versor); -extern inline void bgc_versor_invert_fp32(const BgcVersorFP32* versor, BgcVersorFP32* inverted); -extern inline void bgc_versor_invert_fp64(const BgcVersorFP64* versor, BgcVersorFP64* inverted); +extern inline void bgc_versor_get_shortened_fp32(const BgcVersorFP32* versor, BgcVersorFP32* shortened); +extern inline void bgc_versor_get_shortened_fp64(const BgcVersorFP64* versor, BgcVersorFP64* shortened); + +extern inline void bgc_versor_make_opposite_fp32(BgcVersorFP32* versor); +extern inline void bgc_versor_make_opposite_fp64(BgcVersorFP64* versor); + +extern inline void bgc_versor_get_opposite_fp32(const BgcVersorFP32* versor, BgcVersorFP32* opposite); +extern inline void bgc_versor_get_opposite_fp64(const BgcVersorFP64* versor, BgcVersorFP64* opposite); + +extern inline void bgc_versor_invert_fp32(BgcVersorFP32* versor); +extern inline void bgc_versor_invert_fp64(BgcVersorFP64* versor); + +extern inline void bgc_versor_get_inverse_fp32(const BgcVersorFP32* versor, BgcVersorFP32* inverse); +extern inline void bgc_versor_get_inverse_fp64(const BgcVersorFP64* versor, BgcVersorFP64* inverse); extern inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result); extern inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result); @@ -49,8 +61,8 @@ extern inline void bgc_versor_get_rotation_matrix_fp64(const BgcVersorFP64* vers extern inline void bgc_versor_get_reverse_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix); extern inline void bgc_versor_get_reverse_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix); -extern inline void bgc_versor_get_both_matrixes_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse); -extern inline void bgc_versor_get_both_matrixes_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse); +extern inline void bgc_versor_get_both_matrices_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse); +extern inline void bgc_versor_get_both_matrices_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse); extern inline void bgc_versor_turn_vector_fp32(const BgcVersorFP32* versor, const BgcVector3FP32* vector, BgcVector3FP32* result); extern inline void bgc_versor_turn_vector_fp64(const BgcVersorFP64* versor, const BgcVector3FP64* vector, BgcVector3FP64* result); @@ -63,44 +75,44 @@ extern inline int bgc_versor_are_close_fp64(const BgcVersorFP64* versor1, const // ================= Normalize ================== // -void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin) +void _bgc_versor_normalize_fp32(const float square_modulus, BgcVersorFP32* versor) { // (square_modulus != square_modulus) is true when square_modulus is NaN if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { - twin->s0 = 1.0f; - twin->x1 = 0.0f; - twin->x2 = 0.0f; - twin->x3 = 0.0f; + versor->_s0 = 1.0f; + versor->_x1 = 0.0f; + versor->_x2 = 0.0f; + versor->_x3 = 0.0f; return; } const float multiplier = sqrtf(1.0f / square_modulus); - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + versor->_s0 *= multiplier; + versor->_x1 *= multiplier; + versor->_x2 *= multiplier; + versor->_x3 *= multiplier; } -void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin) +void _bgc_versor_normalize_fp64(const double square_modulus, BgcVersorFP64* versor) { // (square_modulus != square_modulus) is true when square_modulus is NaN if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { - twin->s0 = 1.0; - twin->x1 = 0.0; - twin->x2 = 0.0; - twin->x3 = 0.0; + versor->_s0 = 1.0; + versor->_x1 = 0.0; + versor->_x2 = 0.0; + versor->_x3 = 0.0; return; } const double multiplier = sqrt(1.0 / square_modulus); - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + versor->_s0 *= multiplier; + versor->_x1 *= multiplier; + versor->_x2 *= multiplier; + versor->_x3 *= multiplier; } // ================== Set Turn ================== // @@ -249,7 +261,7 @@ inline int _bgc_versor_validate_basis_fp32(const float primary_square_modulus, c return BGC_FAILED; } - if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP32) { + if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP32 * primary_square_modulus * auxiliary_square_modulus) { //TODO: add error code for: primary_vector and auxiliary_vector are parallel return BGC_FAILED; } @@ -269,7 +281,7 @@ inline int _bgc_versor_validate_basis_fp64(const double primary_square_modulus, return BGC_FAILED; } - if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP64) { + if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP64 * primary_square_modulus * auxiliary_square_modulus) { //TODO: add error code for: primary_vector and auxiliary_vector are parallel return BGC_FAILED; } @@ -423,7 +435,7 @@ int bgc_versor_make_basis_difference_fp64( void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power) { - const float square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3; + const float square_vector = base->_x1 * base->_x1 + base->_x2 * base->_x2 + base->_x3 * base->_x3; if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) { bgc_versor_reset_fp32(power); @@ -432,16 +444,16 @@ void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float expon const float vector_modulus = sqrtf(square_vector); - const float angle = atan2f(vector_modulus, base->s0) * exponent; + const float angle = atan2f(vector_modulus, base->_s0) * exponent; const float multiplier = sinf(angle) / vector_modulus; - bgc_versor_set_values_fp32(cosf(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power); + bgc_versor_set_values_fp32(cosf(angle), base->_x1 * multiplier, base->_x2 * multiplier, base->_x3 * multiplier, power); } void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double exponent, BgcVersorFP64* power) { - const double square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3; + const double square_vector = base->_x1 * base->_x1 + base->_x2 * base->_x2 + base->_x3 * base->_x3; if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) { bgc_versor_reset_fp64(power); @@ -450,21 +462,21 @@ void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double expo const double vector_modulus = sqrt(square_vector); - const double angle = atan2(vector_modulus, base->s0) * exponent; + const double angle = atan2(vector_modulus, base->_s0) * exponent; const double multiplier = sin(angle) / vector_modulus; - bgc_versor_set_values_fp64(cos(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power); + bgc_versor_set_values_fp64(cos(angle), base->_x1 * multiplier, base->_x2 * multiplier, base->_x3 * multiplier, power); } // ============ Sphere Interpolation ============ // void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result) { - const float delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3); - const float delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3); - const float delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1); - const float delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2); + const float delta_s0 = (end->_s0 * start->_s0 + end->_x1 * start->_x1) + (end->_x2 * start->_x2 + end->_x3 * start->_x3); + const float delta_x1 = (end->_x1 * start->_s0 + end->_x3 * start->_x2) - (end->_s0 * start->_x1 + end->_x2 * start->_x3); + const float delta_x2 = (end->_x2 * start->_s0 + end->_x1 * start->_x3) - (end->_s0 * start->_x2 + end->_x3 * start->_x1); + const float delta_x3 = (end->_x3 * start->_s0 + end->_x2 * start->_x1) - (end->_s0 * start->_x3 + end->_x1 * start->_x2); const float square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3; @@ -486,20 +498,20 @@ void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const B // Combining of starting orientation with the turning bgc_versor_set_values_fp32( - (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3), - (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3), - (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1), - (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2), + (turn_s0 * start->_s0 - turn_x1 * start->_x1) - (turn_x2 * start->_x2 + turn_x3 * start->_x3), + (turn_x1 * start->_s0 + turn_s0 * start->_x1) - (turn_x3 * start->_x2 - turn_x2 * start->_x3), + (turn_x2 * start->_s0 + turn_s0 * start->_x2) - (turn_x1 * start->_x3 - turn_x3 * start->_x1), + (turn_x3 * start->_s0 + turn_s0 * start->_x3) - (turn_x2 * start->_x1 - turn_x1 * start->_x2), result ); } void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result) { - const double delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3); - const double delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3); - const double delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1); - const double delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2); + const double delta_s0 = (end->_s0 * start->_s0 + end->_x1 * start->_x1) + (end->_x2 * start->_x2 + end->_x3 * start->_x3); + const double delta_x1 = (end->_x1 * start->_s0 + end->_x3 * start->_x2) - (end->_s0 * start->_x1 + end->_x2 * start->_x3); + const double delta_x2 = (end->_x2 * start->_s0 + end->_x1 * start->_x3) - (end->_s0 * start->_x2 + end->_x3 * start->_x1); + const double delta_x3 = (end->_x3 * start->_s0 + end->_x2 * start->_x1) - (end->_s0 * start->_x3 + end->_x1 * start->_x2); const double square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3; @@ -521,10 +533,10 @@ void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const B // Combining of starting orientation with the turning bgc_versor_set_values_fp64( - (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3), - (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3), - (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1), - (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2), + (turn_s0 * start->_s0 - turn_x1 * start->_x1) - (turn_x2 * start->_x2 + turn_x3 * start->_x3), + (turn_x1 * start->_s0 + turn_s0 * start->_x1) - (turn_x3 * start->_x2 - turn_x2 * start->_x3), + (turn_x2 * start->_s0 + turn_s0 * start->_x2) - (turn_x1 * start->_x3 - turn_x3 * start->_x1), + (turn_x3 * start->_s0 + turn_s0 * start->_x3) - (turn_x2 * start->_x1 - turn_x1 * start->_x2), result ); } @@ -533,7 +545,7 @@ void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const B void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result) { - const float square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; + const float square_modulus = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) { bgc_rotation3_reset_fp32(result); @@ -544,16 +556,16 @@ void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* const float multiplier = 1.0f / vector_modulus; - result->radians = 2.0f * atan2f(vector_modulus, versor->s0); + result->radians = 2.0f * atan2f(vector_modulus, versor->_s0); - result->axis.x1 = versor->x1 * multiplier; - result->axis.x2 = versor->x2 * multiplier; - result->axis.x3 = versor->x3 * multiplier; + result->axis.x1 = versor->_x1 * multiplier; + result->axis.x2 = versor->_x2 * multiplier; + result->axis.x3 = versor->_x3 * multiplier; } void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result) { - const double square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; + const double square_modulus = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) { bgc_rotation3_reset_fp64(result); @@ -564,9 +576,9 @@ void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* const double multiplier = 1.0 / vector_modulus; - result->radians = 2.0 * atan2(vector_modulus, versor->s0); + result->radians = 2.0 * atan2(vector_modulus, versor->_s0); - result->axis.x1 = versor->x1 * multiplier; - result->axis.x2 = versor->x2 * multiplier; - result->axis.x3 = versor->x3 * multiplier; + result->axis.x1 = versor->_x1 * multiplier; + result->axis.x2 = versor->_x2 * multiplier; + result->axis.x3 = versor->_x3 * multiplier; } diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index 6627d93..2c25faa 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -26,23 +26,13 @@ // =================== Types ==================== // typedef struct { - const float s0, x1, x2, x3; + float _s0, _x1, _x2, _x3; } BgcVersorFP32; typedef struct { - const double s0, x1, x2, x3; + double _s0, _x1, _x2, _x3; } BgcVersorFP64; -// ================= Dark Twins ================= // - -typedef struct { - float s0, x1, x2, x3; -} _BgcDarkTwinVersorFP32; - -typedef struct { - double s0, x1, x2, x3; -} _BgcDarkTwinVersorFP64; - // ================= Constants ================== // extern const BgcVersorFP32 BGC_IDLE_VERSOR_FP32; @@ -52,59 +42,51 @@ extern const BgcVersorFP64 BGC_IDLE_VERSOR_FP64; inline void bgc_versor_reset_fp32(BgcVersorFP32* versor) { - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)versor; - - twin->s0 = 1.0f; - twin->x1 = 0.0f; - twin->x2 = 0.0f; - twin->x3 = 0.0f; + versor->_s0 = 1.0f; + versor->_x1 = 0.0f; + versor->_x2 = 0.0f; + versor->_x3 = 0.0f; } inline void bgc_versor_reset_fp64(BgcVersorFP64* versor) { - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)versor; - - twin->s0 = 1.0; - twin->x1 = 0.0; - twin->x2 = 0.0; - twin->x3 = 0.0; + versor->_s0 = 1.0; + versor->_x1 = 0.0; + versor->_x2 = 0.0; + versor->_x3 = 0.0; } // ==================== Set ===================== // -void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin); +void _bgc_versor_normalize_fp32(const float square_modulus, BgcVersorFP32* twin); -void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin); +void _bgc_versor_normalize_fp64(const double square_modulus, BgcVersorFP64* twin); inline void bgc_versor_set_values_fp32(const float s0, const float x1, const float x2, const float x3, BgcVersorFP32* versor) { - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)versor; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; + versor->_s0 = s0; + versor->_x1 = x1; + versor->_x2 = x2; + versor->_x3 = x3; const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); if (!bgc_is_sqare_unit_fp32(square_modulus)) { - _bgc_versor_normalize_fp32(square_modulus, twin); + _bgc_versor_normalize_fp32(square_modulus, versor); } } inline void bgc_versor_set_values_fp64(const double s0, const double x1, const double x2, const double x3, BgcVersorFP64* versor) { - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)versor; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; + versor->_s0 = s0; + versor->_x1 = x1; + versor->_x2 = x2; + versor->_x3 = x3; const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); if (!bgc_is_sqare_unit_fp64(square_modulus)) { - _bgc_versor_normalize_fp64(square_modulus, twin); + _bgc_versor_normalize_fp64(square_modulus, versor); } } @@ -154,80 +136,68 @@ int bgc_versor_make_basis_difference_fp64( inline void bgc_versor_copy_fp32(const BgcVersorFP32* source, BgcVersorFP32* destination) { - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)destination; - - twin->s0 = source->s0; - twin->x1 = source->x1; - twin->x2 = source->x2; - twin->x3 = source->x3; + destination->_s0 = source->_s0; + destination->_x1 = source->_x1; + destination->_x2 = source->_x2; + destination->_x3 = source->_x3; } inline void bgc_versor_copy_fp64(const BgcVersorFP64* source, BgcVersorFP64* destination) { - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)destination; - - twin->s0 = source->s0; - twin->x1 = source->x1; - twin->x2 = source->x2; - twin->x3 = source->x3; + destination->_s0 = source->_s0; + destination->_x1 = source->_x1; + destination->_x2 = source->_x2; + destination->_x3 = source->_x3; } // ==================== Swap ==================== // inline void bgc_versor_swap_fp32(BgcVersorFP32* versor1, BgcVersorFP32* versor2) { - const float s0 = versor1->s0; - const float x1 = versor1->x1; - const float x2 = versor1->x2; - const float x3 = versor1->x3; + const float s0 = versor1->_s0; + const float x1 = versor1->_x1; + const float x2 = versor1->_x2; + const float x3 = versor1->_x3; - _BgcDarkTwinVersorFP32* twin1 = (_BgcDarkTwinVersorFP32*)versor1; + versor1->_s0 = versor2->_s0; + versor1->_x1 = versor2->_x1; + versor1->_x2 = versor2->_x2; + versor1->_x3 = versor2->_x3; - twin1->s0 = versor2->s0; - twin1->x1 = versor2->x1; - twin1->x2 = versor2->x2; - twin1->x3 = versor2->x3; - - _BgcDarkTwinVersorFP32* twin2 = (_BgcDarkTwinVersorFP32*)versor2; - - twin2->s0 = s0; - twin2->x1 = x1; - twin2->x2 = x2; - twin2->x3 = x3; + versor2->_s0 = s0; + versor2->_x1 = x1; + versor2->_x2 = x2; + versor2->_x3 = x3; } inline void bgc_versor_swap_fp64(BgcVersorFP64* versor1, BgcVersorFP64* versor2) { - const double s0 = versor1->s0; - const double x1 = versor1->x1; - const double x2 = versor1->x2; - const double x3 = versor1->x3; + const double s0 = versor1->_s0; + const double x1 = versor1->_x1; + const double x2 = versor1->_x2; + const double x3 = versor1->_x3; - _BgcDarkTwinVersorFP64* twin1 = (_BgcDarkTwinVersorFP64*)versor1; + versor1->_s0 = versor2->_s0; + versor1->_x1 = versor2->_x1; + versor1->_x2 = versor2->_x2; + versor1->_x3 = versor2->_x3; - twin1->s0 = versor2->s0; - twin1->x1 = versor2->x1; - twin1->x2 = versor2->x2; - twin1->x3 = versor2->x3; - - _BgcDarkTwinVersorFP64* twin2 = (_BgcDarkTwinVersorFP64*)versor2; - - twin2->s0 = s0; - twin2->x1 = x1; - twin2->x2 = x2; - twin2->x3 = x3; + versor2->_s0 = s0; + versor2->_x1 = x1; + versor2->_x2 = x2; + versor2->_x3 = x3; } // ================= Comparison ================= // inline int bgc_versor_is_identity_fp32(const BgcVersorFP32* versor) { - return versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3 <= BGC_SQUARE_EPSYLON_FP32; + return versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3 <= BGC_SQUARE_EPSYLON_FP32; } inline int bgc_versor_is_identity_fp64(const BgcVersorFP64* versor) { - return versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3 <= BGC_SQUARE_EPSYLON_FP64; + return versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3 <= BGC_SQUARE_EPSYLON_FP64; } // ================== Convert =================== // @@ -235,10 +205,10 @@ inline int bgc_versor_is_identity_fp64(const BgcVersorFP64* versor) inline void bgc_versor_convert_fp64_to_fp32(const BgcVersorFP64* source, BgcVersorFP32* destination) { bgc_versor_set_values_fp32( - (float)source->s0, - (float)source->x1, - (float)source->x2, - (float)source->x3, + (float)source->_s0, + (float)source->_x1, + (float)source->_x2, + (float)source->_x3, destination ); } @@ -246,70 +216,132 @@ inline void bgc_versor_convert_fp64_to_fp32(const BgcVersorFP64* source, BgcVers inline void bgc_versor_convert_fp32_to_fp64(const BgcVersorFP32* source, BgcVersorFP64* destination) { bgc_versor_set_values_fp64( - source->s0, - source->x1, - source->x2, - source->x3, + source->_s0, + source->_x1, + source->_x2, + source->_x3, destination ); } // ================== Shorten =================== // -inline void bgc_versor_shorten_fp32(const BgcVersorFP32* versor, BgcVersorFP32* shortened) +inline void bgc_versor_shorten_fp32(BgcVersorFP32* versor) { - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)shortened; - - if (versor->s0 >= 0.0f) { - twin->s0 = versor->s0; - twin->x1 = versor->x1; - twin->x2 = versor->x2; - twin->x3 = versor->x3; - return; + if (versor->_s0 < 0.0f) { + versor->_s0 = -versor->_s0; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; } - - twin->s0 = -versor->s0; - twin->x1 = -versor->x1; - twin->x2 = -versor->x2; - twin->x3 = -versor->x3; } -inline void bgc_versor_shorten_fp64(const BgcVersorFP64* versor, BgcVersorFP64* shortened) +inline void bgc_versor_shorten_fp64(BgcVersorFP64* versor) { - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)shortened; + if (versor->_s0 < 0.0) { + versor->_s0 = -versor->_s0; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; + } +} - if (versor->s0 >= 0.0) { - twin->s0 = versor->s0; - twin->x1 = versor->x1; - twin->x2 = versor->x2; - twin->x3 = versor->x3; +inline void bgc_versor_get_shortened_fp32(const BgcVersorFP32* versor, BgcVersorFP32* shortened) +{ + if (versor->_s0 >= 0.0f) { + shortened->_s0 = versor->_s0; + shortened->_x1 = versor->_x1; + shortened->_x2 = versor->_x2; + shortened->_x3 = versor->_x3; return; } - twin->s0 = -versor->s0; - twin->x1 = -versor->x1; - twin->x2 = -versor->x2; - twin->x3 = -versor->x3; + shortened->_s0 = -versor->_s0; + shortened->_x1 = -versor->_x1; + shortened->_x2 = -versor->_x2; + shortened->_x3 = -versor->_x3; +} + +inline void bgc_versor_get_shortened_fp64(const BgcVersorFP64* versor, BgcVersorFP64* shortened) +{ + if (versor->_s0 >= 0.0) { + shortened->_s0 = versor->_s0; + shortened->_x1 = versor->_x1; + shortened->_x2 = versor->_x2; + shortened->_x3 = versor->_x3; + return; + } + + shortened->_s0 = -versor->_s0; + shortened->_x1 = -versor->_x1; + shortened->_x2 = -versor->_x2; + shortened->_x3 = -versor->_x3; +} + +// ================== Negative ================== // + +inline void bgc_versor_make_opposite_fp32(BgcVersorFP32* versor) +{ + versor->_s0 = -versor->_s0; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; +} + +inline void bgc_versor_make_opposite_fp64(BgcVersorFP64* versor) +{ + versor->_s0 = -versor->_s0; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; +} + +inline void bgc_versor_get_opposite_fp32(const BgcVersorFP32* versor, BgcVersorFP32* opposite) +{ + opposite->_s0 = -versor->_s0; + opposite->_x1 = -versor->_x1; + opposite->_x2 = -versor->_x2; + opposite->_x3 = -versor->_x3; +} + +inline void bgc_versor_get_opposite_fp64(const BgcVersorFP64* versor, BgcVersorFP64* opposite) +{ + opposite->_s0 = -versor->_s0; + opposite->_x1 = -versor->_x1; + opposite->_x2 = -versor->_x2; + opposite->_x3 = -versor->_x3; } // =================== Invert =================== // -inline void bgc_versor_invert_fp32(const BgcVersorFP32* versor, BgcVersorFP32* inverted) +inline void bgc_versor_invert_fp32(BgcVersorFP32* versor) { - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)inverted; - twin->s0 = versor->s0; - twin->x1 = -versor->x1; - twin->x2 = -versor->x2; - twin->x3 = -versor->x3; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; } -inline void bgc_versor_invert_fp64(const BgcVersorFP64* versor, BgcVersorFP64* inverted) +inline void bgc_versor_invert_fp64(BgcVersorFP64* versor) { - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)inverted; - twin->s0 = versor->s0; - twin->x1 = -versor->x1; - twin->x2 = -versor->x2; - twin->x3 = -versor->x3; + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; +} + +inline void bgc_versor_get_inverse_fp32(const BgcVersorFP32* versor, BgcVersorFP32* inverse) +{ + inverse->_s0 = versor->_s0; + inverse->_x1 = -versor->_x1; + inverse->_x2 = -versor->_x2; + inverse->_x3 = -versor->_x3; +} + +inline void bgc_versor_get_inverse_fp64(const BgcVersorFP64* versor, BgcVersorFP64* inverse) +{ + inverse->_s0 = versor->_s0; + inverse->_x1 = -versor->_x1; + inverse->_x2 = -versor->_x2; + inverse->_x3 = -versor->_x3; } // =============== Get Exponation =============== // @@ -323,10 +355,10 @@ void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double expo inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result) { bgc_versor_set_values_fp32( - (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3), - (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3), - (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1), - (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2), + (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3), + (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3), + (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1), + (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2), result ); } @@ -334,10 +366,10 @@ inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersor inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result) { bgc_versor_set_values_fp64( - (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3), - (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3), - (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1), - (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2), + (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3), + (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3), + (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1), + (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2), result ); } @@ -346,32 +378,32 @@ inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersor inline void bgc_versor_combine3_fp32(const BgcVersorFP32* third, const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result) { - const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + const float s0 = (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3); + const float x1 = (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3); + const float x2 = (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1); + const float x3 = (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2); bgc_versor_set_values_fp32( - (third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3), - (third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3), - (third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1), - (third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2), + (third->_s0 * s0 - third->_x1 * x1) - (third->_x2 * x2 + third->_x3 * x3), + (third->_x1 * s0 + third->_s0 * x1) - (third->_x3 * x2 - third->_x2 * x3), + (third->_x2 * s0 + third->_s0 * x2) - (third->_x1 * x3 - third->_x3 * x1), + (third->_x3 * s0 + third->_s0 * x3) - (third->_x2 * x1 - third->_x1 * x2), result ); } inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result) { - const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + const double s0 = (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3); + const double x1 = (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3); + const double x2 = (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1); + const double x3 = (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2); bgc_versor_set_values_fp64( - (third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3), - (third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3), - (third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1), - (third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2), + (third->_s0 * s0 - third->_x1 * x1) - (third->_x2 * x2 + third->_x3 * x3), + (third->_x1 * s0 + third->_s0 * x1) - (third->_x3 * x2 - third->_x2 * x3), + (third->_x2 * s0 + third->_s0 * x2) - (third->_x1 * x3 - third->_x3 * x1), + (third->_x3 * s0 + third->_s0 * x3) - (third->_x2 * x1 - third->_x1 * x2), result ); } @@ -381,10 +413,10 @@ inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersor inline void bgc_versor_exclude_fp32(const BgcVersorFP32* base, const BgcVersorFP32* excludant, BgcVersorFP32* difference) { bgc_versor_set_values_fp32( - (base->s0 * excludant->s0 + base->x1 * excludant->x1) + (base->x2 * excludant->x2 + base->x3 * excludant->x3), - (base->x1 * excludant->s0 + base->x3 * excludant->x2) - (base->s0 * excludant->x1 + base->x2 * excludant->x3), - (base->x2 * excludant->s0 + base->x1 * excludant->x3) - (base->s0 * excludant->x2 + base->x3 * excludant->x1), - (base->x3 * excludant->s0 + base->x2 * excludant->x1) - (base->s0 * excludant->x3 + base->x1 * excludant->x2), + (base->_s0 * excludant->_s0 + base->_x1 * excludant->_x1) + (base->_x2 * excludant->_x2 + base->_x3 * excludant->_x3), + (base->_x1 * excludant->_s0 + base->_x3 * excludant->_x2) - (base->_s0 * excludant->_x1 + base->_x2 * excludant->_x3), + (base->_x2 * excludant->_s0 + base->_x1 * excludant->_x3) - (base->_s0 * excludant->_x2 + base->_x3 * excludant->_x1), + (base->_x3 * excludant->_s0 + base->_x2 * excludant->_x1) - (base->_s0 * excludant->_x3 + base->_x1 * excludant->_x2), difference ); } @@ -392,10 +424,10 @@ inline void bgc_versor_exclude_fp32(const BgcVersorFP32* base, const BgcVersorFP inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP64* excludant, BgcVersorFP64* difference) { bgc_versor_set_values_fp64( - (base->s0 * excludant->s0 + base->x1 * excludant->x1) + (base->x2 * excludant->x2 + base->x3 * excludant->x3), - (base->x1 * excludant->s0 + base->x3 * excludant->x2) - (base->s0 * excludant->x1 + base->x2 * excludant->x3), - (base->x2 * excludant->s0 + base->x1 * excludant->x3) - (base->s0 * excludant->x2 + base->x3 * excludant->x1), - (base->x3 * excludant->s0 + base->x2 * excludant->x1) - (base->s0 * excludant->x3 + base->x1 * excludant->x2), + (base->_s0 * excludant->_s0 + base->_x1 * excludant->_x1) + (base->_x2 * excludant->_x2 + base->_x3 * excludant->_x3), + (base->_x1 * excludant->_s0 + base->_x3 * excludant->_x2) - (base->_s0 * excludant->_x1 + base->_x2 * excludant->_x3), + (base->_x2 * excludant->_s0 + base->_x1 * excludant->_x3) - (base->_s0 * excludant->_x2 + base->_x3 * excludant->_x1), + (base->_x3 * excludant->_s0 + base->_x2 * excludant->_x1) - (base->_s0 * excludant->_x3 + base->_x1 * excludant->_x2), difference ); } @@ -416,127 +448,123 @@ void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* inline void bgc_versor_get_rotation_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix) { - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; + const float s0s0 = versor->_s0 * versor->_s0; + const float x1x1 = versor->_x1 * versor->_x1; + const float x2x2 = versor->_x2 * versor->_x2; + const float x3x3 = versor->_x3 * versor->_x3; - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; + const float s0x1 = versor->_s0 * versor->_x1; + const float s0x2 = versor->_s0 * versor->_x2; + const float s0x3 = versor->_s0 * versor->_x3; + const float x1x2 = versor->_x1 * versor->_x2; + const float x1x3 = versor->_x1 * versor->_x3; + const float x2x3 = versor->_x2 * versor->_x3; matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - matrix->r1c2 = x1x2 - s0x3; - matrix->r2c3 = x2x3 - s0x1; - matrix->r3c1 = x1x3 - s0x2; + matrix->r1c2 = 2.0f * (x1x2 - s0x3); + matrix->r2c3 = 2.0f * (x2x3 - s0x1); + matrix->r3c1 = 2.0f * (x1x3 - s0x2); - matrix->r2c1 = x1x2 + s0x3; - matrix->r3c2 = x2x3 + s0x1; - matrix->r1c3 = x1x3 + s0x2; + matrix->r2c1 = 2.0f * (x1x2 + s0x3); + matrix->r3c2 = 2.0f * (x2x3 + s0x1); + matrix->r1c3 = 2.0f * (x1x3 + s0x2); } inline void bgc_versor_get_rotation_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix) { - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; + const double s0s0 = versor->_s0 * versor->_s0; + const double x1x1 = versor->_x1 * versor->_x1; + const double x2x2 = versor->_x2 * versor->_x2; + const double x3x3 = versor->_x3 * versor->_x3; - const double s0x1 = 2.0 * versor->s0 * versor->x1; - const double s0x2 = 2.0 * versor->s0 * versor->x2; - const double s0x3 = 2.0 * versor->s0 * versor->x3; - - const double x1x2 = 2.0 * versor->x1 * versor->x2; - const double x1x3 = 2.0 * versor->x1 * versor->x3; - const double x2x3 = 2.0 * versor->x2 * versor->x3; + const double s0x1 = versor->_s0 * versor->_x1; + const double s0x2 = versor->_s0 * versor->_x2; + const double s0x3 = versor->_s0 * versor->_x3; + const double x1x2 = versor->_x1 * versor->_x2; + const double x1x3 = versor->_x1 * versor->_x3; + const double x2x3 = versor->_x2 * versor->_x3; matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - matrix->r1c2 = x1x2 - s0x3; - matrix->r2c3 = x2x3 - s0x1; - matrix->r3c1 = x1x3 - s0x2; + matrix->r1c2 = 2.0 * (x1x2 - s0x3); + matrix->r2c3 = 2.0 * (x2x3 - s0x1); + matrix->r3c1 = 2.0 * (x1x3 - s0x2); - matrix->r2c1 = x1x2 + s0x3; - matrix->r3c2 = x2x3 + s0x1; - matrix->r1c3 = x1x3 + s0x2; + matrix->r2c1 = 2.0 * (x1x2 + s0x3); + matrix->r3c2 = 2.0 * (x2x3 + s0x1); + matrix->r1c3 = 2.0 * (x1x3 + s0x2); } // ============= Get Reverse Matrix ============= // inline void bgc_versor_get_reverse_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix) { - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; + const float s0s0 = versor->_s0 * versor->_s0; + const float x1x1 = versor->_x1 * versor->_x1; + const float x2x2 = versor->_x2 * versor->_x2; + const float x3x3 = versor->_x3 * versor->_x3; - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; + const float s0x1 = versor->_s0 * versor->_x1; + const float s0x2 = versor->_s0 * versor->_x2; + const float s0x3 = versor->_s0 * versor->_x3; + const float x1x2 = versor->_x1 * versor->_x2; + const float x1x3 = versor->_x1 * versor->_x3; + const float x2x3 = versor->_x2 * versor->_x3; matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - matrix->r1c2 = x1x2 + s0x3; - matrix->r2c3 = x2x3 + s0x1; - matrix->r3c1 = x1x3 + s0x2; + matrix->r1c2 = 2.0f * (x1x2 + s0x3); + matrix->r2c3 = 2.0f * (x2x3 + s0x1); + matrix->r3c1 = 2.0f * (x1x3 + s0x2); - matrix->r2c1 = x1x2 - s0x3; - matrix->r3c2 = x2x3 - s0x1; - matrix->r1c3 = x1x3 - s0x2; + matrix->r2c1 = 2.0f * (x1x2 - s0x3); + matrix->r3c2 = 2.0f * (x2x3 - s0x1); + matrix->r1c3 = 2.0f * (x1x3 - s0x2); } inline void bgc_versor_get_reverse_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix) { - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; + const double s0s0 = versor->_s0 * versor->_s0; + const double x1x1 = versor->_x1 * versor->_x1; + const double x2x2 = versor->_x2 * versor->_x2; + const double x3x3 = versor->_x3 * versor->_x3; - const double s0x1 = 2.0 * versor->s0 * versor->x1; - const double s0x2 = 2.0 * versor->s0 * versor->x2; - const double s0x3 = 2.0 * versor->s0 * versor->x3; - - const double x1x2 = 2.0 * versor->x1 * versor->x2; - const double x1x3 = 2.0 * versor->x1 * versor->x3; - const double x2x3 = 2.0 * versor->x2 * versor->x3; + const double s0x1 = versor->_s0 * versor->_x1; + const double s0x2 = versor->_s0 * versor->_x2; + const double s0x3 = versor->_s0 * versor->_x3; + const double x1x2 = versor->_x1 * versor->_x2; + const double x1x3 = versor->_x1 * versor->_x3; + const double x2x3 = versor->_x2 * versor->_x3; matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - matrix->r1c2 = x1x2 + s0x3; - matrix->r2c3 = x2x3 + s0x1; - matrix->r3c1 = x1x3 + s0x2; + matrix->r1c2 = 2.0 * (x1x2 + s0x3); + matrix->r2c3 = 2.0 * (x2x3 + s0x1); + matrix->r3c1 = 2.0 * (x1x3 + s0x2); - matrix->r2c1 = x1x2 - s0x3; - matrix->r3c2 = x2x3 - s0x1; - matrix->r1c3 = x1x3 - s0x2; + matrix->r2c1 = 2.0 * (x1x2 - s0x3); + matrix->r3c2 = 2.0 * (x2x3 - s0x1); + matrix->r1c3 = 2.0 * (x1x3 - s0x2); } // ============= Get Both Matrixes ============== // -inline void bgc_versor_get_both_matrixes_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse) +inline void bgc_versor_get_both_matrices_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse) { bgc_versor_get_reverse_matrix_fp32(versor, reverse); bgc_matrix3x3_transpose_fp32(reverse, rotation); } -inline void bgc_versor_get_both_matrixes_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse) +inline void bgc_versor_get_both_matrices_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse) { bgc_versor_get_reverse_matrix_fp64(versor, reverse); bgc_matrix3x3_transpose_fp64(reverse, rotation); @@ -546,13 +574,13 @@ inline void bgc_versor_get_both_matrixes_fp64(const BgcVersorFP64* versor, BgcMa inline void bgc_versor_turn_vector_fp32(const BgcVersorFP32* versor, const BgcVector3FP32* vector, BgcVector3FP32* result) { - const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + const float tx1 = 2.0f * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const float tx2 = 2.0f * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const float tx3 = 2.0f * (versor->_x1 * vector->x2 - versor->_x2 * vector->x1); - const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + const float x1 = (vector->x1 + tx1 * versor->_s0) + (versor->_x2 * tx3 - versor->_x3 * tx2); + const float x2 = (vector->x2 + tx2 * versor->_s0) + (versor->_x3 * tx1 - versor->_x1 * tx3); + const float x3 = (vector->x3 + tx3 * versor->_s0) + (versor->_x1 * tx2 - versor->_x2 * tx1); result->x1 = x1; result->x2 = x2; @@ -561,13 +589,13 @@ inline void bgc_versor_turn_vector_fp32(const BgcVersorFP32* versor, const BgcVe inline void bgc_versor_turn_vector_fp64(const BgcVersorFP64* versor, const BgcVector3FP64* vector, BgcVector3FP64* result) { - const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + const double tx1 = 2.0 * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const double tx2 = 2.0 * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const double tx3 = 2.0 * (versor->_x1 * vector->x2 - versor->_x2 * vector->x1); - const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + const double x1 = (vector->x1 + tx1 * versor->_s0) + (versor->_x2 * tx3 - versor->_x3 * tx2); + const double x2 = (vector->x2 + tx2 * versor->_s0) + (versor->_x3 * tx1 - versor->_x1 * tx3); + const double x3 = (vector->x3 + tx3 * versor->_s0) + (versor->_x1 * tx2 - versor->_x2 * tx1); result->x1 = x1; result->x2 = x2; @@ -578,13 +606,13 @@ inline void bgc_versor_turn_vector_fp64(const BgcVersorFP64* versor, const BgcVe inline void bgc_versor_turn_vector_back_fp32(const BgcVersorFP32* versor, const BgcVector3FP32* vector, BgcVector3FP32* result) { - const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + const float tx1 = 2.0f * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const float tx2 = 2.0f * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const float tx3 = 2.0f * (versor->_x1 * vector->x2 - versor->_x2 * vector->x1); - const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + const float x1 = (vector->x1 - tx1 * versor->_s0) + (versor->_x2 * tx3 - versor->_x3 * tx2); + const float x2 = (vector->x2 - tx2 * versor->_s0) + (versor->_x3 * tx1 - versor->_x1 * tx3); + const float x3 = (vector->x3 - tx3 * versor->_s0) + (versor->_x1 * tx2 - versor->_x2 * tx1); result->x1 = x1; result->x2 = x2; @@ -593,13 +621,13 @@ inline void bgc_versor_turn_vector_back_fp32(const BgcVersorFP32* versor, const inline void bgc_versor_turn_vector_back_fp64(const BgcVersorFP64* versor, const BgcVector3FP64* vector, BgcVector3FP64* result) { - const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + const double tx1 = 2.0 * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const double tx2 = 2.0 * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const double tx3 = 2.0 * (versor->_x1 * vector->x2 - versor->_x2 * vector->x1); - const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + const double x1 = (vector->x1 - tx1 * versor->_s0) + (versor->_x2 * tx3 - versor->_x3 * tx2); + const double x2 = (vector->x2 - tx2 * versor->_s0) + (versor->_x3 * tx1 - versor->_x1 * tx3); + const double x3 = (vector->x3 - tx3 * versor->_s0) + (versor->_x1 * tx2 - versor->_x2 * tx1); result->x1 = x1; result->x2 = x2; @@ -610,20 +638,20 @@ inline void bgc_versor_turn_vector_back_fp64(const BgcVersorFP64* versor, const inline int bgc_versor_are_close_fp32(const BgcVersorFP32* versor1, const BgcVersorFP32* versor2) { - const float ds0 = versor1->s0 - versor2->s0; - const float dx1 = versor1->x1 - versor2->x1; - const float dx2 = versor1->x2 - versor2->x2; - const float dx3 = versor1->x3 - versor2->x3; + const float ds0 = versor1->_s0 - versor2->_s0; + const float dx1 = versor1->_x1 - versor2->_x1; + const float dx2 = versor1->_x2 - versor2->_x2; + const float dx3 = versor1->_x3 - versor2->_x3; return (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3) <= BGC_SQUARE_EPSYLON_FP32; } inline int bgc_versor_are_close_fp64(const BgcVersorFP64* versor1, const BgcVersorFP64* versor2) { - const double ds0 = versor1->s0 - versor2->s0; - const double dx1 = versor1->x1 - versor2->x1; - const double dx2 = versor1->x2 - versor2->x2; - const double dx3 = versor1->x3 - versor2->x3; + const double ds0 = versor1->_s0 - versor2->_s0; + const double dx1 = versor1->_x1 - versor2->_x1; + const double dx2 = versor1->_x2 - versor2->_x2; + const double dx3 = versor1->_x3 - versor2->_x3; return (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3) <= BGC_SQUARE_EPSYLON_FP64; }