Добавление функций определения поворотов (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);
|
|||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -50,6 +50,94 @@ 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)
|
||||
|
|
|
|||
|
|
@ -151,6 +151,274 @@ void bgc_versor_set_turn_fp64(const double x1, const double x2, const double x3,
|
|||
bgc_versor_set_values_fp64(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
|
||||
}
|
||||
|
||||
// ========= Make Direction Difference ========== //
|
||||
|
||||
inline int _bgc_versor_make_direction_turn_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, const float square_modulus_product, BgcVersorFP32* result)
|
||||
{
|
||||
BgcVector3FP32 orthogonal_axis;
|
||||
|
||||
bgc_vector3_get_cross_product_fp32(start, end, &orthogonal_axis);
|
||||
|
||||
const float scalar_product = bgc_vector3_get_scalar_product_fp32(start, end);
|
||||
const float square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal_axis);
|
||||
const float square_sine = square_modulus / square_modulus_product;
|
||||
|
||||
if (square_sine > BGC_SQUARE_EPSYLON_FP32) {
|
||||
const float cosine = scalar_product / sqrtf(square_modulus_product);
|
||||
const float angle = 0.5f * atan2f(sqrtf(square_sine), cosine);
|
||||
|
||||
const float multiplier = sinf(angle) * sqrtf(1.0f / square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp32(cosf(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
|
||||
return BGC_SOME_TURN;
|
||||
}
|
||||
|
||||
if (scalar_product < 0.0f) {
|
||||
return BGC_OPPOSITE;
|
||||
}
|
||||
|
||||
bgc_versor_reset_fp32(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
inline int _bgc_versor_make_direction_turn_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, const double square_modulus_product, BgcVersorFP64* result)
|
||||
{
|
||||
BgcVector3FP64 orthogonal_axis;
|
||||
|
||||
bgc_vector3_get_cross_product_fp64(start, end, &orthogonal_axis);
|
||||
|
||||
const double scalar_product = bgc_vector3_get_scalar_product_fp64(start, end);
|
||||
const double square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal_axis);
|
||||
const double square_sine = square_modulus / square_modulus_product;
|
||||
|
||||
if (square_sine > BGC_SQUARE_EPSYLON_FP64) {
|
||||
const double cosine = scalar_product / sqrt(square_modulus_product);
|
||||
const double angle = 0.5 * atan2(sqrt(square_sine), cosine);
|
||||
|
||||
const double multiplier = sin(angle) * sqrt(1.0f / square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp64(cos(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
|
||||
return BGC_SOME_TURN;
|
||||
}
|
||||
|
||||
if (scalar_product < 0.0) {
|
||||
return BGC_OPPOSITE;
|
||||
}
|
||||
|
||||
bgc_versor_reset_fp64(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
int bgc_versor_make_direction_difference_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, BgcVersorFP32* result)
|
||||
{
|
||||
const float start_square_modulus = bgc_vector3_get_square_modulus_fp32(start);
|
||||
const float end_square_modulus = bgc_vector3_get_square_modulus_fp32(end);
|
||||
|
||||
if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP32 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
bgc_versor_reset_fp32(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
return _bgc_versor_make_direction_turn_fp32(start, end, start_square_modulus * end_square_modulus, result);
|
||||
}
|
||||
|
||||
int bgc_versor_make_direction_difference_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, BgcVersorFP64* result)
|
||||
{
|
||||
const double start_square_modulus = bgc_vector3_get_square_modulus_fp64(start);
|
||||
const double end_square_modulus = bgc_vector3_get_square_modulus_fp64(end);
|
||||
|
||||
if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP64 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
bgc_versor_reset_fp64(result);
|
||||
return BGC_ZERO_TURN;
|
||||
}
|
||||
|
||||
return _bgc_versor_make_direction_turn_fp64(start, end, start_square_modulus * end_square_modulus, result);
|
||||
}
|
||||
|
||||
// =============== Set Directions =============== //
|
||||
|
||||
inline int _bgc_versor_validate_basis_fp32(const float primary_square_modulus, const float auxiliary_square_modulus, const float orthogonal_square_modulus)
|
||||
{
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: primary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP32) {
|
||||
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
inline int _bgc_versor_validate_basis_fp64(const double primary_square_modulus, const double auxiliary_square_modulus, const double orthogonal_square_modulus)
|
||||
{
|
||||
if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: primary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: auxiliary_vector is zero
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP64) {
|
||||
//TODO: add error code for: primary_vector and auxiliary_vector are parallel
|
||||
return BGC_FAILED;
|
||||
}
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
int bgc_versor_make_basis_difference_fp32(
|
||||
const BgcVector3FP32* initial_primary_direction,
|
||||
const BgcVector3FP32* initial_auxiliary_direction,
|
||||
const BgcVector3FP32* final_primary_direction,
|
||||
const BgcVector3FP32* final_auxiliary_direction,
|
||||
BgcVersorFP32* result
|
||||
)
|
||||
{
|
||||
BgcVector3FP32 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
||||
|
||||
// Step 1: Validate initial basis:
|
||||
bgc_vector3_get_cross_product_fp32(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);
|
||||
|
||||
const float initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_primary_direction);
|
||||
const float initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_auxiliary_direction);
|
||||
const float initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&initial_orthogonal_direction);
|
||||
|
||||
const int initial_basis_valudation = _bgc_versor_validate_basis_fp32(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);
|
||||
|
||||
if (initial_basis_valudation != BGC_SUCCESS) {
|
||||
return initial_basis_valudation;
|
||||
}
|
||||
|
||||
// Step 1: Validate final basis:
|
||||
bgc_vector3_get_cross_product_fp32(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);
|
||||
|
||||
const float final_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_primary_direction);
|
||||
const float final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_auxiliary_direction);
|
||||
const float final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&final_orthogonal_direction);
|
||||
|
||||
const int final_basis_valudation = _bgc_versor_validate_basis_fp32(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);
|
||||
|
||||
if (final_basis_valudation != BGC_SUCCESS) {
|
||||
return final_basis_valudation;
|
||||
}
|
||||
|
||||
// Step 3: Validate normalize orthogonal vectors:
|
||||
bgc_vector3_divide_fp32(&initial_orthogonal_direction, sqrtf(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
|
||||
bgc_vector3_divide_fp32(&final_orthogonal_direction, sqrtf(final_orthogonal_square_modulus), &final_orthogonal_direction);
|
||||
|
||||
BgcVersorFP32 turn1, turn2;
|
||||
|
||||
// Step 4: Find turn1
|
||||
int turn1_code = _bgc_versor_make_direction_turn_fp32(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);
|
||||
|
||||
if (turn1_code == BGC_OPPOSITE) {
|
||||
bgc_versor_set_values_fp32(0.0f, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
|
||||
}
|
||||
|
||||
bgc_versor_turn_vector_fp32(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);
|
||||
|
||||
// Step 5: Find turn2:
|
||||
int turn2_code = _bgc_versor_make_direction_turn_fp32(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);
|
||||
|
||||
if (turn2_code == BGC_OPPOSITE) {
|
||||
const float turn2_multiplier = sqrtf(1.0f / final_primary_square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp32(0.0f,
|
||||
final_primary_direction->x1 * turn2_multiplier,
|
||||
final_primary_direction->x2 * turn2_multiplier,
|
||||
final_primary_direction->x3 * turn2_multiplier,
|
||||
&turn2
|
||||
);
|
||||
}
|
||||
|
||||
// Step 6: Combine turn1 and turn2:
|
||||
bgc_versor_combine_fp32(&turn2, &turn1, result);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
int bgc_versor_make_basis_difference_fp64(
|
||||
const BgcVector3FP64* initial_primary_direction,
|
||||
const BgcVector3FP64* initial_auxiliary_direction,
|
||||
const BgcVector3FP64* final_primary_direction,
|
||||
const BgcVector3FP64* final_auxiliary_direction,
|
||||
BgcVersorFP64* result
|
||||
)
|
||||
{
|
||||
BgcVector3FP64 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;
|
||||
|
||||
// Step 1: Validate initial basis:
|
||||
bgc_vector3_get_cross_product_fp64(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);
|
||||
|
||||
const double initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_primary_direction);
|
||||
const double initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_auxiliary_direction);
|
||||
const double initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&initial_orthogonal_direction);
|
||||
|
||||
const int initial_basis_valudation = _bgc_versor_validate_basis_fp64(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);
|
||||
|
||||
if (initial_basis_valudation != BGC_SUCCESS) {
|
||||
return initial_basis_valudation;
|
||||
}
|
||||
|
||||
// Step 1: Validate final basis:
|
||||
bgc_vector3_get_cross_product_fp64(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);
|
||||
|
||||
const double final_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_primary_direction);
|
||||
const double final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_auxiliary_direction);
|
||||
const double final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&final_orthogonal_direction);
|
||||
|
||||
const int final_basis_valudation = _bgc_versor_validate_basis_fp64(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);
|
||||
|
||||
if (final_basis_valudation != BGC_SUCCESS) {
|
||||
return final_basis_valudation;
|
||||
}
|
||||
|
||||
// Step 3: Validate normalize orthogonal vectors:
|
||||
bgc_vector3_divide_fp64(&initial_orthogonal_direction, sqrt(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
|
||||
bgc_vector3_divide_fp64(&final_orthogonal_direction, sqrt(final_orthogonal_square_modulus), &final_orthogonal_direction);
|
||||
|
||||
BgcVersorFP64 turn1, turn2;
|
||||
|
||||
// Step 4: Find turn1
|
||||
int turn1_code = _bgc_versor_make_direction_turn_fp64(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);
|
||||
|
||||
if (turn1_code == BGC_OPPOSITE) {
|
||||
bgc_versor_set_values_fp64(0.0, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
|
||||
}
|
||||
|
||||
bgc_versor_turn_vector_fp64(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);
|
||||
|
||||
// Step 5: Find turn2:
|
||||
int turn2_code = _bgc_versor_make_direction_turn_fp64(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);
|
||||
|
||||
if (turn2_code == BGC_OPPOSITE) {
|
||||
const double turn2_multiplier = sqrt(1.0 / final_primary_square_modulus);
|
||||
|
||||
bgc_versor_set_values_fp64(0.0,
|
||||
final_primary_direction->x1 * turn2_multiplier,
|
||||
final_primary_direction->x2 * turn2_multiplier,
|
||||
final_primary_direction->x3 * turn2_multiplier,
|
||||
&turn2
|
||||
);
|
||||
}
|
||||
|
||||
// Step 6: Combine turn1 and turn2:
|
||||
bgc_versor_combine_fp64(&turn2, &turn1, result);
|
||||
|
||||
return BGC_SUCCESS;
|
||||
}
|
||||
|
||||
// =============== Get Exponation =============== //
|
||||
|
||||
void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power)
|
||||
|
|
|
|||
|
|
@ -8,6 +8,20 @@
|
|||
#include "vector3.h"
|
||||
#include "rotation3.h"
|
||||
#include "matrix3x3.h"
|
||||
#include "quaternion.h"
|
||||
|
||||
#define BGC_SOME_TURN 1
|
||||
#define BGC_ZERO_TURN 0
|
||||
#define BGC_OPPOSITE -1
|
||||
|
||||
#define BGC_ERROR_PRIMARY_DIRECTION_UNKNOWN -3001
|
||||
#define BGC_ERROR_PRIMARY_VECTOR_IS_ZERO -3002
|
||||
|
||||
#define BGC_ERROR_AUXILIARY_DIRECTION_UNKNOWN -3011
|
||||
#define BGC_ERROR_AUXILIARY_VECTOR_IS_ZERO -3012
|
||||
|
||||
#define BGC_ERROR_DIRECTIONS_PARALLEL -3021
|
||||
#define BGC_ERROR_VECTORS_PARALLEL -3022
|
||||
|
||||
// =================== Types ==================== //
|
||||
|
||||
|
|
@ -112,6 +126,30 @@ inline void bgc_versor_set_rotation_fp64(const BgcRotation3FP64* rotation, BgcVe
|
|||
bgc_versor_set_turn_fp64(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BGC_ANGLE_UNIT_RADIANS, result);
|
||||
}
|
||||
|
||||
// ========= Make Direction Difference ========== //
|
||||
|
||||
int bgc_versor_make_direction_difference_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, BgcVersorFP32* result);
|
||||
|
||||
int bgc_versor_make_direction_difference_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, BgcVersorFP64* result);
|
||||
|
||||
// =============== Set Directions =============== //
|
||||
|
||||
int bgc_versor_make_basis_difference_fp32(
|
||||
const BgcVector3FP32* initial_primary_direction,
|
||||
const BgcVector3FP32* initial_auxiliary_direction,
|
||||
const BgcVector3FP32* final_primary_direction,
|
||||
const BgcVector3FP32* final_auxiliary_direction,
|
||||
BgcVersorFP32* result
|
||||
);
|
||||
|
||||
int bgc_versor_make_basis_difference_fp64(
|
||||
const BgcVector3FP64* initial_primary_direction,
|
||||
const BgcVector3FP64* initial_auxiliary_direction,
|
||||
const BgcVector3FP64* final_primary_direction,
|
||||
const BgcVector3FP64* final_auxiliary_direction,
|
||||
BgcVersorFP64* result
|
||||
);
|
||||
|
||||
// ==================== Copy ==================== //
|
||||
|
||||
inline void bgc_versor_copy_fp32(const BgcVersorFP32* source, BgcVersorFP32* destination)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue