Изменение полей версора на константные / Making versor's fields constant
This commit is contained in:
parent
e7616ae80c
commit
23fcdc2c28
10 changed files with 423 additions and 425 deletions
|
|
@ -2,5 +2,4 @@
|
|||
<CodeBlocks_workspace_layout_file>
|
||||
<FileVersion major="1" minor="0" />
|
||||
<ActiveProject path="dev/geometry-dev.cbp" />
|
||||
<PreferredTarget name="Release" />
|
||||
</CodeBlocks_workspace_layout_file>
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
# depslib dependency file v1.0
|
||||
1731995770 source:/home/andrey/Projects/Private/C/Geometry/dev/main.c
|
||||
1732169779 source:/home/andrey/Projects/Private/C/Geometry/dev/main.c
|
||||
<stdio.h>
|
||||
<stdlib.h>
|
||||
<math.h>
|
||||
|
|
@ -21,18 +21,18 @@
|
|||
"quaternion.h"
|
||||
"versor.h"
|
||||
|
||||
1730476002 /home/andrey/Projects/Private/C/Geometry/src/basis.h
|
||||
1732094620 /home/andrey/Projects/Private/C/Geometry/src/basis.h
|
||||
|
||||
1730392488 /home/andrey/Projects/Private/C/Geometry/src/angle.h
|
||||
1732096335 /home/andrey/Projects/Private/C/Geometry/src/angle.h
|
||||
<math.h>
|
||||
"basis.h"
|
||||
|
||||
1731673023 /home/andrey/Projects/Private/C/Geometry/src/vector2.h
|
||||
1732095049 /home/andrey/Projects/Private/C/Geometry/src/vector2.h
|
||||
"basis.h"
|
||||
"angle.h"
|
||||
<math.h>
|
||||
|
||||
1731673331 /home/andrey/Projects/Private/C/Geometry/src/vector3.h
|
||||
1732095097 /home/andrey/Projects/Private/C/Geometry/src/vector3.h
|
||||
"basis.h"
|
||||
"angle.h"
|
||||
<math.h>
|
||||
|
|
@ -42,12 +42,12 @@
|
|||
"angle.h"
|
||||
<math.h>
|
||||
|
||||
1731673308 /home/andrey/Projects/Private/C/Geometry/src/matrix2x2.h
|
||||
1732095748 /home/andrey/Projects/Private/C/Geometry/src/matrix2x2.h
|
||||
"angle.h"
|
||||
"vector2.h"
|
||||
"matrixes.h"
|
||||
|
||||
1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix3x3.h
|
||||
1732095414 /home/andrey/Projects/Private/C/Geometry/src/matrix3x3.h
|
||||
"vector3.h"
|
||||
"matrixes.h"
|
||||
|
||||
|
|
@ -61,12 +61,12 @@
|
|||
"vector2.h"
|
||||
"matrix2x2.h"
|
||||
|
||||
1730355414 /home/andrey/Projects/Private/C/Geometry/src/rotation3.h
|
||||
1732095748 /home/andrey/Projects/Private/C/Geometry/src/rotation3.h
|
||||
"basis.h"
|
||||
"angle.h"
|
||||
"vector3.h"
|
||||
|
||||
1731995858 /home/andrey/Projects/Private/C/Geometry/src/versor.h
|
||||
1732169837 /home/andrey/Projects/Private/C/Geometry/src/versor.h
|
||||
<stdint.h>
|
||||
"basis.h"
|
||||
"vector3.h"
|
||||
|
|
@ -107,19 +107,19 @@
|
|||
"versor.h"
|
||||
"affine_map3.h"
|
||||
|
||||
1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrixes.h
|
||||
1732094620 /home/andrey/Projects/Private/C/Geometry/src/matrixes.h
|
||||
|
||||
1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix2x3.h
|
||||
1732095530 /home/andrey/Projects/Private/C/Geometry/src/matrix2x3.h
|
||||
"vector2.h"
|
||||
"vector3.h"
|
||||
"matrixes.h"
|
||||
|
||||
1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix3x2.h
|
||||
1732095530 /home/andrey/Projects/Private/C/Geometry/src/matrix3x2.h
|
||||
"vector2.h"
|
||||
"vector3.h"
|
||||
"matrixes.h"
|
||||
|
||||
1731407834 /home/andrey/Projects/Private/C/Geometry/src/quaternion.h
|
||||
1732095234 /home/andrey/Projects/Private/C/Geometry/src/quaternion.h
|
||||
<math.h>
|
||||
"basis.h"
|
||||
"matrix3x3.h"
|
||||
|
|
|
|||
|
|
@ -2,9 +2,9 @@
|
|||
<CodeBlocks_layout_file>
|
||||
<FileVersion major="1" minor="0" />
|
||||
<ActiveTarget name="Release" />
|
||||
<File name="main.c" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<File name="main.c" open="1" top="1" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="3793" topLine="116" />
|
||||
<Cursor1 position="1168" topLine="46" />
|
||||
</Cursor>
|
||||
</File>
|
||||
</CodeBlocks_layout_file>
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ BgFP32Versor * make_random_versors(const unsigned int amount)
|
|||
|
||||
void print_versor(const BgFP32Versor* versor)
|
||||
{
|
||||
printf("(%f, %f, %f, %f)\n", versor->_s0, versor->_x1, versor->_x2, versor->_x3);
|
||||
printf("(%f, %f, %f, %f)\n", versor->s0, versor->x1, versor->x2, versor->x3);
|
||||
}
|
||||
|
||||
void print_vector(const BgFP32Vector3* vector)
|
||||
|
|
|
|||
|
|
@ -2,9 +2,29 @@
|
|||
<CodeBlocks_layout_file>
|
||||
<FileVersion major="1" minor="0" />
|
||||
<ActiveTarget name="Debug" />
|
||||
<File name="matrix2x3.h" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<File name="geometry.h" open="0" top="0" tabpos="8" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1596" topLine="0" />
|
||||
<Cursor1 position="316" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="rotation3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="128" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1765" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix2x2.h" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="2008" topLine="63" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="4343" topLine="67" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="angle.c" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
|
|
@ -12,49 +32,9 @@
|
|||
<Cursor1 position="0" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="quaternion.h" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<File name="matrixes.h" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1849" topLine="187" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix2x2.c" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="24" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix2x2.h" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="13404" topLine="440" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector2.h" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="556" topLine="12" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector2.c" open="1" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="24" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="versor.c" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="504" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="rotation3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="154" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="versor.h" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="27" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="basis.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="103" topLine="0" />
|
||||
<Cursor1 position="4418" topLine="54" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="basis.c" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
|
|
@ -62,49 +42,79 @@
|
|||
<Cursor1 position="20" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector3.h" open="1" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="0" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="quaternion.h" open="1" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="4195" topLine="122" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector2.c" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1765" topLine="17" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="angle.h" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1082" topLine="0" />
|
||||
<Cursor1 position="12295" topLine="509" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x2.h" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="897" topLine="52" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="quaternion.c" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="6789" topLine="145" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x3.h" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="17612" topLine="537" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="geometry.h" open="0" top="0" tabpos="8" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="316" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector3.h" open="1" top="1" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="15169" topLine="102" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="rotation3.h" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="305" topLine="29" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="23" topLine="72" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector3.c" open="1" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<File name="matrix2x2.c" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="24" topLine="0" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="versor.h" open="1" top="1" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="3483" topLine="119" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x2.h" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="2291" topLine="280" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="vector2.h" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="2044" topLine="216" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="versor.c" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1316" topLine="90" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix3x3.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="0" topLine="99" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="rotation3.h" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="2552" topLine="44" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="basis.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1382" topLine="6" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrix2x3.h" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="2817" topLine="324" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="quaternion.c" open="1" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="1596" topLine="2" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="matrixes.c" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="11981" topLine="205" />
|
||||
</Cursor>
|
||||
</File>
|
||||
</CodeBlocks_layout_file>
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
// ============ Make Rotation Matrix ============ //
|
||||
|
||||
void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
|
||||
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
|
||||
{
|
||||
const float s0s0 = quaternion->s0 * quaternion->s0;
|
||||
const float x1x1 = quaternion->x1 * quaternion->x1;
|
||||
|
|
@ -40,7 +40,7 @@ void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion,
|
|||
matrix->r1c3 = corrector2 * (x1x3 + s0x2);
|
||||
}
|
||||
|
||||
void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
|
||||
void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
|
||||
{
|
||||
const double s0s0 = quaternion->s0 * quaternion->s0;
|
||||
const double x1x1 = quaternion->x1 * quaternion->x1;
|
||||
|
|
@ -80,7 +80,7 @@ void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion,
|
|||
|
||||
// ============ Make Reverse Matrix ============= //
|
||||
|
||||
void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
|
||||
void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
|
||||
{
|
||||
const float s0s0 = quaternion->s0 * quaternion->s0;
|
||||
const float x1x1 = quaternion->x1 * quaternion->x1;
|
||||
|
|
@ -118,7 +118,7 @@ void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion,
|
|||
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
|
||||
}
|
||||
|
||||
void bg_fp64_quaternion_make_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
|
||||
void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
|
||||
{
|
||||
const double s0s0 = quaternion->s0 * quaternion->s0;
|
||||
const double x1x1 = quaternion->x1 * quaternion->x1;
|
||||
|
|
|
|||
|
|
@ -158,15 +158,15 @@ static inline void bg_fp64_quaternion_set_conjugate_fp32(const BgFP32Quaternion*
|
|||
|
||||
// ============ Make Rotation Matrix ============ //
|
||||
|
||||
void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
|
||||
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
|
||||
|
||||
void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
|
||||
void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
|
||||
|
||||
// ============ Make Reverse Matrix ============= //
|
||||
|
||||
void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
|
||||
void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
|
||||
|
||||
void bg_fp64_quaternion_make_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
|
||||
void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
|
||||
|
||||
// ==================== Add ===================== //
|
||||
|
||||
|
|
|
|||
92
src/versor.c
92
src/versor.c
|
|
@ -7,51 +7,45 @@ const BgFP32Versor BG_FP32_IDLE_VERSOR = { 1.0f, 0.0f, 0.0f, 0.0f };
|
|||
|
||||
const BgFP64Versor BG_FP64_IDLE_VERSOR = { 1.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
void __bg_fp32_versor_normalize(const float square_module, BgFP32Versor* versor)
|
||||
void __bg_fp32_versor_normalize(const float square_module, __BgFP32VersorDarkTwin* twin)
|
||||
{
|
||||
if (square_module <= BG_FP32_SQUARE_EPSYLON) {
|
||||
bg_fp32_versor_reset(versor);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3) <= BG_FP32_SQUARE_EPSYLON * square_module) {
|
||||
bg_fp32_versor_reset(versor);
|
||||
if (square_module <= BG_FP32_SQUARE_EPSYLON || (twin->x1 * twin->x1 + twin->x2 * twin->x2 + twin->x3 * twin->x3) <= BG_FP32_SQUARE_EPSYLON * square_module) {
|
||||
twin->s0 = 1.0f;
|
||||
twin->x1 = 0.0f;
|
||||
twin->x2 = 0.0f;
|
||||
twin->x3 = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
const float module = sqrtf(square_module);
|
||||
|
||||
versor->_s0 /= module;
|
||||
versor->_x1 /= module;
|
||||
versor->_x2 /= module;
|
||||
versor->_x3 /= module;
|
||||
twin->s0 /= module;
|
||||
twin->x1 /= module;
|
||||
twin->x2 /= module;
|
||||
twin->x3 /= module;
|
||||
}
|
||||
|
||||
void __bg_fp64_versor_normalize(const double square_module, BgFP64Versor* versor)
|
||||
void __bg_fp64_versor_normalize(const double square_module, __BgFP64VersorDarkTwin* twin)
|
||||
{
|
||||
if (square_module <= BG_FP64_SQUARE_EPSYLON) {
|
||||
bg_fp64_versor_reset(versor);
|
||||
return;
|
||||
}
|
||||
|
||||
const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3;
|
||||
|
||||
if (square_vector <= BG_FP64_SQUARE_EPSYLON * square_module) {
|
||||
bg_fp64_versor_reset(versor);
|
||||
if (square_module <= BG_FP64_SQUARE_EPSYLON || (twin->x1 * twin->x1 + twin->x2 * twin->x2 + twin->x3 * twin->x3) <= BG_FP64_SQUARE_EPSYLON * square_module) {
|
||||
twin->s0 = 1.0;
|
||||
twin->x1 = 0.0;
|
||||
twin->x2 = 0.0;
|
||||
twin->x3 = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
const double module = sqrt(square_module);
|
||||
|
||||
versor->_s0 /= module;
|
||||
versor->_x1 /= module;
|
||||
versor->_x2 /= module;
|
||||
versor->_x3 /= module;
|
||||
twin->s0 /= module;
|
||||
twin->x1 /= module;
|
||||
twin->x2 /= module;
|
||||
twin->x3 /= module;
|
||||
}
|
||||
|
||||
// ==================== Make ==================== //
|
||||
// =============== Set Crude Turn =============== //
|
||||
|
||||
void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result)
|
||||
void bg_fp32_versor_set_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result)
|
||||
{
|
||||
const float square_vector = x1 * x1 + x2 * x2 + x3 * x3;
|
||||
|
||||
|
|
@ -71,16 +65,10 @@ void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const f
|
|||
|
||||
const float multiplier = sine / sqrtf(square_vector);
|
||||
|
||||
bg_fp32_versor_set_values(
|
||||
cosf(half_angle),
|
||||
x1 * multiplier,
|
||||
x2 * multiplier,
|
||||
x3 * multiplier,
|
||||
result
|
||||
);
|
||||
bg_fp32_versor_set_values(cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
|
||||
}
|
||||
|
||||
void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result)
|
||||
void bg_fp64_versor_set_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result)
|
||||
{
|
||||
const double square_vector = x1 * x1 + x2 * x2 + x3 * x3;
|
||||
|
||||
|
|
@ -100,13 +88,7 @@ void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const
|
|||
|
||||
const double multiplier = sine / sqrt(square_vector);
|
||||
|
||||
bg_fp64_versor_set_values(
|
||||
cos(half_angle),
|
||||
x1 * multiplier,
|
||||
x2 * multiplier,
|
||||
x3 * multiplier,
|
||||
result
|
||||
);
|
||||
bg_fp64_versor_set_values(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
|
||||
}
|
||||
|
||||
// ================= Rotation3 ================== //
|
||||
|
|
@ -117,20 +99,20 @@ void bg_fp32_versor_get_rotation(const BgFP32Versor* versor, BgFP32Rotation3* re
|
|||
return;
|
||||
}
|
||||
|
||||
if (versor->_s0 <= -(1.0f - BG_FP32_EPSYLON) || 1.0f - BG_FP32_EPSYLON <= versor->_s0) {
|
||||
if (versor->s0 <= -(1.0f - BG_FP32_EPSYLON) || 1.0f - BG_FP32_EPSYLON <= versor->s0) {
|
||||
bg_fp32_rotation_reset(result);
|
||||
return;
|
||||
}
|
||||
|
||||
const float square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3;
|
||||
const float square_vector = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;
|
||||
|
||||
result->radians = 2.0f * acosf(versor->_s0 / sqrtf(versor->_s0 * versor->_s0 + square_vector));
|
||||
result->radians = 2.0f * acosf(versor->s0 / sqrtf(versor->s0 * versor->s0 + square_vector));
|
||||
|
||||
const float vector_module = sqrtf(square_vector);
|
||||
|
||||
result->axis.x1 = versor->_x1 / vector_module;
|
||||
result->axis.x2 = versor->_x2 / vector_module;
|
||||
result->axis.x3 = versor->_x3 / vector_module;
|
||||
result->axis.x1 = versor->x1 / vector_module;
|
||||
result->axis.x2 = versor->x2 / vector_module;
|
||||
result->axis.x3 = versor->x3 / vector_module;
|
||||
}
|
||||
|
||||
void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* result)
|
||||
|
|
@ -139,18 +121,18 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re
|
|||
return;
|
||||
}
|
||||
|
||||
if (versor->_s0 <= -(1.0 - BG_FP64_EPSYLON) || 1.0 - BG_FP64_EPSYLON <= versor->_s0) {
|
||||
if (versor->s0 <= -(1.0 - BG_FP64_EPSYLON) || 1.0 - BG_FP64_EPSYLON <= versor->s0) {
|
||||
bg_fp64_rotation_reset(result);
|
||||
return;
|
||||
}
|
||||
|
||||
const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3;
|
||||
const double square_vector = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;
|
||||
|
||||
result->radians = 2.0 * acos(versor->_s0 / sqrt(versor->_s0 * versor->_s0 + square_vector));
|
||||
result->radians = 2.0 * acos(versor->s0 / sqrt(versor->s0 * versor->s0 + square_vector));
|
||||
|
||||
const double vector_module = sqrt(square_vector);
|
||||
|
||||
result->axis.x1 = versor->_x1 / vector_module;
|
||||
result->axis.x2 = versor->_x2 / vector_module;
|
||||
result->axis.x3 = versor->_x3 / vector_module;
|
||||
result->axis.x1 = versor->x1 / vector_module;
|
||||
result->axis.x2 = versor->x2 / vector_module;
|
||||
result->axis.x3 = versor->x3 / vector_module;
|
||||
}
|
||||
|
|
|
|||
517
src/versor.h
517
src/versor.h
|
|
@ -8,14 +8,28 @@
|
|||
#include "rotation3.h"
|
||||
#include "matrix3x3.h"
|
||||
|
||||
// =================== Types ==================== //
|
||||
|
||||
typedef struct {
|
||||
float _s0, _x1, _x2, _x3;
|
||||
const float s0, x1, x2, x3;
|
||||
} BgFP32Versor;
|
||||
|
||||
typedef struct {
|
||||
double _s0, _x1, _x2, _x3;
|
||||
const double s0, x1, x2, x3;
|
||||
} BgFP64Versor;
|
||||
|
||||
// ================= Dark Twins ================= //
|
||||
|
||||
typedef struct {
|
||||
float s0, x1, x2, x3;
|
||||
} __BgFP32VersorDarkTwin;
|
||||
|
||||
typedef struct {
|
||||
double s0, x1, x2, x3;
|
||||
} __BgFP64VersorDarkTwin;
|
||||
|
||||
// ================= Constants ================== //
|
||||
|
||||
extern const BgFP32Versor BG_FP32_IDLE_VERSOR;
|
||||
extern const BgFP64Versor BG_FP64_IDLE_VERSOR;
|
||||
|
||||
|
|
@ -23,116 +37,59 @@ extern const BgFP64Versor BG_FP64_IDLE_VERSOR;
|
|||
|
||||
static inline void bg_fp32_versor_reset(BgFP32Versor* versor)
|
||||
{
|
||||
versor->_s0 = 1.0f;
|
||||
versor->_x1 = 0.0f;
|
||||
versor->_x2 = 0.0f;
|
||||
versor->_x3 = 0.0f;
|
||||
__BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)versor;
|
||||
|
||||
twin->s0 = 1.0f;
|
||||
twin->x1 = 0.0f;
|
||||
twin->x2 = 0.0f;
|
||||
twin->x3 = 0.0f;
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_reset(BgFP64Versor* versor)
|
||||
{
|
||||
versor->_s0 = 1.0;
|
||||
versor->_x1 = 0.0;
|
||||
versor->_x2 = 0.0;
|
||||
versor->_x3 = 0.0;
|
||||
}
|
||||
__BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)versor;
|
||||
|
||||
// ============== Get Scalar Part =============== //
|
||||
|
||||
static inline float bg_fp32_versor_get_scalar(const BgFP32Versor* versor)
|
||||
{
|
||||
return versor->_s0;
|
||||
}
|
||||
|
||||
static inline double bg_fp64_versor_get_scalar(const BgFP64Versor* versor)
|
||||
{
|
||||
return versor->_s0;
|
||||
}
|
||||
|
||||
// ============== Get Vector Part =============== //
|
||||
|
||||
static inline void bg_fp32_versor_get_vector_part(const BgFP32Versor* versor, BgFP32Vector3 * result)
|
||||
{
|
||||
result->x1 = versor->_x1;
|
||||
result->x2 = versor->_x2;
|
||||
result->x3 = versor->_x3;
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_get_vector_part(const BgFP64Versor* versor, BgFP64Vector3 * result)
|
||||
{
|
||||
result->x1 = versor->_x1;
|
||||
result->x2 = versor->_x2;
|
||||
result->x3 = versor->_x3;
|
||||
}
|
||||
|
||||
// =================== Get x1 =================== //
|
||||
|
||||
static inline float bg_fp32_versor_get_x1(const BgFP32Versor* versor)
|
||||
{
|
||||
return versor->_x1;
|
||||
}
|
||||
|
||||
static inline double bg_fp64_versor_get_x1(const BgFP64Versor* versor)
|
||||
{
|
||||
return versor->_x1;
|
||||
}
|
||||
|
||||
// =================== Get x2 =================== //
|
||||
|
||||
static inline float bg_fp32_versor_get_x2(const BgFP32Versor* versor)
|
||||
{
|
||||
return versor->_x2;
|
||||
}
|
||||
|
||||
static inline double bg_fp64_versor_get_x2(const BgFP64Versor* versor)
|
||||
{
|
||||
return versor->_x2;
|
||||
}
|
||||
|
||||
// =================== Get x3 =================== //
|
||||
|
||||
|
||||
static inline float bg_fp32_versor_get_x3(const BgFP32Versor* versor)
|
||||
{
|
||||
return versor->_x3;
|
||||
}
|
||||
|
||||
static inline double bg_fp64_versor_get_x3(const BgFP64Versor* versor)
|
||||
{
|
||||
return versor->_x3;
|
||||
twin->s0 = 1.0;
|
||||
twin->x1 = 0.0;
|
||||
twin->x2 = 0.0;
|
||||
twin->x3 = 0.0;
|
||||
}
|
||||
|
||||
// ==================== Set ===================== //
|
||||
|
||||
void __bg_fp32_versor_normalize(const float square_module, BgFP32Versor* versor);
|
||||
void __bg_fp32_versor_normalize(const float square_module, __BgFP32VersorDarkTwin* twin);
|
||||
|
||||
void __bg_fp64_versor_normalize(const double square_module, BgFP64Versor* versor);
|
||||
void __bg_fp64_versor_normalize(const double square_module, __BgFP64VersorDarkTwin* twin);
|
||||
|
||||
static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* result)
|
||||
static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor)
|
||||
{
|
||||
result->_s0 = s0;
|
||||
result->_x1 = x1;
|
||||
result->_x2 = x2;
|
||||
result->_x3 = x3;
|
||||
__BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)versor;
|
||||
|
||||
twin->s0 = s0;
|
||||
twin->x1 = x1;
|
||||
twin->x2 = x2;
|
||||
twin->x3 = x3;
|
||||
|
||||
const float square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (square_module < 1.0f - BG_FP32_TWO_EPSYLON || 1.0f + BG_FP32_TWO_EPSYLON < square_module) {
|
||||
__bg_fp32_versor_normalize(square_module, result);
|
||||
__bg_fp32_versor_normalize(square_module, (__BgFP32VersorDarkTwin*)versor);
|
||||
}
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* result)
|
||||
static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor)
|
||||
{
|
||||
result->_s0 = s0;
|
||||
result->_x1 = x1;
|
||||
result->_x2 = x2;
|
||||
result->_x3 = x3;
|
||||
__BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)versor;
|
||||
|
||||
twin->s0 = s0;
|
||||
twin->x1 = x1;
|
||||
twin->x2 = x2;
|
||||
twin->x3 = x3;
|
||||
|
||||
const double square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (square_module < 1.0 - BG_FP64_TWO_EPSYLON || 1.0 + BG_FP64_TWO_EPSYLON < square_module) {
|
||||
__bg_fp64_versor_normalize(square_module, result);
|
||||
__bg_fp64_versor_normalize(square_module, twin);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -140,56 +97,64 @@ static inline void bg_fp64_versor_set_values(const double s0, const double x1, c
|
|||
|
||||
static inline void bg_fp32_versor_copy(const BgFP32Versor* from, BgFP32Versor* to)
|
||||
{
|
||||
to->_s0 = from->_s0;
|
||||
to->_x1 = from->_x1;
|
||||
to->_x2 = from->_x2;
|
||||
to->_x3 = from->_x3;
|
||||
__BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)to;
|
||||
|
||||
twin->s0 = from->s0;
|
||||
twin->x1 = from->x1;
|
||||
twin->x2 = from->x2;
|
||||
twin->x3 = from->x3;
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_copy(const BgFP64Versor* from, BgFP64Versor* to)
|
||||
{
|
||||
to->_s0 = from->_s0;
|
||||
to->_x1 = from->_x1;
|
||||
to->_x2 = from->_x2;
|
||||
to->_x3 = from->_x3;
|
||||
__BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)to;
|
||||
|
||||
twin->s0 = from->s0;
|
||||
twin->x1 = from->x1;
|
||||
twin->x2 = from->x2;
|
||||
twin->x3 = from->x3;
|
||||
}
|
||||
|
||||
// ==================== Make ==================== //
|
||||
// =============== Set Crude Turn =============== //
|
||||
|
||||
void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result);
|
||||
void bg_fp32_versor_set_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result);
|
||||
|
||||
void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result);
|
||||
void bg_fp64_versor_set_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result);
|
||||
|
||||
static inline void bg_fp32_versor_make_from_turn(const BgFP32Vector3* axis, const float angle, const angle_unit_t unit, BgFP32Versor* result)
|
||||
// ================== Set Turn ================== //
|
||||
|
||||
static inline void bg_fp32_versor_set_turn(const BgFP32Vector3* axis, const float angle, const angle_unit_t unit, BgFP32Versor* result)
|
||||
{
|
||||
bg_fp32_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result);
|
||||
bg_fp32_versor_set_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result);
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_make_from_turn(const BgFP64Vector3* axis, const double angle, const angle_unit_t unit, BgFP64Versor* result)
|
||||
static inline void bg_fp64_versor_set_turn(const BgFP32Vector3* axis, const double angle, const angle_unit_t unit, BgFP64Versor* result)
|
||||
{
|
||||
bg_fp64_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result);
|
||||
bg_fp64_versor_set_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result);
|
||||
}
|
||||
|
||||
static inline void bg_fp32_versor_make_from_rotation(const BgFP32Rotation3* rotation, BgFP32Versor* result)
|
||||
// ================ Set Rotation ================ //
|
||||
|
||||
static inline void bg_fp32_versor_set_rotation(const BgFP32Rotation3* rotation, BgFP32Versor* result)
|
||||
{
|
||||
bg_fp32_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result);
|
||||
bg_fp32_versor_set_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result);
|
||||
}
|
||||
|
||||
static inline void bg_fp64_versor_make_from_rotation(const BgFP64Rotation3* rotation, BgFP64Versor* result)
|
||||
static inline void bg_fp64_versor_set_rotation(const BgFP64Rotation3* rotation, BgFP64Versor* result)
|
||||
{
|
||||
bg_fp64_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result);
|
||||
bg_fp64_versor_set_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result);
|
||||
}
|
||||
|
||||
// ================= Comparison ================= //
|
||||
|
||||
static inline int bg_fp32_versor_is_idle(const BgFP32Versor* versor)
|
||||
{
|
||||
return 1.0f - BG_FP32_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - BG_FP32_EPSYLON);
|
||||
return 1.0f - BG_FP32_EPSYLON <= versor->s0 || versor->s0 <= -(1.0 - BG_FP32_EPSYLON);
|
||||
}
|
||||
|
||||
static inline int bg_fp64_versor_is_idle(const BgFP64Versor* versor)
|
||||
{
|
||||
return 1.0 - BG_FP64_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - BG_FP64_EPSYLON);
|
||||
return 1.0 - BG_FP64_EPSYLON <= versor->s0 || versor->s0 <= -(1.0 - BG_FP64_EPSYLON);
|
||||
}
|
||||
|
||||
// ============= Copy to twin type ============== //
|
||||
|
|
@ -197,10 +162,10 @@ static inline int bg_fp64_versor_is_idle(const BgFP64Versor* versor)
|
|||
static inline void bg_fp32_versor_set_from_fp64(const BgFP64Versor* versor, BgFP32Versor* result)
|
||||
{
|
||||
bg_fp32_versor_set_values(
|
||||
(float) versor->_s0,
|
||||
(float) versor->_x1,
|
||||
(float) versor->_x2,
|
||||
(float) versor->_x3,
|
||||
(float) versor->s0,
|
||||
(float) versor->x1,
|
||||
(float) versor->x2,
|
||||
(float) versor->x3,
|
||||
result
|
||||
);
|
||||
}
|
||||
|
|
@ -208,10 +173,10 @@ static inline void bg_fp32_versor_set_from_fp64(const BgFP64Versor* versor, BgFP
|
|||
static inline void bg_fp64_versor_set_from_fp32(const BgFP32Versor* versor, BgFP64Versor* result)
|
||||
{
|
||||
bg_fp64_versor_set_values(
|
||||
versor->_s0,
|
||||
versor->_x1,
|
||||
versor->_x2,
|
||||
versor->_x3,
|
||||
versor->s0,
|
||||