95 lines
3.9 KiB
C
95 lines
3.9 KiB
C
#include "./slerp.h"
|
|
|
|
extern inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp);
|
|
extern inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp);
|
|
|
|
extern inline void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
|
|
extern inline void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
|
|
|
|
extern inline void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
|
|
extern inline void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
|
|
|
|
extern inline void bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result);
|
|
extern inline void bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result);
|
|
|
|
void bgc_slerp_make_fp32(const BgcVersorFP32* start, const BgcVersorFP32* augment, BgcSlerpFP32* slerp)
|
|
{
|
|
const float square_vector = augment->x1 * augment->x1 + augment->x2 * augment->x2 + augment->x3 * augment->x3;
|
|
|
|
if (square_vector != square_vector) {
|
|
bgc_slerp_reset_fp32(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
|
|
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 = 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->s0);
|
|
|
|
const float multiplier = 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 = -multiplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3);
|
|
slerp->x1_sin_weight = multiplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2);
|
|
slerp->x2_sin_weight = multiplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1);
|
|
slerp->x3_sin_weight = multiplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2);
|
|
}
|
|
|
|
void bgc_slerp_make_fp64(const BgcVersorFP64* start, const BgcVersorFP64* augment, BgcSlerpFP64* slerp)
|
|
{
|
|
const double square_vector = augment->x1 * augment->x1 + augment->x2 * augment->x2 + augment->x3 * augment->x3;
|
|
|
|
if (square_vector != square_vector) {
|
|
bgc_slerp_reset_fp64(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
|
|
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 = 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->s0);
|
|
|
|
const double multiplier = 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 = -multiplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3);
|
|
slerp->x1_sin_weight = multiplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2);
|
|
slerp->x2_sin_weight = multiplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1);
|
|
slerp->x3_sin_weight = multiplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2);
|
|
}
|