95 lines
4.5 KiB
C
95 lines
4.5 KiB
C
#include "./slerp.h"
|
|
|
|
extern inline void bgc_fp32_slerp_reset(BGC_FP32_Slerp* slerp);
|
|
extern inline void bgc_fp64_slerp_reset(BGC_FP64_Slerp* slerp);
|
|
|
|
extern inline void bgc_fp32_slerp_make_full(BGC_FP32_Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end);
|
|
extern inline void bgc_fp64_slerp_make_full(BGC_FP64_Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end);
|
|
|
|
extern inline void bgc_fp32_slerp_make_shortened(BGC_FP32_Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end);
|
|
extern inline void bgc_fp64_slerp_make_shortened(BGC_FP64_Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end);
|
|
|
|
extern inline void bgc_fp32_slerp_get_phase_versor(BGC_FP32_Turn3* versor, const BGC_FP32_Slerp* slerp, const float phase);
|
|
extern inline void bgc_fp64_slerp_get_phase_versor(BGC_FP64_Turn3* versor, const BGC_FP64_Slerp* slerp, const double phase);
|
|
|
|
void bgc_fp32_slerp_make(BGC_FP32_Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* augment)
|
|
{
|
|
const float square_vector = augment->_versor.x1 * augment->_versor.x1 + augment->_versor.x2 * augment->_versor.x2 + augment->_versor.x3 * augment->_versor.x3;
|
|
|
|
if (square_vector != square_vector) {
|
|
bgc_fp32_slerp_reset(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_FP32_SQUARE_EPSILON) {
|
|
slerp->s0_cos_weight = start->_versor.s0;
|
|
slerp->x1_cos_weight = start->_versor.x1;
|
|
slerp->x2_cos_weight = start->_versor.x2;
|
|
slerp->x3_cos_weight = start->_versor.x3;
|
|
|
|
slerp->s0_sin_weight = 0.0f;
|
|
slerp->x1_sin_weight = 0.0f;
|
|
slerp->x2_sin_weight = 0.0f;
|
|
slerp->x3_sin_weight = 0.0f;
|
|
|
|
slerp->radians = 0.0f;
|
|
return;
|
|
}
|
|
|
|
const float vector_modulus = sqrtf(square_vector);
|
|
|
|
slerp->radians = atan2f(vector_modulus, augment->_versor.s0);
|
|
|
|
const float multiplier = 1.0f / vector_modulus;
|
|
|
|
slerp->s0_cos_weight = start->_versor.s0;
|
|
slerp->x1_cos_weight = start->_versor.x1;
|
|
slerp->x2_cos_weight = start->_versor.x2;
|
|
slerp->x3_cos_weight = start->_versor.x3;
|
|
|
|
slerp->s0_sin_weight = -multiplier * (augment->_versor.x1 * start->_versor.x1 + augment->_versor.x2 * start->_versor.x2 + augment->_versor.x3 * start->_versor.x3);
|
|
slerp->x1_sin_weight = multiplier * (augment->_versor.x1 * start->_versor.s0 + augment->_versor.x2 * start->_versor.x3 - augment->_versor.x3 * start->_versor.x2);
|
|
slerp->x2_sin_weight = multiplier * (augment->_versor.x2 * start->_versor.s0 - augment->_versor.x1 * start->_versor.x3 + augment->_versor.x3 * start->_versor.x1);
|
|
slerp->x3_sin_weight = multiplier * (augment->_versor.x3 * start->_versor.s0 - augment->_versor.x2 * start->_versor.x1 + augment->_versor.x1 * start->_versor.x2);
|
|
}
|
|
|
|
void bgc_fp64_slerp_make(BGC_FP64_Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* augment)
|
|
{
|
|
const double square_vector = augment->_versor.x1 * augment->_versor.x1 + augment->_versor.x2 * augment->_versor.x2 + augment->_versor.x3 * augment->_versor.x3;
|
|
|
|
if (square_vector != square_vector) {
|
|
bgc_fp64_slerp_reset(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_FP64_SQUARE_EPSILON) {
|
|
slerp->s0_cos_weight = start->_versor.s0;
|
|
slerp->x1_cos_weight = start->_versor.x1;
|
|
slerp->x2_cos_weight = start->_versor.x2;
|
|
slerp->x3_cos_weight = start->_versor.x3;
|
|
|
|
slerp->s0_sin_weight = 0.0;
|
|
slerp->x1_sin_weight = 0.0;
|
|
slerp->x2_sin_weight = 0.0;
|
|
slerp->x3_sin_weight = 0.0;
|
|
|
|
slerp->radians = 0.0;
|
|
return;
|
|
}
|
|
|
|
const double vector_modulus = sqrt(square_vector);
|
|
|
|
slerp->radians = atan2(vector_modulus, augment->_versor.s0);
|
|
|
|
const double multiplier = 1.0 / vector_modulus;
|
|
|
|
slerp->s0_cos_weight = start->_versor.s0;
|
|
slerp->x1_cos_weight = start->_versor.x1;
|
|
slerp->x2_cos_weight = start->_versor.x2;
|
|
slerp->x3_cos_weight = start->_versor.x3;
|
|
|
|
slerp->s0_sin_weight = -multiplier * (augment->_versor.x1 * start->_versor.x1 + augment->_versor.x2 * start->_versor.x2 + augment->_versor.x3 * start->_versor.x3);
|
|
slerp->x1_sin_weight = multiplier * (augment->_versor.x1 * start->_versor.s0 + augment->_versor.x2 * start->_versor.x3 - augment->_versor.x3 * start->_versor.x2);
|
|
slerp->x2_sin_weight = multiplier * (augment->_versor.x2 * start->_versor.s0 - augment->_versor.x1 * start->_versor.x3 + augment->_versor.x3 * start->_versor.x1);
|
|
slerp->x3_sin_weight = multiplier * (augment->_versor.x3 * start->_versor.s0 - augment->_versor.x2 * start->_versor.x1 + augment->_versor.x1 * start->_versor.x2);
|
|
}
|