Изменение функций нормализации, переименование make функий в set values, добавление внутренних restrict функций, гарантирующих оптимизальное выполнение открытых функций, независимо от компилятора
This commit is contained in:
parent
e6ac9023ec
commit
6945c69ef2
20 changed files with 324 additions and 337 deletions
|
|
@ -40,7 +40,7 @@ BGC_FP32_Affine3* _create_bgc_affine3_random_list(int affine_amount)
|
|||
BGC_FP32_Position3 position;
|
||||
|
||||
for (int i = 0; i < affine_amount; i++) {
|
||||
bgc_fp32_turn3_set_raw_values(
|
||||
bgc_fp32_turn3_set_values(
|
||||
&position.turn,
|
||||
get_random_value_fp32(),
|
||||
get_random_value_fp32(),
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ structure_fp32_t* make_structures(const unsigned int amount)
|
|||
const float multiplier = 2.0f / RAND_MAX;
|
||||
|
||||
for (unsigned int i = 0; i < amount; i++) {
|
||||
bgc_fp32_turn3_set_raw_values(
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor1,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
|
|
@ -37,7 +37,7 @@ structure_fp32_t* make_structures(const unsigned int amount)
|
|||
rand() * multiplier - 1.0f
|
||||
);
|
||||
|
||||
bgc_fp32_turn3_set_raw_values(
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor2,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
|
|
@ -127,9 +127,9 @@ int main()
|
|||
int main() {
|
||||
BGC_FP32_Complex complex, exponent, result;
|
||||
|
||||
bgc_fp32_complex_make(0, 1, &complex);
|
||||
bgc_fp32_complex_set_values(0, 1, &complex);
|
||||
|
||||
bgc_fp32_complex_make(4, 0, &exponent);
|
||||
bgc_fp32_complex_set_values(4, 0, &exponent);
|
||||
|
||||
bgc_fp32_complex_get_exponation(&complex, exponent.real, exponent.imaginary, &result);
|
||||
|
||||
|
|
@ -157,11 +157,11 @@ void test_pair_difference_fp32()
|
|||
BGC_FP32_Turn3 turn;
|
||||
|
||||
// No turn
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -169,11 +169,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// Turn around (1, 1, 0) axis on 180 degrees
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 1.0f, 0.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -181,11 +181,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// 180 degree turn
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, -1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, -1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -193,11 +193,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// 90 degree turn around x3 axis
|
||||
bgc_fp32_vector3_make(&initial_main, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 3.1f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 3.1f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, 10.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch,-1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, 10.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch,-1.0f, 0.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -205,11 +205,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// Unorthogonal pairs turn at 90 degrees around x3 axis
|
||||
bgc_fp32_vector3_make(&initial_main, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, -2.0f, 3.1f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, -2.0f, 3.1f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, 10.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, -1.0f, 5.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, 10.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, -1.0f, 5.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -217,10 +217,10 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// Zero vectors
|
||||
bgc_fp32_vector3_make(&initial_main, 0.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 0.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
int code;
|
||||
|
||||
|
|
@ -235,10 +235,10 @@ void test_pair_difference_fp32()
|
|||
}
|
||||
|
||||
// Parallel vectors
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 2.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
code = bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -251,11 +251,11 @@ void test_pair_difference_fp32()
|
|||
}
|
||||
|
||||
// Small angle turn (about 1 degree):
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 0.999848f, 0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, -0.017452f, 0.999848f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.999848f, 0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, -0.017452f, 0.999848f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn , &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -263,11 +263,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// About 179 degrees turn
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, -0.999848f, -0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.017452f, -0.999848f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, -0.999848f, -0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.017452f, -0.999848f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -275,11 +275,11 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// 120 degrees around (-1, -1, 1)
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 1.0f, 0.0f);
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 0.0f, -1.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 0.0f, -1.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -288,10 +288,10 @@ void test_pair_difference_fp32()
|
|||
|
||||
|
||||
// About 1 degree turn difference between initial_main and initial_branch directions
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.999848f, 0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, -1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.999848f, 0.017452f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, 1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, -1.0f, 0.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -299,35 +299,35 @@ void test_pair_difference_fp32()
|
|||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
// About 0.01 degree turn difference between initial_main and initial_branch directions
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 1.0f, 0.000001f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_main, 0.0f, -1.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&final_branch, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 1.0f, 0.000001f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_main, 0.0f, -1.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 1.0f, 0.0f, 0.0f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
printf("\nAbout 0.01 degree turn difference between initial_main and initial_branch directions:\n");
|
||||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 0.999999f, 0.00014142f); // почти (0,1,0), но крошечный z
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.0f, 0.0f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 0.999999f, 0.00014142f); // почти (0,1,0), но крошечный z
|
||||
|
||||
bgc_fp32_vector3_make(&final_main, -0.999999f, 0.0f, 0.00014142f);
|
||||
bgc_fp32_vector3_make(&final_branch, 0.0f, 0.999999f, -0.00014142f);
|
||||
bgc_fp32_vector3_set_values(&final_main, -0.999999f, 0.0f, 0.00014142f);
|
||||
bgc_fp32_vector3_set_values(&final_branch, 0.0f, 0.999999f, -0.00014142f);
|
||||
|
||||
bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
printf("\nNear 180° with tiny branch deviation:\n");
|
||||
print_quaternion_fp32(&turn._versor);
|
||||
|
||||
bgc_fp32_vector3_make(&initial_main, 1.0f, 0.2f, 0.1f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.1f, 1.0f, 0.3f); // почти (0,1,0), но крошечный z
|
||||
bgc_fp32_vector3_set_values(&initial_main, 1.0f, 0.2f, 0.1f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.1f, 1.0f, 0.3f); // почти (0,1,0), но крошечный z
|
||||
|
||||
BGC_FP32_Turn3 known;
|
||||
|
||||
bgc_fp32_turn3_set_rotation(&known, 0.0f, 0.0f, 1.0f, 90.0f, BGC_ANGLE_UNIT_DEGREES);
|
||||
|
||||
bgc_fp32_vector3_make(&initial_main, -0.999999f, 0.0f, 0.00014142f);
|
||||
bgc_fp32_vector3_make(&initial_branch, 0.0f, 0.999999f, -0.00014142f);
|
||||
bgc_fp32_vector3_set_values(&initial_main, -0.999999f, 0.0f, 0.00014142f);
|
||||
bgc_fp32_vector3_set_values(&initial_branch, 0.0f, 0.999999f, -0.00014142f);
|
||||
|
||||
bgc_fp32_turn3_vector(&final_main, &known, &initial_main);
|
||||
bgc_fp32_turn3_vector(&final_branch, &known, &initial_branch);
|
||||
|
|
@ -348,10 +348,10 @@ void test_pair_difference_fp64()
|
|||
BGC_FP64_Turn3 turn;
|
||||
|
||||
// No turn
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -359,10 +359,10 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// Turn around (1, 1, 0) axis on 180 degrees
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 1.0, 0.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -370,11 +370,11 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// 180 degree turn
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&initial_branch, -1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, -1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -382,11 +382,11 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// 90 degree turn around x3 axis
|
||||
bgc_fp64_vector3_make(&initial_main, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 3.1, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 3.1, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, 10.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, -1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, 10.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, -1.0, 0.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -394,11 +394,11 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// Unorthogonal pairs turn at 90 degrees around x3 axis
|
||||
bgc_fp64_vector3_make(&initial_main, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, -2.0, 3.1, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, -2.0, 3.1, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, 10.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, -1.0, 5.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, 10.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, -1.0, 5.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -406,10 +406,10 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// Zero vectors
|
||||
bgc_fp64_vector3_make(&initial_main, 0.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 0.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
int code;
|
||||
|
||||
|
|
@ -424,10 +424,10 @@ void test_pair_difference_fp64()
|
|||
}
|
||||
|
||||
// Parallel vectors
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 2.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
code = bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -440,11 +440,11 @@ void test_pair_difference_fp64()
|
|||
}
|
||||
|
||||
// Small angle turn (about 1 degree):
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&final_main, 0.999848, 0.017452, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, -0.017452, 0.999848, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.999848, 0.017452, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, -0.017452, 0.999848, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -452,11 +452,11 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// About 179 degrees turn
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&final_main, -0.999848, -0.017452, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.017452, -0.999848, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, -0.999848, -0.017452, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.017452, -0.999848, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -464,11 +464,11 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// 120 degrees around (-1, -1, 1)
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.0, 1.0, 0.0);
|
||||
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 0.0, 0.0, -1.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 0.0, 0.0, -1.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -477,10 +477,10 @@ void test_pair_difference_fp64()
|
|||
|
||||
|
||||
// About 1 degree turn difference between initial_main and initial_branch directions
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 0.999848, 0.017452, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, -1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 0.999848, 0.017452, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, 1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, -1.0, 0.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
@ -488,10 +488,10 @@ void test_pair_difference_fp64()
|
|||
print_quaternion_fp64(&turn._versor);
|
||||
|
||||
// About 0.001 degree turn difference between initial_main and initial_branch directions
|
||||
bgc_fp64_vector3_make(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_make(&initial_branch, 1.0, 0.000001, 0.0);
|
||||
bgc_fp64_vector3_make(&final_main, 0.0, -1.0, 0.0);
|
||||
bgc_fp64_vector3_make(&final_branch, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_main, 1.0, 0.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&initial_branch, 1.0, 0.000001, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_main, 0.0, -1.0, 0.0);
|
||||
bgc_fp64_vector3_set_values(&final_branch, 1.0, 0.0, 0.0);
|
||||
|
||||
bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
|
||||
|
||||
|
|
|
|||
|
|
@ -133,17 +133,17 @@ inline int bgc_fp64_affine2_get_inverse(BGC_FP64_Affine2* const inverse, const B
|
|||
inline void bgc_fp32_affine2_combine(BGC_FP32_Affine2* const combination, const BGC_FP32_Affine2* const first, const BGC_FP32_Affine2* const second)
|
||||
{
|
||||
BGC_FP32_Vector2 first_shift;
|
||||
bgc_fp32_matrix2x2_multiply_by_vector2(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp32_matrix2x2_multiply_by_matrix2x2(&combination->distortion, &second->distortion, &first->distortion);
|
||||
_bgc_fp32_restrict_matrix2x2_multiply_by_vector2(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp32_vector2_add(&combination->shift, &second->shift, &first_shift);
|
||||
bgc_fp32_matrix2x2_multiply_by_matrix2x2(&combination->distortion, &second->distortion, &first->distortion);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_affine2_combine(BGC_FP64_Affine2* const combination, const BGC_FP64_Affine2* const first, const BGC_FP64_Affine2* const second)
|
||||
{
|
||||
BGC_FP64_Vector2 first_shift;
|
||||
bgc_fp64_matrix2x2_multiply_by_vector2(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp64_matrix2x2_multiply_by_matrix2x2(&combination->distortion, &second->distortion, &first->distortion);
|
||||
_bgc_fp64_restrict_matrix2x2_multiply_by_vector2(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp64_vector2_add(&combination->shift, &second->shift, &first_shift);
|
||||
bgc_fp64_matrix2x2_multiply_by_matrix2x2(&combination->distortion, &second->distortion, &first->distortion);
|
||||
}
|
||||
|
||||
// =============== Transform Point =============== //
|
||||
|
|
@ -151,14 +151,14 @@ inline void bgc_fp64_affine2_combine(BGC_FP64_Affine2* const combination, const
|
|||
inline void bgc_fp32_affine2_transform_point(BGC_FP32_Vector2* const transformed_point, const BGC_FP32_Affine2* const affine, const BGC_FP32_Vector2* const initial_point)
|
||||
{
|
||||
BGC_FP32_Vector2 distorted;
|
||||
bgc_fp32_matrix2x2_multiply_by_vector2(&distorted, &affine->distortion, initial_point);
|
||||
_bgc_fp32_restrict_matrix2x2_multiply_by_vector2(&distorted, &affine->distortion, initial_point);
|
||||
bgc_fp32_vector2_add(transformed_point, &affine->shift, &distorted);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_affine2_transform_point(BGC_FP64_Vector2* const transformed_point, const BGC_FP64_Affine2* const affine, const BGC_FP64_Vector2* const initial_point)
|
||||
{
|
||||
BGC_FP64_Vector2 distorted;
|
||||
bgc_fp64_matrix2x2_multiply_by_vector2(&distorted, &affine->distortion, initial_point);
|
||||
_bgc_fp64_restrict_matrix2x2_multiply_by_vector2(&distorted, &affine->distortion, initial_point);
|
||||
bgc_fp64_vector2_add(transformed_point, &affine->shift, &distorted);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -132,17 +132,17 @@ inline int bgc_fp64_affine3_get_inverse(BGC_FP64_Affine3* const destination, con
|
|||
inline void bgc_fp32_affine3_combine(BGC_FP32_Affine3* const combination, const BGC_FP32_Affine3* const first, const BGC_FP32_Affine3* const second)
|
||||
{
|
||||
BGC_FP32_Vector3 first_shift;
|
||||
bgc_fp32_matrix3x3_multiply_by_vector3(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp32_matrix3x3_multiply_by_matrix3x3(&combination->distortion, &second->distortion, &first->distortion);
|
||||
_bgc_fp32_restrict_matrix3x3_multiply_by_vector3(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp32_vector3_add(&combination->shift, &first_shift, &second->shift);
|
||||
bgc_fp32_matrix3x3_multiply_by_matrix3x3(&combination->distortion, &second->distortion, &first->distortion);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_affine3_combine(BGC_FP64_Affine3* const combination, const BGC_FP64_Affine3* const first, const BGC_FP64_Affine3* const second)
|
||||
{
|
||||
BGC_FP64_Vector3 first_shift;
|
||||
bgc_fp64_matrix3x3_multiply_by_vector3(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp64_matrix3x3_multiply_by_matrix3x3(&combination->distortion, &second->distortion, &first->distortion);
|
||||
_bgc_fp64_restrict_matrix3x3_multiply_by_vector3(&first_shift, &second->distortion, &first->shift);
|
||||
bgc_fp64_vector3_add(&combination->shift, &first_shift, &second->shift);
|
||||
bgc_fp64_matrix3x3_multiply_by_matrix3x3(&combination->distortion, &second->distortion, &first->distortion);
|
||||
}
|
||||
|
||||
// =============== Transform Point =============== //
|
||||
|
|
@ -150,14 +150,14 @@ inline void bgc_fp64_affine3_combine(BGC_FP64_Affine3* const combination, const
|
|||
inline void bgc_fp32_affine3_transform_point(BGC_FP32_Vector3* const transformed_point, const BGC_FP32_Affine3* const affine, const BGC_FP32_Vector3* const initial_point)
|
||||
{
|
||||
BGC_FP32_Vector3 distorted;
|
||||
bgc_fp32_matrix3x3_multiply_by_vector3(&distorted, &affine->distortion, initial_point);
|
||||
_bgc_fp32_restrict_matrix3x3_multiply_by_vector3(&distorted, &affine->distortion, initial_point);
|
||||
bgc_fp32_vector3_add(transformed_point, &affine->shift, &distorted);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_affine3_transform_point(BGC_FP64_Vector3* const transformed_point, const BGC_FP64_Affine3* const affine, const BGC_FP64_Vector3* const initial_point)
|
||||
{
|
||||
BGC_FP64_Vector3 distorted;
|
||||
bgc_fp64_matrix3x3_multiply_by_vector3(&distorted, &affine->distortion, initial_point);
|
||||
_bgc_fp64_restrict_matrix3x3_multiply_by_vector3(&distorted, &affine->distortion, initial_point);
|
||||
bgc_fp64_vector3_add(transformed_point, &affine->shift, &distorted);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
extern inline void bgc_fp32_complex_reset(BGC_FP32_Complex* const complex);
|
||||
extern inline void bgc_fp64_complex_reset(BGC_FP64_Complex* const complex);
|
||||
|
||||
extern inline void bgc_fp32_complex_make(BGC_FP32_Complex* const complex, const float real, const float imaginary);
|
||||
extern inline void bgc_fp64_complex_make(BGC_FP64_Complex* const complex, const double real, const double imaginary);
|
||||
extern inline void bgc_fp32_complex_set_values(BGC_FP32_Complex* const complex, const float real, const float imaginary);
|
||||
extern inline void bgc_fp64_complex_set_values(BGC_FP64_Complex* const complex, const double real, const double imaginary);
|
||||
|
||||
extern inline float bgc_fp32_complex_get_square_modulus(const BGC_FP32_Complex* const number);
|
||||
extern inline double bgc_fp64_complex_get_square_modulus(const BGC_FP64_Complex* const number);
|
||||
|
|
|
|||
|
|
@ -23,13 +23,13 @@ inline void bgc_fp64_complex_reset(BGC_FP64_Complex* const complex)
|
|||
|
||||
// ==================== Set ===================== //
|
||||
|
||||
inline void bgc_fp32_complex_make(BGC_FP32_Complex* const complex, const float real, const float imaginary)
|
||||
inline void bgc_fp32_complex_set_values(BGC_FP32_Complex* const complex, const float real, const float imaginary)
|
||||
{
|
||||
complex->real = real;
|
||||
complex->imaginary = imaginary;
|
||||
}
|
||||
|
||||
inline void bgc_fp64_complex_make(BGC_FP64_Complex* const complex, const double real, const double imaginary)
|
||||
inline void bgc_fp64_complex_set_values(BGC_FP64_Complex* const complex, const double real, const double imaginary)
|
||||
{
|
||||
complex->real = real;
|
||||
complex->imaginary = imaginary;
|
||||
|
|
|
|||
|
|
@ -75,6 +75,9 @@ extern inline void bgc_fp64_matrix2x2_multiply_by_real(BGC_FP64_Matrix2x2* const
|
|||
extern inline void bgc_fp32_matrix2x2_multiply_by_vector2(BGC_FP32_Vector2* const product, const BGC_FP32_Matrix2x2* const matrix, const BGC_FP32_Vector2* const vector);
|
||||
extern inline void bgc_fp64_matrix2x2_multiply_by_vector2(BGC_FP64_Vector2* const product, const BGC_FP64_Matrix2x2* const matrix, const BGC_FP64_Vector2* const vector);
|
||||
|
||||
extern inline void _bgc_fp32_restrict_matrix2x2_multiply_by_vector2(BGC_FP32_Vector2* restrict const product, const BGC_FP32_Matrix2x2* const matrix, const BGC_FP32_Vector2* const vector);
|
||||
extern inline void _bgc_fp64_restrict_matrix2x2_multiply_by_vector2(BGC_FP64_Vector2* restrict const product, const BGC_FP64_Matrix2x2* const matrix, const BGC_FP64_Vector2* const vector);
|
||||
|
||||
extern inline void bgc_fp32_matrix2x2_multiply_by_matrix2x2(BGC_FP32_Matrix2x2* const product, const BGC_FP32_Matrix2x2* const matrix1, const BGC_FP32_Matrix2x2* const matrix2);
|
||||
extern inline void bgc_fp64_matrix2x2_multiply_by_matrix2x2(BGC_FP64_Matrix2x2* const product, const BGC_FP64_Matrix2x2* const matrix1, const BGC_FP64_Matrix2x2* const matrix2);
|
||||
|
||||
|
|
|
|||
|
|
@ -597,6 +597,20 @@ inline void bgc_fp64_matrix2x2_multiply_by_vector2(BGC_FP64_Vector2* const produ
|
|||
product->x2 = x2;
|
||||
}
|
||||
|
||||
// ======= Restrict Right Vector Product ======== //
|
||||
|
||||
inline void _bgc_fp32_restrict_matrix2x2_multiply_by_vector2(BGC_FP32_Vector2* restrict const product, const BGC_FP32_Matrix2x2* const matrix, const BGC_FP32_Vector2* const vector)
|
||||
{
|
||||
product->x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2;
|
||||
product->x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2;
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_restrict_matrix2x2_multiply_by_vector2(BGC_FP64_Vector2* restrict const product, const BGC_FP64_Matrix2x2* const matrix, const BGC_FP64_Vector2* const vector)
|
||||
{
|
||||
product->x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2;
|
||||
product->x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2;
|
||||
}
|
||||
|
||||
// ========== Matrix Product 2x2 at 2x2 ========= //
|
||||
|
||||
inline void bgc_fp32_matrix2x2_multiply_by_matrix2x2(BGC_FP32_Matrix2x2* const product, const BGC_FP32_Matrix2x2* const matrix1, const BGC_FP32_Matrix2x2* const matrix2)
|
||||
|
|
|
|||
|
|
@ -69,6 +69,9 @@ extern inline void bgc_fp64_matrix3x3_multiply_by_real(BGC_FP64_Matrix3x3* const
|
|||
extern inline void bgc_fp32_matrix3x3_multiply_by_vector3(BGC_FP32_Vector3* const product, const BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Vector3* const vector);
|
||||
extern inline void bgc_fp64_matrix3x3_multiply_by_vector3(BGC_FP64_Vector3* const product, const BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Vector3* const vector);
|
||||
|
||||
extern inline void _bgc_fp32_restrict_matrix3x3_multiply_by_vector3(BGC_FP32_Vector3* restrict const product, const BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Vector3* const vector);
|
||||
extern inline void _bgc_fp64_restrict_matrix3x3_multiply_by_vector3(BGC_FP64_Vector3* restrict const product, const BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Vector3* const vector);
|
||||
|
||||
extern inline void bgc_fp32_matrix3x3_multiply_by_matrix2x3(BGC_FP32_Matrix2x3* const product, const BGC_FP32_Matrix3x3* const matrix1, const BGC_FP32_Matrix2x3* const matrix2);
|
||||
extern inline void bgc_fp64_matrix3x3_multiply_by_matrix2x3(BGC_FP64_Matrix2x3* const product, const BGC_FP64_Matrix3x3* const matrix1, const BGC_FP64_Matrix2x3* const matrix2);
|
||||
|
||||
|
|
|
|||
|
|
@ -840,6 +840,21 @@ inline void bgc_fp64_matrix3x3_multiply_by_vector3(BGC_FP64_Vector3* const produ
|
|||
product->x3 = x3;
|
||||
}
|
||||
|
||||
// ======= Restrict Right Vector Product ======== //
|
||||
|
||||
inline void _bgc_fp32_restrict_matrix3x3_multiply_by_vector3(BGC_FP32_Vector3* restrict const product, const BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Vector3* const vector)
|
||||
{
|
||||
product->x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3;
|
||||
product->x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3;
|
||||
product->x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3;
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_restrict_matrix3x3_multiply_by_vector3(BGC_FP64_Vector3* restrict const product, const BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Vector3* const vector)
|
||||
{
|
||||
product->x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3;
|
||||
product->x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3;
|
||||
product->x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3;
|
||||
}
|
||||
|
||||
// ========== Matrix Product 3x3 at 2x3 ========= //
|
||||
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@ extern inline void bgc_fp64_quaternion_reset(BGC_FP64_Quaternion* const quaterni
|
|||
extern inline void bgc_fp32_quaternion_make_unit(BGC_FP32_Quaternion* const quaternion);
|
||||
extern inline void bgc_fp64_quaternion_make_unit(BGC_FP64_Quaternion* const quaternion);
|
||||
|
||||
extern inline void bgc_fp32_quaternion_make(BGC_FP32_Quaternion* const quaternion, const float s0, const float x1, const float x2, const float x3);
|
||||
extern inline void bgc_fp64_quaternion_make(BGC_FP64_Quaternion* const quaternion, const double s0, const double x1, const double x2, const double x3);
|
||||
extern inline void bgc_fp32_quaternion_set_values(BGC_FP32_Quaternion* const quaternion, const float s0, const float x1, const float x2, const float x3);
|
||||
extern inline void bgc_fp64_quaternion_set_values(BGC_FP64_Quaternion* const quaternion, const double s0, const double x1, const double x2, const double x3);
|
||||
|
||||
extern inline float bgc_fp32_quaternion_get_square_magnitude(const BGC_FP32_Quaternion* const quaternion);
|
||||
extern inline double bgc_fp64_quaternion_get_square_magnitude(const BGC_FP64_Quaternion* const quaternion);
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ inline void bgc_fp64_quaternion_make_unit(BGC_FP64_Quaternion* const quaternion)
|
|||
|
||||
// ==================== Set ===================== //
|
||||
|
||||
inline void bgc_fp32_quaternion_make(BGC_FP32_Quaternion* const quaternion, const float s0, const float x1, const float x2, const float x3)
|
||||
inline void bgc_fp32_quaternion_set_values(BGC_FP32_Quaternion* const quaternion, const float s0, const float x1, const float x2, const float x3)
|
||||
{
|
||||
quaternion->s0 = s0;
|
||||
quaternion->x1 = x1;
|
||||
|
|
@ -54,7 +54,7 @@ inline void bgc_fp32_quaternion_make(BGC_FP32_Quaternion* const quaternion, cons
|
|||
quaternion->x3 = x3;
|
||||
}
|
||||
|
||||
inline void bgc_fp64_quaternion_make(BGC_FP64_Quaternion* const quaternion, const double s0, const double x1, const double x2, const double x3)
|
||||
inline void bgc_fp64_quaternion_set_values(BGC_FP64_Quaternion* const quaternion, const double s0, const double x1, const double x2, const double x3)
|
||||
{
|
||||
quaternion->s0 = s0;
|
||||
quaternion->x1 = x1;
|
||||
|
|
|
|||
|
|
@ -7,11 +7,14 @@ const BGC_FP64_Turn2 BGC_FP64_IDLE_TURN2 = { 1.0, 0.0 };
|
|||
extern inline void bgc_fp32_turn2_reset(BGC_FP32_Turn2* const turn);
|
||||
extern inline void bgc_fp64_turn2_reset(BGC_FP64_Turn2* const turn);
|
||||
|
||||
extern inline void bgc_fp32_turn2_make(BGC_FP32_Turn2* const turn, const float x1, const float x2);
|
||||
extern inline void bgc_fp64_turn2_make(BGC_FP64_Turn2* const turn, const double x1, const double x2);
|
||||
extern void _bgc_fp32_turn2_normalize(BGC_FP32_Turn2* const turn);
|
||||
extern void _bgc_fp64_turn2_normalize(BGC_FP64_Turn2* const turn);
|
||||
|
||||
extern inline void bgc_fp32_turn2_make_for_angle(BGC_FP32_Turn2* const turn, const float angle, const int angle_unit);
|
||||
extern inline void bgc_fp64_turn2_make_for_angle(BGC_FP64_Turn2* const turn, const double angle, const int angle_unit);
|
||||
extern inline void bgc_fp32_turn2_set_values(BGC_FP32_Turn2* const turn, const float x1, const float x2);
|
||||
extern inline void bgc_fp64_turn2_set_values(BGC_FP64_Turn2* const turn, const double x1, const double x2);
|
||||
|
||||
extern inline void bgc_fp32_turn2_set_angle(BGC_FP32_Turn2* const turn, const float angle, const int angle_unit);
|
||||
extern inline void bgc_fp64_turn2_set_angle(BGC_FP64_Turn2* const turn, const double angle, const int angle_unit);
|
||||
|
||||
extern inline int bgc_fp32_turn2_is_idle(const BGC_FP32_Turn2* const turn);
|
||||
extern inline int bgc_fp64_turn2_is_idle(const BGC_FP64_Turn2* const turn);
|
||||
|
|
@ -57,35 +60,3 @@ extern inline void bgc_fp64_turn2_vector_back(BGC_FP64_Vector2* const turned_vec
|
|||
|
||||
extern inline int bgc_fp32_turn2_are_close(const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2);
|
||||
extern inline int bgc_fp64_turn2_are_close(const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2);
|
||||
|
||||
void _bgc_fp32_turn2_normalize(BGC_FP32_Turn2* const turn)
|
||||
{
|
||||
const float square_modulus = turn->_cos * turn->_cos + turn->_sin * turn->_sin;
|
||||
|
||||
if (square_modulus <= BGC_FP32_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
turn->_cos = 1.0f;
|
||||
turn->_sin = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
const float multiplier = sqrtf(1.0f / square_modulus);
|
||||
|
||||
turn->_cos *= multiplier;
|
||||
turn->_sin *= multiplier;
|
||||
}
|
||||
|
||||
void _bgc_fp64_turn2_normalize(BGC_FP64_Turn2* const turn)
|
||||
{
|
||||
const double square_modulus = turn->_cos * turn->_cos + turn->_sin * turn->_sin;
|
||||
|
||||
if (square_modulus <= BGC_FP64_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
turn->_cos = 1.0;
|
||||
turn->_sin = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
const double multiplier = sqrt(1.0 / square_modulus);
|
||||
|
||||
turn->_cos *= multiplier;
|
||||
turn->_sin *= multiplier;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,9 +26,51 @@ inline void bgc_fp64_turn2_reset(BGC_FP64_Turn2* const turn)
|
|||
turn->_sin = 0.0;
|
||||
}
|
||||
|
||||
// ================= Normalize ================== //
|
||||
|
||||
inline void _bgc_fp32_turn2_normalize(BGC_FP32_Turn2* const turn)
|
||||
{
|
||||
const float square_modulus = turn->_cos * turn->_cos + turn->_sin * turn->_sin;
|
||||
|
||||
if (bgc_fp32_is_square_unit(square_modulus)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_modulus <= BGC_FP32_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
turn->_cos = 1.0f;
|
||||
turn->_sin = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
const float multiplier = sqrtf(1.0f / square_modulus);
|
||||
|
||||
turn->_cos *= multiplier;
|
||||
turn->_sin *= multiplier;
|
||||
}
|
||||
|
||||
inline void _bgc_fp64_turn2_normalize(BGC_FP64_Turn2* const turn)
|
||||
{
|
||||
const double square_modulus = turn->_cos * turn->_cos + turn->_sin * turn->_sin;
|
||||
|
||||
if (bgc_fp64_is_square_unit(square_modulus)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_modulus <= BGC_FP64_SQUARE_EPSILON || isnan(square_modulus)) {
|
||||
turn->_cos = 1.0;
|
||||
turn->_sin = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
const double multiplier = sqrt(1.0 / square_modulus);
|
||||
|
||||
turn->_cos *= multiplier;
|
||||
turn->_sin *= multiplier;
|
||||
}
|
||||
|
||||
// ================== Set Turn ================== //
|
||||
|
||||
inline void bgc_fp32_turn2_make_for_angle(BGC_FP32_Turn2* const turn, const float angle, const int angle_unit)
|
||||
inline void bgc_fp32_turn2_set_angle(BGC_FP32_Turn2* const turn, const float angle, const int angle_unit)
|
||||
{
|
||||
const float radians = bgc_fp32_angle_to_radians(angle, angle_unit);
|
||||
|
||||
|
|
@ -36,7 +78,7 @@ inline void bgc_fp32_turn2_make_for_angle(BGC_FP32_Turn2* const turn, const floa
|
|||
turn->_sin = sinf(radians);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn2_make_for_angle(BGC_FP64_Turn2* const turn, const double angle, const int angle_unit)
|
||||
inline void bgc_fp64_turn2_set_angle(BGC_FP64_Turn2* const turn, const double angle, const int angle_unit)
|
||||
{
|
||||
const double radians = bgc_fp64_angle_to_radians(angle, angle_unit);
|
||||
|
||||
|
|
@ -59,32 +101,20 @@ inline int bgc_fp64_turn2_is_idle(const BGC_FP64_Turn2* const turn)
|
|||
|
||||
// ==================== Set ===================== //
|
||||
|
||||
void _bgc_fp32_turn2_normalize(BGC_FP32_Turn2* const twin);
|
||||
|
||||
void _bgc_fp64_turn2_normalize(BGC_FP64_Turn2* const twin);
|
||||
|
||||
inline void bgc_fp32_turn2_make(BGC_FP32_Turn2* const turn, const float x1, const float x2)
|
||||
inline void bgc_fp32_turn2_set_values(BGC_FP32_Turn2* const turn, const float x1, const float x2)
|
||||
{
|
||||
const float square_modulus = x1 * x1 + x2 * x2;
|
||||
|
||||
turn->_cos = x1;
|
||||
turn->_sin = x2;
|
||||
|
||||
if (!bgc_fp32_is_square_unit(square_modulus)) {
|
||||
_bgc_fp32_turn2_normalize(turn);
|
||||
}
|
||||
_bgc_fp32_turn2_normalize(turn);
|
||||
}
|
||||
|
||||