Добавление сферической интерполяции, переход от применения acos к применению atan2, исправление ошибок
This commit is contained in:
parent
f06b35ae34
commit
9d7011e81e
17 changed files with 558 additions and 134 deletions
|
@ -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
|
||||
|
|
|
@ -21,8 +21,8 @@
|
|||
<ItemGroup>
|
||||
<ClInclude Include="angle.h" />
|
||||
<ClInclude Include="basic-geometry.h" />
|
||||
<ClInclude Include="complex.h" />
|
||||
<ClInclude Include="cotes-number.h" />
|
||||
<ClInclude Include="complex.h" />
|
||||
<ClInclude Include="cotes-number.h" />
|
||||
<ClInclude Include="matrix2x2.h" />
|
||||
<ClInclude Include="matrix2x3.h" />
|
||||
<ClInclude Include="matrix3x2.h" />
|
||||
|
@ -31,14 +31,15 @@
|
|||
<ClInclude Include="quaternion.h" />
|
||||
<ClInclude Include="rotation3.h" />
|
||||
<ClInclude Include="utilities.h" />
|
||||
<ClInclude Include="slerp.h" />
|
||||
<ClInclude Include="versor.h" />
|
||||
<ClInclude Include="vector2.h" />
|
||||
<ClInclude Include="vector3.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="angle.c" />
|
||||
<ClInclude Include="complex.c" />
|
||||
<ClInclude Include="cotes-number.c" />
|
||||
<ClInclude Include="complex.c" />
|
||||
<ClInclude Include="cotes-number.c" />
|
||||
<ClCompile Include="utilities.c" />
|
||||
<ClCompile Include="matrix2x2.c" />
|
||||
<ClCompile Include="matrix2x3.c" />
|
||||
|
@ -47,6 +48,7 @@
|
|||
<ClCompile Include="matrixes.c" />
|
||||
<ClCompile Include="quaternion.c" />
|
||||
<ClCompile Include="rotation3.c" />
|
||||
<ClCompile Include="slerp.c" />
|
||||
<ClCompile Include="versor.c" />
|
||||
<ClCompile Include="vector2.c" />
|
||||
<ClCompile Include="vector3.c" />
|
||||
|
|
|
@ -18,12 +18,12 @@
|
|||
<ClInclude Include="angle.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="complex.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="cotes-number.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="complex.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="cotes-number.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="utilities.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
|
@ -60,17 +60,20 @@
|
|||
<ClInclude Include="matrixes.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="complex.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="cotes-number.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="slerp.h">
|
||||
<Filter>Файлы заголовков</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="angle.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="complex.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="cotes-number.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="utilities.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
|
@ -104,5 +107,8 @@
|
|||
<ClCompile Include="matrix3x2.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="slerp.c">
|
||||
<Filter>Исходные файлы</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
</Project>
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -1,3 +1,4 @@
|
|||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
129
basic-geometry/slerp.c
Normal file
129
basic-geometry/slerp.c
Normal file
|
@ -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);
|
||||
}
|
94
basic-geometry/slerp.h
Normal file
94
basic-geometry/slerp.h
Normal file
|
@ -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
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue