diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c index 4cdb30f..2b30542 100644 --- a/basic-geometry-dev/main.c +++ b/basic-geometry-dev/main.c @@ -234,7 +234,7 @@ int main() for (int j = 0; j < 1000; j++) { for (unsigned int i = 0; i < amount; i++) { bg_fp32_versor_combine(&versors1[i], &versors2[i], &results[i]); - bg_fp32_versor_get_rotation_matrix(&versors1[i], &matrixes[i]); + bg_fp32_versor_make_rotation_matrix(&versors1[i], &matrixes[i]); bg_fp32_matrix3x3_right_product(&matrixes[i], &vectors[i], &vectors[i]); //bg_fp32_versor_turn(&results[i], &vectors[i], &vectors[i]); } diff --git a/basic-geometry/basic-geometry.h b/basic-geometry/basic-geometry.h index 80740bb..17d93f7 100644 --- a/basic-geometry/basic-geometry.h +++ b/basic-geometry/basic-geometry.h @@ -14,6 +14,8 @@ #include "matrix3x2.h" #include "matrix3x3.h" +#include "tangent.h" + #include "rotation3.h" #include "quaternion.h" diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index fdc6f21..7506257 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -29,6 +29,7 @@ + @@ -43,6 +44,7 @@ + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 9a1b6d4..8637611 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -54,6 +54,9 @@ Файлы заголовков + + Файлы заголовков + @@ -92,5 +95,8 @@ Исходные файлы + + Исходные файлы + \ No newline at end of file diff --git a/basic-geometry/tangent.c b/basic-geometry/tangent.c new file mode 100644 index 0000000..3e7f50c --- /dev/null +++ b/basic-geometry/tangent.c @@ -0,0 +1,5 @@ +#include "tangent.h" + +const BgFP32Tangent BG_FP32_IDLE_TANGENT = { 1.0f, 0.0f }; + +const BgFP64Tangent BG_FP64_IDLE_TANGENT = { 1.0, 0.0 }; diff --git a/basic-geometry/tangent.h b/basic-geometry/tangent.h new file mode 100644 index 0000000..a0487eb --- /dev/null +++ b/basic-geometry/tangent.h @@ -0,0 +1,296 @@ +#ifndef _GEOMETRY_TANGENT_H_ +#define _GEOMETRY_TANGENT_H_ + +#include + +#include "basis.h" +#include "angle.h" +#include "vector2.h" +#include "matrix2x2.h" + +typedef struct +{ + float _cos, _sin; +} BgFP32Tangent; + +typedef struct +{ + double _cos, _sin; +} BgFP64Tangent; + +extern const BgFP32Tangent BG_FP32_IDLE_TANGENT; +extern const BgFP64Tangent BG_FP64_IDLE_TANGENT; + +// =================== Reset ==================== // + +static inline void bg_fp32_tangent_reset(BgFP32Tangent* tangent) +{ + tangent->_cos = 1.0f; + tangent->_sin = 0.0f; +} + +static inline void bg_fp64_tangent_reset(BgFP64Tangent* tangent) +{ + tangent->_cos = 1.0; + tangent->_sin = 0.0; +} + +// ==================== Copy ==================== // + +static inline void bg_fp32_tangent_copy(const BgFP32Tangent* from, BgFP32Tangent* to) +{ + to->_cos = from->_cos; + to->_sin = from->_sin; +} + +static inline void bg_fp64_tangent_copy(const BgFP64Tangent* from, BgFP64Tangent* to) +{ + to->_cos = from->_cos; + to->_sin = from->_sin; +} + +// ==================== Make ==================== // + +static inline void bg_fp32_tangent_set_values(const float x1, const float x2, BgFP32Tangent* tangent) +{ + const float square_module = x1 * x1 + x2 * x2; + + tangent->_cos = x1; + tangent->_sin = x2; + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_module && square_module <= 1.0f + BG_FP32_TWO_EPSYLON) { + return; + } + + if (square_module <= BG_FP32_SQUARE_EPSYLON) { + tangent->_cos = 1.0f; + tangent->_sin = 0.0f; + return; + } + + const float multiplier = sqrtf(1.0f / square_module); + + tangent->_cos = x1 * multiplier; + tangent->_sin = x2 * multiplier; +} + +static inline void bg_fp64_tangent_set_values(const double x1, const double x2, BgFP64Tangent* tangent) +{ + const double square_module = x1 * x1 + x2 * x2; + + tangent->_cos = x1; + tangent->_sin = x2; + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_module && square_module <= 1.0 + BG_FP64_TWO_EPSYLON) { + return; + } + + if (square_module <= BG_FP64_SQUARE_EPSYLON) { + tangent->_cos = 1.0; + tangent->_sin = 0.0; + return; + } + + const double multiplier = sqrt(1.0 / square_module); + + tangent->_cos = x1 * multiplier; + tangent->_sin = x2 * multiplier; +} + +// ================== Set Angle ================= // + +static inline void bg_fp32_tangent_set_angle(const float angle, const angle_unit_t unit, BgFP32Tangent* tangent) +{ + const float radians = bg_fp32_angle_to_radians(angle, unit); + + tangent->_cos = cosf(radians); + tangent->_sin = sinf(radians); +} + +static inline void bg_fp64_tangent_set_angle(const double angle, const angle_unit_t unit, BgFP64Tangent* tangent) +{ + const double radians = bg_fp64_angle_to_radians(angle, unit); + + tangent->_cos = cos(radians); + tangent->_sin = sin(radians); +} + +// ============= Copy to twin type ============== // + +static inline void bg_fp32_tangent_copy_to_double(const BgFP32Tangent* from, BgFP64Tangent* to) +{ + bg_fp64_tangent_set_values((double)from->_cos, (double)from->_sin, to); +} + +static inline void bg_fp64_tangent_copy_to_single(const BgFP64Tangent* from, BgFP32Tangent* to) +{ + bg_fp32_tangent_set_values((float)from->_cos, (float)from->_sin, to); +} + +// ================= Inversion ================== // + +static inline void bg_fp32_tangent_invert(BgFP32Tangent* tangent) +{ + tangent->_sin = -tangent->_sin; +} + +static inline void bg_fp64_tangent_invert(BgFP64Tangent* tangent) +{ + tangent->_sin = -tangent->_sin; +} + +// ================ Set Inverted ================ // + +static inline void bg_fp32_tangent_set_inverted(const BgFP32Tangent* tangent, BgFP32Tangent* result) +{ + result->_cos = tangent->_cos; + result->_sin = -tangent->_sin; +} + +static inline void bg_fp64_tangent_set_inverted(const BgFP64Tangent* tangent, BgFP64Tangent* result) +{ + result->_cos = tangent->_cos; + result->_sin = -tangent->_sin; +} + +// ============== Rotation Matrix =============== // + +static inline void bg_fp32_tangent_make_rotation_matrix(const BgFP32Tangent* tangent, BgFP32Matrix2x2* matrix) +{ + matrix->r1c1 = tangent->_cos; + matrix->r1c2 = -tangent->_sin; + matrix->r2c1 = tangent->_sin; + matrix->r2c2 = tangent->_cos; +} + +static inline void bg_fp64_tangent_make_rotation_matrix(const BgFP64Tangent* tangent, BgFP64Matrix2x2* matrix) +{ + matrix->r1c1 = tangent->_cos; + matrix->r1c2 = -tangent->_sin; + matrix->r2c1 = tangent->_sin; + matrix->r2c2 = tangent->_cos; +} + +// ============== Reverse Matrix ================ // + +static inline void bg_fp32_tangent_make_reverse_matrix(const BgFP32Tangent* tangent, BgFP32Matrix2x2* matrix) +{ + matrix->r1c1 = tangent->_cos; + matrix->r1c2 = tangent->_sin; + matrix->r2c1 = -tangent->_sin; + matrix->r2c2 = tangent->_cos; +} + +static inline void bg_fp64_tangent_make_reverse_matrix(const BgFP64Tangent* tangent, BgFP64Matrix2x2* matrix) +{ + matrix->r1c1 = tangent->_cos; + matrix->r1c2 = tangent->_sin; + matrix->r2c1 = -tangent->_sin; + matrix->r2c2 = tangent->_cos; +} + +// =================== Angle =================== // + +static inline float bg_fp32_tangent_get_angle(const BgFP32Tangent* tangent, const angle_unit_t unit) +{ + if (tangent->_cos >= 1.0f - BG_FP32_TWO_EPSYLON) { + return 0.0f; + } + + if (tangent->_cos <= -1.0f + BG_FP32_TWO_EPSYLON) { + return bg_fp32_angle_get_half_circle(unit); + } + + if (tangent->_sin >= 1.0f - BG_FP32_TWO_EPSYLON) { + return bg_fp32_angle_get_quater_circle(unit); + } + + if (tangent->_sin <= -1.0f + BG_FP32_TWO_EPSYLON) { + return 0.75f * bg_fp32_angle_get_full_circle(unit); + } + + return bg_fp32_radians_to_units(atan2f(tangent->_cos, tangent->_sin), unit); +} + +static inline double bg_fp64_tangent_get_angle(const BgFP64Tangent* tangent, const angle_unit_t unit) +{ + if (tangent->_cos >= 1.0 - BG_FP64_TWO_EPSYLON) { + return 0.0; + } + + if (tangent->_cos <= -1.0 + BG_FP64_TWO_EPSYLON) { + return bg_fp64_angle_get_half_circle(unit); + } + + if (tangent->_sin >= 1.0 - BG_FP64_TWO_EPSYLON) { + return bg_fp64_angle_get_quater_circle(unit); + } + + if (tangent->_sin <= -1.0 + BG_FP64_TWO_EPSYLON) { + return 0.75 * bg_fp64_angle_get_full_circle(unit); + } + + return bg_fp64_radians_to_units(atan2(tangent->_cos, tangent->_sin), unit); +} + +// ================ Combination ================= // + +static inline void bg_fp32_tangent_combine(const BgFP32Tangent* tangent1, const BgFP32Tangent* tangent2, BgFP32Tangent* result) +{ + bg_fp32_tangent_set_values( + tangent1->_cos * tangent2->_cos - tangent1->_sin * tangent2->_sin, + tangent1->_cos * tangent2->_sin + tangent1->_sin * tangent2->_cos, + result + ); +} + +static inline void bg_fp64_tangent_combine(const BgFP64Tangent* tangent1, const BgFP64Tangent* tangent2, BgFP64Tangent* result) +{ + bg_fp64_tangent_set_values( + tangent1->_cos * tangent2->_cos - tangent1->_sin * tangent2->_sin, + tangent1->_cos * tangent2->_sin + tangent1->_sin * tangent2->_cos, + result + ); +} + +// ================ Turn Vector ================= // + +static inline void bg_fp32_tangent_turn(const BgFP32Tangent* tangent, const BgFP32Vector2* vector, BgFP32Vector2* result) +{ + const float x1 = tangent->_cos * vector->x1 - tangent->_sin * vector->x2; + const float x2 = tangent->_sin * vector->x1 + tangent->_cos * vector->x2; + + result->x1 = x1; + result->x2 = x2; +} + +static inline void bg_fp64_tangent_turn(const BgFP64Tangent* tangent, const BgFP64Vector2* vector, BgFP64Vector2* result) +{ + const double x1 = tangent->_cos * vector->x1 - tangent->_sin * vector->x2; + const double x2 = tangent->_sin * vector->x1 + tangent->_cos * vector->x2; + + result->x1 = x1; + result->x2 = x2; +} + +// ============ Turn Vector Backward ============ // + +static inline void bg_fp32_tangent_turn_back(const BgFP32Tangent* tangent, const BgFP32Vector2* vector, BgFP32Vector2* result) +{ + const float x1 = tangent->_sin * vector->x2 + tangent->_cos * vector->x1; + const float x2 = tangent->_cos * vector->x2 - tangent->_sin * vector->x1; + + result->x1 = x1; + result->x2 = x2; +} + +static inline void bg_fp64_tangent_turn_back(const BgFP64Tangent* tangent, const BgFP64Vector2* vector, BgFP64Vector2* result) +{ + const double x1 = tangent->_sin * vector->x2 + tangent->_cos * vector->x1; + const double x2 = tangent->_cos * vector->x2 - tangent->_sin * vector->x1; + + result->x1 = x1; + result->x2 = x2; +} + +#endif diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index fdedb73..b05e72c 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -530,7 +530,7 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re // =========== Make Rotation Matrix3x3 ========== // -static inline void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +static inline void bg_fp32_versor_make_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) { const float s0s0 = versor->s0 * versor->s0; const float x1x1 = versor->x1 * versor->x1; @@ -558,7 +558,7 @@ static inline void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor matrix->r1c3 = x1x3 + s0x2; } -static inline void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +static inline void bg_fp64_versor_make_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) { const double s0s0 = versor->s0 * versor->s0; const double x1x1 = versor->x1 * versor->x1; @@ -588,7 +588,7 @@ static inline void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor // =========== Make Reverse Matrix3x3 =========== // -static inline void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +static inline void bg_fp32_versor_make_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) { const float s0s0 = versor->s0 * versor->s0; const float x1x1 = versor->x1 * versor->x1; @@ -616,7 +616,7 @@ static inline void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, matrix->r1c3 = x1x3 - s0x2; } -static inline void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +static inline void bg_fp64_versor_make_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) { const double s0s0 = versor->s0 * versor->s0; const double x1x1 = versor->x1 * versor->x1;