104 lines
5.7 KiB
C
104 lines
5.7 KiB
C
#include "./slerp3.h"
|
|
|
|
extern inline void bgc_fp32_slerp3_reset(BGC_FP32_Slerp3* const slerp);
|
|
extern inline void bgc_fp64_slerp3_reset(BGC_FP64_Slerp3* const slerp);
|
|
|
|
extern inline void bgc_fp32_slerp3_make_full(BGC_FP32_Slerp3* const slerp, const BGC_FP32_Turn3* const start, const BGC_FP32_Turn3* const end);
|
|
extern inline void bgc_fp64_slerp3_make_full(BGC_FP64_Slerp3* const slerp, const BGC_FP64_Turn3* const start, const BGC_FP64_Turn3* const end);
|
|
|
|
extern inline void bgc_fp32_slerp3_make_shortened(BGC_FP32_Slerp3* const slerp, const BGC_FP32_Turn3* const start, const BGC_FP32_Turn3* const end);
|
|
extern inline void bgc_fp64_slerp3_make_shortened(BGC_FP64_Slerp3* const slerp, const BGC_FP64_Turn3* const start, const BGC_FP64_Turn3* const end);
|
|
|
|
extern inline void bgc_fp32_slerp3_get_phase_turn(BGC_FP32_Turn3* const versor, const BGC_FP32_Slerp3* const slerp, const float phase);
|
|
extern inline void bgc_fp64_slerp3_get_phase_turn(BGC_FP64_Turn3* const versor, const BGC_FP64_Slerp3* const slerp, const double phase);
|
|
|
|
extern inline void bgc_fp32_slerp3_get_phase_rotation_matrix(BGC_FP32_Matrix3x3* const rotation_matrix, const BGC_FP32_Slerp3* const slerp, const float phase);
|
|
extern inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* const rotation_matrix, const BGC_FP64_Slerp3* const slerp, const double phase);
|
|
|
|
extern inline void bgc_fp32_slerp3_get_phase_reverse_matrix(BGC_FP32_Matrix3x3* const reverse_matrix, const BGC_FP32_Slerp3* const slerp, const float phase);
|
|
extern inline void bgc_fp64_slerp3_get_phase_reverse_matrix(BGC_FP64_Matrix3x3* const reverse_matrix, const BGC_FP64_Slerp3* const slerp, const double phase);
|
|
|
|
extern inline void bgc_fp32_slerp3_get_phase_both_matrices(BGC_FP32_Matrix3x3* const rotation_matrix, BGC_FP32_Matrix3x3* const reverse_matrix, const BGC_FP32_Slerp3* const slerp, const float phase);
|
|
extern inline void bgc_fp64_slerp3_get_phase_both_matrices(BGC_FP64_Matrix3x3* const rotation_matrix, BGC_FP64_Matrix3x3* const reverse_matrix, const BGC_FP64_Slerp3* const slerp, const double phase);
|
|
|
|
void bgc_fp32_slerp3_make(BGC_FP32_Slerp3* const slerp, const BGC_FP32_Turn3* const start, const BGC_FP32_Turn3* const augment)
|
|
{
|
|
const float square_vector = augment->_versor.x * augment->_versor.x + augment->_versor.y * augment->_versor.y + augment->_versor.z * augment->_versor.z;
|
|
|
|
if (isnan(square_vector)) {
|
|
bgc_fp32_slerp3_reset(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_FP32_SQUARE_EPSILON) {
|
|
slerp->_cosine_weight.s = start->_versor.s;
|
|
slerp->_cosine_weight.x = start->_versor.x;
|
|
slerp->_cosine_weight.y = start->_versor.y;
|
|
slerp->_cosine_weight.z = start->_versor.z;
|
|
|
|
slerp->_sine_weight.s = 0.0f;
|
|
slerp->_sine_weight.x = 0.0f;
|
|
slerp->_sine_weight.y = 0.0f;
|
|
slerp->_sine_weight.z = 0.0f;
|
|
|
|
slerp->radians = 0.0f;
|
|
return;
|
|
}
|
|
|
|
const float vector_modulus = sqrtf(square_vector);
|
|
|
|
slerp->radians = atan2f(vector_modulus, augment->_versor.s);
|
|
|
|
const float multiplier = 1.0f / vector_modulus;
|
|
|
|
slerp->_cosine_weight.s = start->_versor.s;
|
|
slerp->_cosine_weight.x = start->_versor.x;
|
|
slerp->_cosine_weight.y = start->_versor.y;
|
|
slerp->_cosine_weight.z = start->_versor.z;
|
|
|
|
slerp->_sine_weight.s = -multiplier * (augment->_versor.x * start->_versor.x + augment->_versor.y * start->_versor.y + augment->_versor.z * start->_versor.z);
|
|
slerp->_sine_weight.x = multiplier * (augment->_versor.x * start->_versor.s + augment->_versor.y * start->_versor.z - augment->_versor.z * start->_versor.y);
|
|
slerp->_sine_weight.y = multiplier * (augment->_versor.y * start->_versor.s - augment->_versor.x * start->_versor.z + augment->_versor.z * start->_versor.x);
|
|
slerp->_sine_weight.z = multiplier * (augment->_versor.z * start->_versor.s - augment->_versor.y * start->_versor.x + augment->_versor.x * start->_versor.y);
|
|
}
|
|
|
|
void bgc_fp64_slerp3_make(BGC_FP64_Slerp3* const slerp, const BGC_FP64_Turn3* const start, const BGC_FP64_Turn3* const augment)
|
|
{
|
|
const double square_vector = augment->_versor.x * augment->_versor.x + augment->_versor.y * augment->_versor.y + augment->_versor.z * augment->_versor.z;
|
|
|
|
if (isnan(square_vector)) {
|
|
bgc_fp64_slerp3_reset(slerp);
|
|
return;
|
|
}
|
|
|
|
if (square_vector <= BGC_FP64_SQUARE_EPSILON) {
|
|
slerp->_cosine_weight.s = start->_versor.s;
|
|
slerp->_cosine_weight.x = start->_versor.x;
|
|
slerp->_cosine_weight.y = start->_versor.y;
|
|
slerp->_cosine_weight.z = start->_versor.z;
|
|
|
|
slerp->_sine_weight.s = 0.0;
|
|
slerp->_sine_weight.x = 0.0;
|
|
slerp->_sine_weight.y = 0.0;
|
|
slerp->_sine_weight.z = 0.0;
|
|
|
|
slerp->radians = 0.0;
|
|
return;
|
|
}
|
|
|
|
const double vector_modulus = sqrt(square_vector);
|
|
|
|
slerp->radians = atan2(vector_modulus, augment->_versor.s);
|
|
|
|
const double multiplier = 1.0 / vector_modulus;
|
|
|
|
slerp->_cosine_weight.s = start->_versor.s;
|
|
slerp->_cosine_weight.x = start->_versor.x;
|
|
slerp->_cosine_weight.y = start->_versor.y;
|
|
slerp->_cosine_weight.z = start->_versor.z;
|
|
|
|
slerp->_sine_weight.s = -multiplier * (augment->_versor.x * start->_versor.x + augment->_versor.y * start->_versor.y + augment->_versor.z * start->_versor.z);
|
|
slerp->_sine_weight.x = multiplier * (augment->_versor.x * start->_versor.s + augment->_versor.y * start->_versor.z - augment->_versor.z * start->_versor.y);
|
|
slerp->_sine_weight.y = multiplier * (augment->_versor.y * start->_versor.s - augment->_versor.x * start->_versor.z + augment->_versor.z * start->_versor.x);
|
|
slerp->_sine_weight.z = multiplier * (augment->_versor.z * start->_versor.s - augment->_versor.y * start->_versor.x + augment->_versor.x * start->_versor.y);
|
|
}
|