Добавление функций определения поворотов (versor) между направлениями и базисами
This commit is contained in:
parent
e6a94ab8d9
commit
2a4d5522d3
12 changed files with 784 additions and 287 deletions
|
|
@ -53,12 +53,12 @@ structure_fp32_t* make_structures(const unsigned int amount)
|
|||
|
||||
void print_versor_fp32(const BgcVersorFP32* versor)
|
||||
{
|
||||
printf("Versor (%f, %f, %f, %f)\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 (%lf, %lf, %lf, %lf)\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)
|
||||
|
|
@ -149,7 +149,328 @@ int main() {
|
|||
}
|
||||
*/
|
||||
|
||||
int main() {
|
||||
void test_basis_difference_fp32()
|
||||
{
|
||||
|
||||
BgcVector3FP32 initial_primary, initial_auxiliary;
|
||||
BgcVector3FP32 final_primary, final_auxiliary;
|
||||
BgcVersorFP32 turn;
|
||||
|
||||
// No turn
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nNo turn:\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// Turn around (1, 1, 0) axis on 180 degrees
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nTurn around (1, 1, 0) axis on 180 degrees:\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// 180 degree turn
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(-1.0f, 0.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n180 degree turn around (0, 1, 0):\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// 90 degree turn around x3 axis
|
||||
bgc_vector3_set_values_fp32(2.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 3.1f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(0.0f, 10.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(-1.0f, 0.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n90 degree turn around (0, 0, 1):\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// Unorthogonal pairs turn at 90 degrees around x3 axis
|
||||
bgc_vector3_set_values_fp32(2.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(-2.0f, 3.1f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(0.0f, 10.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(-1.0f, 5.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nUnorthogonal pairs turn at 90 degrees around (0, 0, 1):\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// Zero vectors
|
||||
bgc_vector3_set_values_fp32(0.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
int code;
|
||||
|
||||
code = bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
if (code >= 0) {
|
||||
printf("\nZero vectors: this cannot be!\n");
|
||||
print_versor_fp32(&turn);
|
||||
}
|
||||
else {
|
||||
printf("\nZero vector validation works fine\n");
|
||||
}
|
||||
|
||||
// Parallel vectors
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(2.0f, 0.0f, 0.0f, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
code = bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
if (code >= 0) {
|
||||
printf("\nParallel vectors: this cannot be!\n");
|
||||
print_versor_fp32(&turn);
|
||||
}
|
||||
else {
|
||||
printf("\nParallelism validation works fine\n");
|
||||
}
|
||||
|
||||
// Small angle turn (about 1 degree):
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(0.999848f, 0.017452f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(-0.017452f, 0.999848f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nSmall angle turn (about 1 degree):\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// About 179 degrees turn
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(-0.999848f, -0.017452f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.017452f, -0.999848f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 179 degrees turn:\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// 120 degrees around (-1, -1, 1)
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 0.0f, -1.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n120 degees turn:\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
|
||||
// About 1 degree turn difference between initial_primary and initial_auxiliary directions
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(0.999848f, 0.017452f, 0.0f, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp32(0.0f, 1.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(-1.0f, 0.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 1 degree turn difference between initial_primary and initial_auxiliary directions:\n");
|
||||
print_versor_fp32(&turn);
|
||||
|
||||
// About 0.01 degree turn difference between initial_primary and initial_auxiliary directions
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &initial_primary);
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.000001f, 0.0f, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp32(0.0f, -1.0f, 0.0f, &final_primary);
|
||||
bgc_vector3_set_values_fp32(1.0f, 0.0f, 0.0f, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp32(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 0.01 degree turn difference between initial_primary and initial_auxiliary directions:\n");
|
||||
print_versor_fp32(&turn);
|
||||
}
|
||||
|
||||
void test_basis_difference_fp64()
|
||||
{
|
||||
|
||||
BgcVector3FP64 initial_primary, initial_auxiliary;
|
||||
BgcVector3FP64 final_primary, final_auxiliary;
|
||||
BgcVersorFP64 turn;
|
||||
|
||||
// No turn
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nNo turn:\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// Turn around (1, 1, 0) axis on 180 degrees
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nTurn around (1, 1, 0) axis on 180 degrees:\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// 180 degree turn
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(-1.0, 0.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n180 degree turn around (0, 1, 0):\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// 90 degree turn around x3 axis
|
||||
bgc_vector3_set_values_fp64(2.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 3.1, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(0.0, 10.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(-1.0, 0.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n90 degree turn around (0, 0, 1):\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// Unorthogonal pairs turn at 90 degrees around x3 axis
|
||||
bgc_vector3_set_values_fp64(2.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(-2.0, 3.1, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(0.0, 10.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(-1.0, 5.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nUnorthogonal pairs turn at 90 degrees around (0, 0, 1):\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// Zero vectors
|
||||
bgc_vector3_set_values_fp64(0.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_auxiliary);
|
||||
|
||||
int code;
|
||||
|
||||
code = bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
if (code >= 0) {
|
||||
printf("\nZero vectors: this cannot be!\n");
|
||||
print_versor_fp64(&turn);
|
||||
}
|
||||
else {
|
||||
printf("\nZero vector validation works fine\n");
|
||||
}
|
||||
|
||||
// Parallel vectors
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(2.0, 0.0, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_auxiliary);
|
||||
|
||||
code = bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
if (code >= 0) {
|
||||
printf("\nParallel vectors: this cannot be!\n");
|
||||
print_versor_fp64(&turn);
|
||||
}
|
||||
else {
|
||||
printf("\nParallelism validation works fine\n");
|
||||
}
|
||||
|
||||
// Small angle turn (about 1 degree):
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(0.999848, 0.017452, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(-0.017452, 0.999848, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nSmall angle turn (about 1 degree):\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// About 179 degrees turn
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(-0.999848, -0.017452, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.017452, -0.999848, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 179 degrees turn:\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// 120 degrees around (-1, -1, 1)
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &initial_auxiliary);
|
||||
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(0.0, 0.0, -1.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\n120 degees turn:\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
|
||||
// About 1 degree turn difference between initial_primary and initial_auxiliary directions
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(0.999848, 0.017452, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(0.0, 1.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(-1.0, 0.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 1 degree turn difference between initial_primary and initial_auxiliary directions:\n");
|
||||
print_versor_fp64(&turn);
|
||||
|
||||
// About 0.001 degree turn difference between initial_primary and initial_auxiliary directions
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &initial_primary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.000001, 0.0, &initial_auxiliary);
|
||||
bgc_vector3_set_values_fp64(0.0, -1.0, 0.0, &final_primary);
|
||||
bgc_vector3_set_values_fp64(1.0, 0.0, 0.0, &final_auxiliary);
|
||||
|
||||
bgc_versor_make_basis_difference_fp64(&initial_primary, &initial_auxiliary, &final_primary, &final_auxiliary, &turn);
|
||||
|
||||
printf("\nAbout 0.01 degree turn difference between initial_primary and initial_auxiliary directions:\n");
|
||||
print_versor_fp64(&turn);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
//BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f };
|
||||
//BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f };
|
||||
/*
|
||||
|
|
@ -162,12 +483,7 @@ int main() {
|
|||
printf("Result: %0.12f, %0.12f, %0.12f, %0.12f\n", result.s0, result.x1, result.x2, result.x3);
|
||||
*/
|
||||
|
||||
BgcVersorFP64 start = { 1.0, 0.0, 0.0, 0.0 };
|
||||
BgcVersorFP64 end = { -0.707, 0.707, 0.0, 0.0 };
|
||||
BgcVersorFP64 result;
|
||||
BgcSlerpFP64 slerp;
|
||||
bgc_slerp_make_full_fp64(&start, &end, &slerp);
|
||||
bgc_slerp_get_turn_for_phase_fp64(&slerp, 0.5f, &result);
|
||||
printf("Result: %0.15f, %0.15f, %0.15f, %0.15f\n", result.s0, result.x1, result.x2, result.x3);
|
||||
test_basis_difference_fp64();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
<ItemGroup>
|
||||
<ClInclude Include="angle.h" />
|
||||
<ClInclude Include="basic-geometry.h" />
|
||||
<ClInclude Include="basis3.h" />
|
||||
<ClInclude Include="complex.h" />
|
||||
<ClInclude Include="cotes-number.h" />
|
||||
<ClInclude Include="matrix2x2.h" />
|
||||
|
|
@ -41,7 +40,6 @@
|
|||
<ClCompile Include="angle.c" />
|
||||
<ClInclude Include="complex.c" />
|
||||
<ClInclude Include="cotes-number.c" />
|
||||
<ClCompile Include="basis3.c" />
|
||||
<ClCompile Include="utilities.c" />
|
||||
<ClCompile Include="matrix2x2.c" />
|
||||
<ClCompile Include="matrix2x3.c" />
|
||||
|
|
|
|||
|
|
@ -69,9 +69,6 @@
|
|||
<ClInclude Include="slerp.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="basis3.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="angle.c">
|
||||
|
|
@ -113,8 +110,5 @@
|
|||
<ClCompile Include="slerp.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="basis3.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -1,232 +0,0 @@
|
|||
#include <math.h>
|
||||
#include "./basis3.h"
|
||||
|
||||
typedef struct {
|
||||
int e1, e2, e3;
|
||||
} _BgcBasis3State;
|
||||
|
||||
static inline void _bgc_basis3_state_reset(_BgcBasis3State* state)
|
||||
{
|
||||
state->e1 = 0;
|
||||
state->e2 = 0;
|
||||
state->e3 = 0;
|
||||
}
|
||||
|
||||
static inline int _bgc_basis3_validate_directions(const int primary_direction, const int auxiliary_direction)
|
||||
{
|
||||
if (!bgc_is_correct_direction(primary_direction)) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_DIRECTION_UNKNOWN;
|
||||
}
|
||||
|
||||
if (!bgc_is_correct_direction(auxiliary_direction)) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_DIRECTION_UNKNOWN;
|
||||
}
|
||||
|
||||
if (primary_direction == auxiliary_direction || primary_direction == -auxiliary_direction) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
static inline void _bgc_basis3_load_axis_fp32(const int direction, const BgcVector3FP32* axis, _BgcBasis3State* state, BgcBasis3FP32* basis)
|
||||
{
|
||||
switch (direction) {
|
||||
case BGC_DIRECTION_X1:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X1:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X2:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X2:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X3:
|
||||
bgc_vector3_copy_fp32(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X3:
|
||||
bgc_vector3_reverse_fp32(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int bgc_basis3_make_from_directions_fp32(
|
||||
const int primary_direction, const BgcVector3FP32* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP32* auxiliary_vector,
|
||||
BgcBasis3FP32* basis
|
||||
) {
|
||||
const int direstion_validation_code = _bgc_basis3_validate_directions(primary_direction, auxiliary_direction);
|
||||
|
||||
if (direstion_validation_code != BGC_SUCCESS) {
|
||||
return direstion_validation_code;
|
||||
}
|
||||
|
||||
const float primary_square_modulus = bgc_vector3_get_square_modulus_fp32(primary_vector);
|
||||
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
const float auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(auxiliary_vector);
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
BgcVector3FP32 orthogonal;
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(primary_vector, auxiliary_vector, &orthogonal);
|
||||
|
||||
const float orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP32 * primary_square_modulus * auxiliary_square_modulus) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
BgcVector3FP32 primary_axis, auxiliary_axis;
|
||||
|
||||
bgc_vector3_multiply_fp32(primary_vector, sqrtf(1.0f / primary_square_modulus), &primary_axis);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp32(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp32(auxiliary_vector, &primary_axis), &auxiliary_axis);
|
||||
|
||||
bgc_vector3_multiply_fp32(&auxiliary_axis, sqrtf(1.0f / bgc_vector3_get_square_modulus_fp32(&auxiliary_axis)), &auxiliary_axis);
|
||||
|
||||
_BgcBasis3State state;
|
||||
|
||||
_bgc_basis3_state_reset(&state);
|
||||
|
||||
_bgc_basis3_load_axis_fp32(primary_direction, &primary_axis, &state, basis);
|
||||
_bgc_basis3_load_axis_fp32(auxiliary_direction, &auxiliary_axis, &state, basis);
|
||||
|
||||
if (!state.e1) {
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e2, &basis->e3, &basis->e1);
|
||||
bgc_vector3_normalize_fp32(&basis->e1);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
if (!state.e2) {
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e3, &basis->e1, &basis->e2);
|
||||
bgc_vector3_normalize_fp32(&basis->e2);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(&basis->e1, &basis->e2, &basis->e3);
|
||||
bgc_vector3_normalize_fp32(&basis->e3);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
static inline void _bgc_basis3_load_axis_fp64(const int direction, const BgcVector3FP64* axis, _BgcBasis3State* state, BgcBasis3FP64* basis)
|
||||
{
|
||||
switch (direction) {
|
||||
case BGC_DIRECTION_X1:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X1:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e1);
|
||||
state->e1 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X2:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X2:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e2);
|
||||
state->e2 = 1;
|
||||
break;
|
||||
|
||||
case BGC_DIRECTION_X3:
|
||||
bgc_vector3_copy_fp64(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
|
||||
case -BGC_DIRECTION_X3:
|
||||
bgc_vector3_reverse_fp64(axis, &basis->e3);
|
||||
state->e3 = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int bgc_basis3_make_from_directions_fp64(
|
||||
const int primary_direction, const BgcVector3FP64* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP64* auxiliary_vector,
|
||||
BgcBasis3FP64* basis
|
||||
) {
|
||||
const int direstion_validation_code = _bgc_basis3_validate_directions(primary_direction, auxiliary_direction);
|
||||
|
||||
if (direstion_validation_code != BGC_SUCCESS) {
|
||||
return direstion_validation_code;
|
||||
}
|
||||
|
||||
const double primary_square_modulus = bgc_vector3_get_square_modulus_fp64(primary_vector);
|
||||
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
const double auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(auxiliary_vector);
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
|
||||
}
|
||||
|
||||
BgcVector3FP64 orthogonal;
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(primary_vector, auxiliary_vector, &orthogonal);
|
||||
|
||||
const double orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal);
|
||||
|
||||
if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP64 * primary_square_modulus * auxiliary_square_modulus) {
|
||||
return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
|
||||
}
|
||||
|
||||
BgcVector3FP64 primary_axis, auxiliary_axis;
|
||||
|
||||
bgc_vector3_multiply_fp64(primary_vector, sqrt(1.0 / primary_square_modulus), &primary_axis);
|
||||
|
||||
bgc_vector3_subtract_scaled_fp64(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp64(auxiliary_vector, &primary_axis), &auxiliary_axis);
|
||||
|
||||
bgc_vector3_multiply_fp64(&auxiliary_axis, sqrt(1.0 / bgc_vector3_get_square_modulus_fp64(&auxiliary_axis)), &auxiliary_axis);
|
||||
|
||||
_BgcBasis3State state;
|
||||
|
||||
_bgc_basis3_state_reset(&state);
|
||||
|
||||
_bgc_basis3_load_axis_fp64(primary_direction, &primary_axis, &state, basis);
|
||||
_bgc_basis3_load_axis_fp64(auxiliary_direction, &auxiliary_axis, &state, basis);
|
||||
|
||||
if (!state.e1) {
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e2, &basis->e3, &basis->e1);
|
||||
bgc_vector3_normalize_fp64(&basis->e1);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
if (!state.e2) {
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e3, &basis->e1, &basis->e2);
|
||||
bgc_vector3_normalize_fp64(&basis->e2);
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(&basis->e1, &basis->e2, &basis->e3);
|
||||
bgc_vector3_normalize_fp64(&basis->e3);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
#ifndef _BGC_BASIS3_H_
|
||||
#define _BGC_BASIS3_H_
|
||||
|
||||
#include "./vector3.h"
|
||||
|
||||
#define BGC_ERROR_BASIS3_PRIMARY_DIRECTION_UNKNOWN -3001
|
||||
#define BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO -3002
|
||||
|
||||
#define BGC_ERROR_BASIS3_AUXILIARY_DIRECTION_UNKNOWN -3011
|
||||
#define BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO -3012
|
||||
|
||||
#define BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL -3021
|
||||
|
||||
|
||||
typedef struct {
|
||||
BgcVector3FP32 e1, e2, e3;
|
||||
} BgcBasis3FP32;
|
||||
|
||||
typedef struct {
|
||||
BgcVector3FP64 e1, e2, e3;
|
||||
} BgcBasis3FP64;
|
||||
|
||||
int bgc_basis3_make_from_directions_fp32(
|
||||
const int primary_direction, const BgcVector3FP32* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP32* auxiliary_vector,
|
||||
BgcBasis3FP32* basis
|
||||
);
|
||||
|
||||
int bgc_basis3_make_from_directions_fp64(
|
||||
const int primary_direction, const BgcVector3FP64* primary_vector,
|
||||
const int auxiliary_direction, const BgcVector3FP64* auxiliary_vector,
|
||||
BgcBasis3FP64* basis
|
||||
);
|
||||
|
||||
#endif
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
#define BGC_EPSYLON_EFFECTIVENESS_LIMIT_FP32 1.0f
|
||||
|
||||
#define BGC_EPSYLON_FP32 4.76837E-7f
|
||||
#define BGC_SQUARE_EPSYLON_FP32 2.27373906E-13f
|
||||
#define BGC_SQUARE_EPSYLON_FP32 (BGC_EPSYLON_FP32 * BGC_EPSYLON_FP32)
|
||||
|
||||
#define BGC_ONE_THIRD_FP32 0.3333333333f
|
||||
#define BGC_ONE_SIXTH_FP32 0.1666666667f
|
||||
|
|
@ -19,7 +19,7 @@
|
|||
#define BGC_EPSYLON_EFFECTIVENESS_LIMIT_FP64 1.0
|
||||
|
||||
#define BGC_EPSYLON_FP64 4.996003611E-14
|
||||
#define BGC_SQUARE_EPSYLON_FP64 2.496005208112504E-27
|
||||
#define BGC_SQUARE_EPSYLON_FP64 (BGC_EPSYLON_FP64 * BGC_EPSYLON_FP64)
|
||||
|
||||
#define BGC_ONE_THIRD_FP64 0.3333333333333333333
|
||||
#define BGC_ONE_SIXTH_FP64 0.1666666666666666667
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@ 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);
|
||||
|
||||
|
|
|
|||
|
|
@ -44,6 +44,62 @@ 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)
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@ extern inline void bgc_vector3_reset_fp64(BgcVector3FP64* vector);
|
|||