diff --git a/basic-geometry/dual-vector3.c b/basic-geometry/dual-vector3.c index 54bca52..fd7f61e 100644 --- a/basic-geometry/dual-vector3.c +++ b/basic-geometry/dual-vector3.c @@ -3,6 +3,12 @@ extern inline void bgc_fp32_dual_vector3_reset(BGC_FP32_DualVector3* const vector); extern inline void bgc_fp64_dual_vector3_reset(BGC_FP64_DualVector3* const vector); +extern inline void bgc_fp32_dual_vector3_get_squared_length(BGC_FP32_DualNumber* const squared_length, const BGC_FP32_DualVector3* const vector); +extern inline void bgc_fp64_dual_vector3_get_squared_length(BGC_FP64_DualNumber* const squared_length, const BGC_FP64_DualVector3* const vector); + +extern inline void bgc_fp32_dual_vector3_get_length(BGC_FP32_DualNumber* const length, const BGC_FP32_DualVector3* const vector); +extern inline void bgc_fp64_dual_vector3_get_length(BGC_FP64_DualNumber* const length, const BGC_FP64_DualVector3* const vector); + extern inline void bgc_fp32_dual_vector3_copy(BGC_FP32_DualVector3* const destination, const BGC_FP32_DualVector3* const source); extern inline void bgc_fp64_dual_vector3_copy(BGC_FP64_DualVector3* const destination, const BGC_FP64_DualVector3* const source); diff --git a/basic-geometry/dual-vector3.h b/basic-geometry/dual-vector3.h index 9033cf4..7ddf740 100644 --- a/basic-geometry/dual-vector3.h +++ b/basic-geometry/dual-vector3.h @@ -18,6 +18,54 @@ inline void bgc_fp64_dual_vector3_reset(BGC_FP64_DualVector3* const vector) bgc_fp64_vector3_reset(&vector->dual_part); } +// =============== Squared Length =============== // + +inline void bgc_fp32_dual_vector3_get_squared_length(BGC_FP32_DualNumber* const squared_length, const BGC_FP32_DualVector3* const vector) +{ + squared_length->real_part = bgc_fp32_vector3_get_squared_length(&vector->real_part); + squared_length->dual_part = 2.0f * bgc_fp32_vector3_get_dot_product(&vector->real_part, &vector->dual_part); +} + +inline void bgc_fp64_dual_vector3_get_squared_length(BGC_FP64_DualNumber* const squared_length, const BGC_FP64_DualVector3* const vector) +{ + squared_length->real_part = bgc_fp64_vector3_get_squared_length(&vector->real_part); + squared_length->dual_part = 2.0 * bgc_fp64_vector3_get_dot_product(&vector->real_part, &vector->dual_part); +} + +// =================== Length =================== // + +inline void bgc_fp32_dual_vector3_get_length(BGC_FP32_DualNumber* const length, const BGC_FP32_DualVector3* const vector) +{ + const float square_real_part = bgc_fp32_vector3_get_squared_length(&vector->real_part); + + if (square_real_part <= BGC_FP32_SQUARE_EPSILON) { + length->real_part = 0.0f; + length->dual_part = 0.0f; + return; + } + + const float real_part = sqrtf(square_real_part); + + length->real_part = real_part; + length->dual_part = bgc_fp32_vector3_get_dot_product(&vector->real_part, &vector->dual_part) / real_part; +} + +inline void bgc_fp64_dual_vector3_get_length(BGC_FP64_DualNumber* const length, const BGC_FP64_DualVector3* const vector) +{ + const double square_real_part = bgc_fp64_vector3_get_squared_length(&vector->real_part); + + if (square_real_part <= BGC_FP64_SQUARE_EPSILON) { + length->real_part = 0.0; + length->dual_part = 0.0; + return; + } + + const double real_part = sqrt(square_real_part); + + length->real_part = real_part; + length->dual_part = bgc_fp64_vector3_get_dot_product(&vector->real_part, &vector->dual_part) / real_part; +} + // ==================== Copy ==================== // inline void bgc_fp32_dual_vector3_copy(BGC_FP32_DualVector3* const destination, const BGC_FP32_DualVector3* const source)