diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c
index 901b4e3..6dcf2ae 100644
--- a/basic-geometry-dev/main.c
+++ b/basic-geometry-dev/main.c
@@ -138,17 +138,36 @@ int main() {
return 0;
}
*/
-
+/*
int main() {
- const float exponent = 2.0f;
-
- BgcVersorFP32 turn, result;
-
- bgc_versor_set_turn_fp32(0, 0, 1, 120, BGC_ANGLE_UNIT_DEGREES, &turn);
-
- bgc_versor_get_exponation_fp32(&turn, exponent, &result);
-
- printf("(%f, %f, %f, %f) ^ %f = (%f, %f, %f, %f)\n", turn.s0, turn.x1, turn.x2, turn.x3, exponent, result.s0, result.x1, result.x2, result.x3);
-
+ BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f };
+ BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f };
+ BgcVersorFP32 result;
+ bgc_versor_spherical_interpolation_fp32(&start, &end, 0.5f, &result);
+ printf("Result: %0.12f, %0.12f, %0.12f, %0.12f\n", result.s0, result.x1, result.x2, result.x3);
+ return 0;
+}
+*/
+
+int main() {
+ //BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f };
+ //BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f };
+ /*
+ BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f };
+ BgcVersorFP32 end = { 0.9999f, 0.01414f, 0.0f, 0.0f };
+ BgcSlerpFP32 slerp;
+ BgcVersorFP32 result;
+ bgc_slerp_make_fp32(&start, &end, &slerp);
+ bgc_slerp_get_turn_for_phase_fp32(&slerp, 0.5f, &result);
+ 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);
return 0;
}
diff --git a/basic-geometry/basic-geometry.h b/basic-geometry/basic-geometry.h
index fb83f61..fcaf5bb 100644
--- a/basic-geometry/basic-geometry.h
+++ b/basic-geometry/basic-geometry.h
@@ -13,7 +13,7 @@
#include "./matrix2x3.h"
#include "./matrix3x2.h"
#include "./matrix3x3.h"
-
+
#include "./complex.h"
#include "./cotes-number.h"
@@ -21,5 +21,6 @@
#include "./quaternion.h"
#include "./versor.h"
+#include "./slerp.h"
#endif
diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj
index 94fb723..3614dc7 100644
--- a/basic-geometry/basic-geometry.vcxproj
+++ b/basic-geometry/basic-geometry.vcxproj
@@ -21,8 +21,8 @@
-
-
+
+
@@ -31,14 +31,15 @@
+
-
-
+
+
@@ -47,6 +48,7 @@
+
diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters
index 260e3e3..440dcb1 100644
--- a/basic-geometry/basic-geometry.vcxproj.filters
+++ b/basic-geometry/basic-geometry.vcxproj.filters
@@ -18,12 +18,12 @@
Файлы заголовков
-
- Файлы заголовков
-
-
- Файлы заголовков
-
+
+ Файлы заголовков
+
+
+ Файлы заголовков
+
Файлы заголовков
@@ -60,17 +60,20 @@
Файлы заголовков
+
+ Исходные файлы
+
+
+ Исходные файлы
+
+
+ Файлы заголовков
+
Исходные файлы
-
- Исходные файлы
-
-
- Исходные файлы
-
Исходные файлы
@@ -104,5 +107,8 @@
Исходные файлы
+
+ Исходные файлы
+
\ No newline at end of file
diff --git a/basic-geometry/complex.c b/basic-geometry/complex.c
index 59c6979..ef3a0bd 100644
--- a/basic-geometry/complex.c
+++ b/basic-geometry/complex.c
@@ -69,8 +69,8 @@ extern inline void bgc_complex_get_mean_of_two_fp64(const BgcComplexFP64* number
extern inline void bgc_complex_get_mean_of_three_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const BgcComplexFP32* number3, BgcComplexFP32* mean);
extern inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const BgcComplexFP64* number3, BgcComplexFP64* mean);
-extern inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation);
-extern inline void bgc_complex_get_linear_interpolation_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation);
+extern inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation);
+extern inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation);
extern inline void bgc_complex_minimize_fp32(const BgcComplexFP32* number, BgcComplexFP32* minimal);
extern inline void bgc_complex_minimize_fp64(const BgcComplexFP64* number, BgcComplexFP64* minimal);
diff --git a/basic-geometry/complex.h b/basic-geometry/complex.h
index c7b77ff..e551609 100644
--- a/basic-geometry/complex.h
+++ b/basic-geometry/complex.h
@@ -428,7 +428,7 @@ inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, co
// =================== Linear =================== //
-inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation)
+inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation)
{
const float counterphase = 1.0f - phase;
@@ -436,7 +436,7 @@ inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* numb
interpolation->imaginary = number1->imaginary * counterphase + number2->imaginary * phase;
}
-inline void bgc_complex_get_linear_interpolation_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation)
+inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation)
{
const double counterphase = 1.0 - phase;
diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c
index 963dba7..df057b2 100644
--- a/basic-geometry/quaternion.c
+++ b/basic-geometry/quaternion.c
@@ -1,3 +1,4 @@
+#include
#include "quaternion.h"
extern inline void bgc_quaternion_reset_fp32(BgcQuaternionFP32* quaternion);
@@ -63,8 +64,8 @@ extern inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multipl
extern inline void bgc_quaternion_divide_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient);
extern inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient);
-extern inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation);
-extern inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation);
+extern inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation);
+extern inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation);
extern inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation);
extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation);
@@ -75,6 +76,89 @@ extern inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64
extern inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse);
extern inline int bgc_quaternion_get_both_matrixes_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse);
+extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
+extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
-extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
-extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2);
+// =============== Get Exponation =============== //
+
+int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power)
+{
+ const float s0s0 = base->s0 * base->s0;
+ const float x1x1 = base->x1 * base->x1;
+ const float x2x2 = base->x2 * base->x2;
+ const float x3x3 = base->x3 * base->x3;
+
+ const float square_vector = x1x1 + (x2x2 + x3x3);
+ const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
+
+ // square_modulus != square_modulus means checking for NaN value at square_modulus
+ if (square_modulus != square_modulus) {
+ return 0;
+ }
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
+ if (base->s0 < 0.0f) {
+ return 0;
+ }
+
+ power->s0 = powf(base->s0, exponent);
+ power->x1 = 0.0f;
+ power->x2 = 0.0f;
+ power->x3 = 0.0f;
+
+ return 1;
+ }
+
+ const float vector_modulus = sqrtf(square_vector);
+ const float power_angle = atan2f(vector_modulus, base->s0) * exponent;
+ const float power_modulus = powf(square_modulus, 0.5f * exponent);
+ const float multiplier = power_modulus * sinf(power_angle) / vector_modulus;
+
+ power->s0 = power_modulus * cosf(power_angle);
+ power->x1 = base->x1 * multiplier;
+ power->x2 = base->x2 * multiplier;
+ power->x3 = base->x3 * multiplier;
+
+ return 1;
+}
+
+int bgc_quaternion_get_exponation_fp64(const BgcQuaternionFP64* base, const double exponent, BgcQuaternionFP64* power)
+{
+ const double s0s0 = base->s0 * base->s0;
+ const double x1x1 = base->x1 * base->x1;
+ const double x2x2 = base->x2 * base->x2;
+ const double x3x3 = base->x3 * base->x3;
+
+ const double square_vector = x1x1 + (x2x2 + x3x3);
+ const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
+
+ // square_modulus != square_modulus means checking for NaN value at square_modulus
+ if (square_modulus != square_modulus) {
+ return 0;
+ }
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
+ if (base->s0 < 0.0) {
+ return 0;
+ }
+
+ power->s0 = pow(base->s0, exponent);
+ power->x1 = 0.0;
+ power->x2 = 0.0;
+ power->x3 = 0.0;
+
+ return 1;
+ }
+
+ const double vector_modulus = sqrt(square_vector);
+ const double power_angle = atan2(vector_modulus, base->s0) * exponent;
+ const double power_modulus = pow(square_modulus, 0.5 * exponent);
+ const double multiplier = power_modulus * sin(power_angle) / vector_modulus;
+
+ power->s0 = power_modulus * cos(power_angle);
+ power->x1 = base->x1 * multiplier;
+ power->x2 = base->x2 * multiplier;
+ power->x3 = base->x3 * multiplier;
+
+ return 1;
+}
diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h
index 54be3fa..ebf06db 100644
--- a/basic-geometry/quaternion.h
+++ b/basic-geometry/quaternion.h
@@ -473,9 +473,9 @@ inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const
bgc_quaternion_multiply_fp64(dividend, 1.0 / divisor, quotient);
}
-// =================== Linear =================== //
+// ============ Linear Interpolation ============ //
-inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation)
+inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation)
{
const float counterphase = 1.0f - phase;
@@ -485,7 +485,7 @@ inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32
interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase;
}
-inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation)
+inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation)
{
const double counterphase = 1.0 - phase;
@@ -495,6 +495,12 @@ inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64
interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase;
}
+// =============== Get Exponation =============== //
+
+int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power);
+
+int bgc_quaternion_get_exponation_fp64(const BgcQuaternionFP64* base, const double exponent, BgcQuaternionFP64* power);
+
// ============ Get Rotation Matrix ============= //
inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation)
diff --git a/basic-geometry/slerp.c b/basic-geometry/slerp.c
new file mode 100644
index 0000000..b318059
--- /dev/null
+++ b/basic-geometry/slerp.c
@@ -0,0 +1,129 @@
+#include "./slerp.h"
+
+extern inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp);
+extern inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp);
+
+extern inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result);
+extern inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result);
+
+void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp)
+{
+ BgcVersorFP32 delta;
+
+ bgc_versor_exclude_fp32(end, start, &delta);
+
+ const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
+ bgc_slerp_reset_fp32(slerp);
+ return;
+ }
+
+ const float vector_modulus = sqrtf(square_vector);
+
+ slerp->radians = atan2f(vector_modulus, delta.s0);
+
+ const float mutliplier = 1.0f / vector_modulus;
+
+ slerp->s0_cos_weight = start->s0;
+ slerp->x1_cos_weight = start->x1;
+ slerp->x2_cos_weight = start->x2;
+ slerp->x3_cos_weight = start->x3;
+
+ slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
+ slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
+ slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
+ slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
+}
+
+void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp)
+{
+ BgcVersorFP64 delta;
+
+ bgc_versor_exclude_fp64(end, start, &delta);
+
+ const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
+ bgc_slerp_reset_fp64(slerp);
+ return;
+ }
+
+ const double vector_modulus = sqrt(square_vector);
+
+ slerp->radians = atan2(vector_modulus, delta.s0);
+
+ const double mutliplier = 1.0 / vector_modulus;
+
+ slerp->s0_cos_weight = start->s0;
+ slerp->x1_cos_weight = start->x1;
+ slerp->x2_cos_weight = start->x2;
+ slerp->x3_cos_weight = start->x3;
+
+ slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
+ slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
+ slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
+ slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
+}
+
+void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp)
+{
+ BgcVersorFP32 delta;
+
+ bgc_versor_exclude_fp32(end, start, &delta);
+ bgc_versor_shorten_fp32(&delta, &delta);
+
+ const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
+ bgc_slerp_reset_fp32(slerp);
+ return;
+ }
+
+ const float vector_modulus = sqrtf(square_vector);
+
+ slerp->radians = atan2f(vector_modulus, delta.s0);
+
+ const float mutliplier = 1.0f / vector_modulus;
+
+ slerp->s0_cos_weight = start->s0;
+ slerp->x1_cos_weight = start->x1;
+ slerp->x2_cos_weight = start->x2;
+ slerp->x3_cos_weight = start->x3;
+
+ slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
+ slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
+ slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
+ slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
+}
+
+void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp)
+{
+ BgcVersorFP64 delta;
+
+ bgc_versor_exclude_fp64(end, start, &delta);
+ bgc_versor_shorten_fp64(&delta, &delta);
+
+ const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;
+
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
+ bgc_slerp_reset_fp64(slerp);
+ return;
+ }
+
+ const double vector_modulus = sqrt(square_vector);
+
+ slerp->radians = atan2(vector_modulus, delta.s0);
+
+ const double mutliplier = 1.0 / vector_modulus;
+
+ slerp->s0_cos_weight = start->s0;
+ slerp->x1_cos_weight = start->x1;
+ slerp->x2_cos_weight = start->x2;
+ slerp->x3_cos_weight = start->x3;
+
+ slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
+ slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
+ slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
+ slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
+}
diff --git a/basic-geometry/slerp.h b/basic-geometry/slerp.h
new file mode 100644
index 0000000..9289008
--- /dev/null
+++ b/basic-geometry/slerp.h
@@ -0,0 +1,94 @@
+#ifndef _BGC_VERSOR_SLERP_H_
+#define _BGC_VERSOR_SLERP_H_
+
+#include "./versor.h"
+
+typedef struct {
+ float s0_cos_weight, s0_sin_weight;
+ float x1_cos_weight, x1_sin_weight;
+ float x2_cos_weight, x2_sin_weight;
+ float x3_cos_weight, x3_sin_weight;
+ float radians;
+} BgcSlerpFP32;
+
+typedef struct {
+ double s0_cos_weight, s0_sin_weight;
+ double x1_cos_weight, x1_sin_weight;
+ double x2_cos_weight, x2_sin_weight;
+ double x3_cos_weight, x3_sin_weight;
+ double radians;
+} BgcSlerpFP64;
+
+inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp)
+{
+ slerp->s0_cos_weight = 1.0f;
+ slerp->s0_sin_weight = 0.0f;
+
+ slerp->x1_cos_weight = 0.0f;
+ slerp->x1_sin_weight = 0.0f;
+
+ slerp->x2_cos_weight = 0.0f;
+ slerp->x2_sin_weight = 0.0f;
+
+ slerp->x3_cos_weight = 0.0f;
+ slerp->x3_sin_weight = 0.0f;
+
+ slerp->radians = 0.0f;
+}
+
+inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp)
+{
+ slerp->s0_cos_weight = 1.0;
+ slerp->s0_sin_weight = 0.0;
+
+ slerp->x1_cos_weight = 0.0;
+ slerp->x1_sin_weight = 0.0;
+
+ slerp->x2_cos_weight = 0.0;
+ slerp->x2_sin_weight = 0.0;
+
+ slerp->x3_cos_weight = 0.0;
+ slerp->x3_sin_weight = 0.0;
+
+ slerp->radians = 0.0;
+}
+
+void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
+
+void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
+
+void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
+
+void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
+
+inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result)
+{
+ const float angle = slerp->radians * phase;
+ const float cosine = cosf(angle);
+ const float sine = sinf(angle);
+
+ bgc_versor_set_values_fp32(
+ slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine,
+ slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine,
+ slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine,
+ slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine,
+ result
+ );
+}
+
+inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result)
+{
+ const double angle = slerp->radians * phase;
+ const double cosine = cos(angle);
+ const double sine = sin(angle);
+
+ bgc_versor_set_values_fp64(
+ slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine,
+ slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine,
+ slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine,
+ slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine,
+ result
+ );
+}
+
+#endif
diff --git a/basic-geometry/utilities.h b/basic-geometry/utilities.h
index 09d4866..4c4d3cb 100644
--- a/basic-geometry/utilities.h
+++ b/basic-geometry/utilities.h
@@ -11,6 +11,8 @@
#define BGC_ONE_SEVENTH_FP32 0.142857142857f
#define BGC_ONE_NINETH_FP32 0.1111111111f
+#define BGC_ARCCOSINE_PRECISION_LIMIT_FP32 0.70711f
+
#define BGC_GOLDEN_RATIO_HIGH_FP32 1.618034f
#define BGC_GOLDEN_RATIO_LOW_FP32 0.618034f
diff --git a/basic-geometry/vector2.c b/basic-geometry/vector2.c
index b0b7c64..637de61 100644
--- a/basic-geometry/vector2.c
+++ b/basic-geometry/vector2.c
@@ -57,8 +57,8 @@ extern inline void bgc_vector2_get_mean_of_two_fp64(const BgcVector2FP64* vector
extern inline void bgc_vector2_get_mean_of_three_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const BgcVector2FP32* vector3, BgcVector2FP32* mean);
extern inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcVector2FP64* vector3, BgcVector2FP64* mean);
-extern inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation);
-extern inline void bgc_vector2_get_linear_interpolation_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation);
+extern inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation);
+extern inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation);
extern inline void bgc_vector2_minimize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* minimal);
extern inline void bgc_vector2_minimize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* minimal);
@@ -90,52 +90,44 @@ float bgc_vector2_get_angle_fp32(const BgcVector2FP32* vector1, const BgcVector2
{
const float square_modulus1 = bgc_vector2_get_square_modulus_fp32(vector1);
- if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) {
+ // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1
+ if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus1 != square_modulus1) {
return 0.0f;
}
const float square_modulus2 = bgc_vector2_get_square_modulus_fp32(vector2);
- if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) {
+ // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2
+ if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 != square_modulus2) {
return 0.0f;
}
- const float cosine = bgc_vector2_get_scalar_product_fp32(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);
+ const float scalar = bgc_vector2_get_scalar_product_fp32(vector1, vector2);
- if (cosine >= 1.0f - BGC_EPSYLON_FP32) {
- return 0.0f;
- }
+ const float cross = bgc_vector2_get_cross_product_fp32(vector1, vector2);
- if (cosine <= -1.0f + BGC_EPSYLON_FP32) {
- return bgc_angle_get_half_circle_fp32(unit);
- }
-
- return bgc_radians_to_units_fp32(acosf(cosine), unit);
+ return bgc_radians_to_units_fp32(atan2f(cross >= 0 ? cross : -cross, scalar), unit);
}
double bgc_vector2_get_angle_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcAngleUnitEnum unit)
{
const double square_modulus1 = bgc_vector2_get_square_modulus_fp64(vector1);
- if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) {
+ // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1
+ if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus1 != square_modulus1) {
return 0.0;
}
const double square_modulus2 = bgc_vector2_get_square_modulus_fp64(vector2);
- if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) {
+ // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2
+ if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 != square_modulus2) {
return 0.0;
}
- const double cosine = bgc_vector2_get_scalar_product_fp64(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);
+ const double scalar = bgc_vector2_get_scalar_product_fp64(vector1, vector2);
- if (cosine >= 1.0 - BGC_EPSYLON_FP64) {
- return 0.0;
- }
+ const double cross = bgc_vector2_get_cross_product_fp64(vector1, vector2);
- if (cosine <= -1.0 + BGC_EPSYLON_FP64) {
- return bgc_angle_get_half_circle_fp64(unit);
- }
-
- return bgc_radians_to_units_fp64(acos(cosine), unit);
+ return bgc_radians_to_units_fp64(atan2(cross >= 0 ? cross : -cross, scalar), unit);
}
diff --git a/basic-geometry/vector2.h b/basic-geometry/vector2.h
index 8852721..1effc5e 100644
--- a/basic-geometry/vector2.h
+++ b/basic-geometry/vector2.h
@@ -314,7 +314,7 @@ inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, co
// =================== Linear =================== //
-inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation)
+inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation)
{
const float counterphase = 1.0f - phase;
@@ -322,7 +322,7 @@ inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vect
interpolation->x2 = vector1->x2 * counterphase + vector2->x2 * phase;
}
-inline void bgc_vector2_get_linear_interpolation_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation)
+inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation)
{
const double counterphase = 1.0 - phase;
diff --git a/basic-geometry/vector3.c b/basic-geometry/vector3.c
index e15ac8d..8c269ed 100644
--- a/basic-geometry/vector3.c
+++ b/basic-geometry/vector3.c
@@ -57,8 +57,8 @@ extern inline void bgc_vector3_get_mean_of_two_fp64(const BgcVector3FP64* vector
extern inline void bgc_vector3_get_mean_of_three_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* result);
extern inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* result);
-extern inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation);
-extern inline void bgc_vector3_get_linear_interpolation_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation);
+extern inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation);
+extern inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation);
extern inline void bgc_vector3_minimize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* minimal);
extern inline void bgc_vector3_minimize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* minimal);
@@ -96,52 +96,52 @@ float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3
{
const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1);
- if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) {
+ // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1
+ if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus1 != square_modulus1) {
return 0.0f;
}
const float square_modulus2 = bgc_vector3_get_square_modulus_fp32(vector2);
- if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) {
+ // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2
+ if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 != square_modulus2) {
return 0.0f;
}
- const float cosine = bgc_vector3_get_scalar_product_fp32(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2);
+ BgcVector3FP32 cross_product;
- if (cosine >= 1.0f - BGC_EPSYLON_FP32) {
- return 0.0f;
- }
+ bgc_vector3_get_cross_product_fp32(vector1, vector2, &cross_product);
- if (cosine <= -1.0f + BGC_EPSYLON_FP32) {
- return bgc_angle_get_half_circle_fp32(unit);
- }
+ const float scalar = bgc_vector3_get_scalar_product_fp32(vector1, vector2);
- return bgc_radians_to_units_fp32(acosf(cosine), unit);
+ const float cross = bgc_vector3_get_modulus_fp32(&cross_product);
+
+ return bgc_radians_to_units_fp32(atan2f(cross, scalar), unit);
}
double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum unit)
{
const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1);
- if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) {
+ // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1
+ if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus1 != square_modulus1) {
return 0.0;
}
const double square_modulus2 = bgc_vector3_get_square_modulus_fp64(vector2);
- if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) {
+ // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2
+ if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 != square_modulus2) {
return 0.0;
}
- const double cosine = bgc_vector3_get_scalar_product_fp64(vector1, vector2) / sqrt(square_modulus1 * square_modulus2);
+ BgcVector3FP64 cross_product;
- if (cosine >= 1.0 - BGC_EPSYLON_FP64) {
- return 0.0;
- }
+ bgc_vector3_get_cross_product_fp64(vector1, vector2, &cross_product);
- if (cosine <= -1.0 + BGC_EPSYLON_FP64) {
- return bgc_angle_get_half_circle_fp64(unit);
- }
+ const double scalar = bgc_vector3_get_scalar_product_fp64(vector1, vector2);
- return bgc_radians_to_units_fp64(acos(cosine), unit);
+ const double cross = bgc_vector3_get_modulus_fp64(&cross_product);
+
+ return bgc_radians_to_units_fp64(atan2(cross, scalar), unit);
}
diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h
index 33451fe..52fe86c 100644
--- a/basic-geometry/vector3.h
+++ b/basic-geometry/vector3.h
@@ -350,7 +350,7 @@ inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, co
// =================== Linear =================== //
-inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation)
+inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation)
{
const float counterphase = 1.0f - phase;
@@ -359,7 +359,7 @@ inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vect
interpolation->x3 = vector1->x3 * counterphase + vector2->x3 * phase;
}
-inline void bgc_vector3_get_linear_interpolation_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation)
+inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation)
{
const double counterphase = 1.0 - phase;
diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c
index 8405012..842fc40 100644
--- a/basic-geometry/versor.c
+++ b/basic-geometry/versor.c
@@ -81,7 +81,6 @@ void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
-
}
void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin)
@@ -152,54 +151,22 @@ 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);
}
-// ================ Get Rotation ================ //
-
-void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result)
-{
- if (versor->s0 <= -(1.0f - BGC_EPSYLON_FP32) || 1.0f - BGC_EPSYLON_FP32 <= versor->s0) {
- bgc_rotation3_reset_fp32(result);
- return;
- }
-
- const float multiplier = sqrtf(1.0f / (versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3));
-
- result->radians = 2.0f * acosf(versor->s0);
-
- result->axis.x1 = versor->x1 * multiplier;
- result->axis.x2 = versor->x2 * multiplier;
- result->axis.x3 = versor->x3 * multiplier;
-}
-
-void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result)
-{
- if (versor->s0 <= -(1.0 - BGC_EPSYLON_FP64) || 1.0 - BGC_EPSYLON_FP64 <= versor->s0) {
- bgc_rotation3_reset_fp64(result);
- return;
- }
-
- const double multiplier = sqrt(1.0 / (versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3));
-
- result->radians = 2.0 * acos(versor->s0);
-
- result->axis.x1 = versor->x1 * multiplier;
- result->axis.x2 = versor->x2 * multiplier;
- result->axis.x3 = versor->x3 * multiplier;
-}
-
// =============== Get Exponation =============== //
void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power)
{
const float square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3;
- if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
bgc_versor_reset_fp32(power);
return;
}
- const float angle = acosf(base->s0) * exponent;
+ const float vector_modulus = sqrtf(square_vector);
- const float multiplier = sinf(angle) / sqrtf(square_vector);
+ const float angle = atan2f(vector_modulus, base->s0) * exponent;
+
+ const float multiplier = sinf(angle) / vector_modulus;
bgc_versor_set_values_fp32(cosf(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power);
}
@@ -208,14 +175,130 @@ void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double expo
{
const double square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3;
- if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
bgc_versor_reset_fp64(power);
return;
}
- const double angle = acos(base->s0) * exponent;
+ const double vector_modulus = sqrt(square_vector);
- const double multiplier = sin(angle) / sqrt(square_vector);
+ const double angle = atan2(vector_modulus, base->s0) * exponent;
+
+ const double multiplier = sin(angle) / vector_modulus;
bgc_versor_set_values_fp64(cos(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power);
}
+
+// ============ Sphere Interpolation ============ //
+
+void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result)
+{
+ const float delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3);
+ const float delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3);
+ const float delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1);
+ const float delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2);
+
+ const float square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;
+
+ // square_vector != square_vector means checking for NaN value at square_vector
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
+ bgc_versor_copy_fp32(end, result);
+ return;
+ }
+
+ // Calculating of the turning which fits the phase:
+ const float vector_modulus = sqrtf(square_vector);
+ const float angle = atan2f(vector_modulus, delta_s0) * phase;
+ const float multiplier = sinf(angle) / vector_modulus;
+
+ const float turn_s0 = cosf(angle);
+ const float turn_x1 = delta_x1 * multiplier;
+ const float turn_x2 = delta_x2 * multiplier;
+ const float turn_x3 = delta_x3 * multiplier;
+
+ // Combining of starting orientation with the turning
+ bgc_versor_set_values_fp32(
+ (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3),
+ (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3),
+ (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1),
+ (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2),
+ result
+ );
+}
+
+void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result)
+{
+ const double delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3);
+ const double delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3);
+ const double delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1);
+ const double delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2);
+
+ const double square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;
+
+ // square_vector != square_vector means checking for NaN value at square_vector
+ if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
+ bgc_versor_copy_fp64(end, result);
+ return;
+ }
+
+ // Calculating of the turning which fits the phase:
+ const double vector_modulus = sqrt(square_vector);
+ const double angle = atan2(vector_modulus, delta_s0) * phase;
+ const double multiplier = sin(angle) / vector_modulus;
+
+ const double turn_s0 = cos(angle);
+ const double turn_x1 = delta_x1 * multiplier;
+ const double turn_x2 = delta_x2 * multiplier;
+ const double turn_x3 = delta_x3 * multiplier;
+
+ // Combining of starting orientation with the turning
+ bgc_versor_set_values_fp64(
+ (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3),
+ (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3),
+ (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1),
+ (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2),
+ result
+ );
+}
+
+// ================ Get Rotation ================ //
+
+void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result)
+{
+ const float square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;
+
+ if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
+ bgc_rotation3_reset_fp32(result);
+ return;
+ }
+
+ const float vector_modulus = sqrtf(square_modulus);
+
+ const float multiplier = 1.0f / vector_modulus;
+
+ result->radians = 2.0f * atan2f(vector_modulus, versor->s0);
+
+ result->axis.x1 = versor->x1 * multiplier;
+ result->axis.x2 = versor->x2 * multiplier;
+ result->axis.x3 = versor->x3 * multiplier;
+}
+
+void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result)
+{
+ const double square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;
+
+ if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
+ bgc_rotation3_reset_fp64(result);
+ return;
+ }
+
+ const double vector_modulus = sqrt(square_modulus);
+
+ const double multiplier = 1.0 / vector_modulus;
+
+ result->radians = 2.0 * atan2(vector_modulus, versor->s0);
+
+ result->axis.x1 = versor->x1 * multiplier;
+ result->axis.x2 = versor->x2 * multiplier;
+ result->axis.x3 = versor->x3 * multiplier;
+}
diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h
index 6240b0f..c516c8d 100644
--- a/basic-geometry/versor.h
+++ b/basic-geometry/versor.h
@@ -223,14 +223,14 @@ inline void bgc_versor_shorten_fp32(const BgcVersorFP32* versor, BgcVersorFP32*
_BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)shortened;
if (versor->s0 >= 0.0f) {
- twin->x1 = versor->s0;
+ twin->s0 = versor->s0;
twin->x1 = versor->x1;
twin->x2 = versor->x2;
twin->x3 = versor->x3;
return;
}
- twin->x1 = -versor->s0;
+ twin->s0 = -versor->s0;
twin->x1 = -versor->x1;
twin->x2 = -versor->x2;
twin->x3 = -versor->x3;
@@ -241,14 +241,14 @@ inline void bgc_versor_shorten_fp64(const BgcVersorFP64* versor, BgcVersorFP64*
_BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)shortened;
if (versor->s0 >= 0.0) {
- twin->x1 = versor->s0;
+ twin->s0 = versor->s0;
twin->x1 = versor->x1;
twin->x2 = versor->x2;
twin->x3 = versor->x3;
return;
}
- twin->x1 = -versor->s0;
+ twin->s0 = -versor->s0;
twin->x1 = -versor->x1;
twin->x2 = -versor->x2;
twin->x3 = -versor->x3;
@@ -362,6 +362,12 @@ inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP
);
}
+// ============ Sphere Interpolation ============ //
+
+void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result);
+
+void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result);
+
// ================ Get Rotation ================ //
void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result);