diff --git a/basic-geometry/matrix3x3.h b/basic-geometry/matrix3x3.h index b0bd7c6..b8554b0 100644 --- a/basic-geometry/matrix3x3.h +++ b/basic-geometry/matrix3x3.h @@ -280,6 +280,22 @@ inline int bgc_matrix3x3_is_rotation_fp32(const BgcMatrix3x3FP32* matrix) return 0; } + BgcMatrix3x3FP32 transposed, product; + + bgc_matrix3x3_transpose_fp32(matrix, &transposed); + bgc_matrix_product_3x3_at_3x3_fp32(matrix, &transposed, &product); + + return bgc_is_unit_fp32(product.r1c1) && bgc_is_zero_fp32(product.r1c2) && bgc_is_zero_fp32(product.r1c3) + && bgc_is_zero_fp32(product.r2c1) && bgc_is_unit_fp32(product.r2c2) && bgc_is_zero_fp32(product.r2c3) + && bgc_is_zero_fp32(product.r3c1) && bgc_is_zero_fp32(product.r3c2) && bgc_is_unit_fp32(product.r3c3); +} + +inline int bgc_matrix3x3_is_rotation_fp32a(const BgcMatrix3x3FP32* matrix) +{ + if (!bgc_is_unit_fp32(bgc_matrix3x3_get_determinant_fp32(matrix))) { + return 0; + } + const float product_r1c1 = matrix->r1c1 * matrix->r1c1 + matrix->r1c2 * matrix->r2c1 + matrix->r1c3 * matrix->r3c1; const float product_r1c2 = matrix->r1c1 * matrix->r1c2 + matrix->r1c2 * matrix->r2c2 + matrix->r1c3 * matrix->r3c2; const float product_r1c3 = matrix->r1c1 * matrix->r1c3 + matrix->r1c2 * matrix->r2c3 + matrix->r1c3 * matrix->r3c3; diff --git a/basic-geometry/slerp.c b/basic-geometry/slerp.c index 43f5cf0..40753eb 100644 --- a/basic-geometry/slerp.c +++ b/basic-geometry/slerp.c @@ -40,17 +40,17 @@ void bgc_slerp_make_fp32(const BgcVersorFP32* start, const BgcVersorFP32* augmen slerp->radians = atan2f(vector_modulus, augment->s0); - const float mutliplier = 1.0f / vector_modulus; + 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 = -mutliplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3); - slerp->x1_sin_weight = mutliplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2); - slerp->x2_sin_weight = mutliplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1); - slerp->x3_sin_weight = mutliplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2); + 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) @@ -81,15 +81,15 @@ void bgc_slerp_make_fp64(const BgcVersorFP64* start, const BgcVersorFP64* augmen slerp->radians = atan2(vector_modulus, augment->s0); - const double mutliplier = 1.0 / vector_modulus; + 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 = -mutliplier * (augment->x1 * start->x1 + augment->x2 * start->x2 + augment->x3 * start->x3); - slerp->x1_sin_weight = mutliplier * (augment->x1 * start->s0 + augment->x2 * start->x3 - augment->x3 * start->x2); - slerp->x2_sin_weight = mutliplier * (augment->x2 * start->s0 - augment->x1 * start->x3 + augment->x3 * start->x1); - slerp->x3_sin_weight = mutliplier * (augment->x3 * start->s0 - augment->x2 * start->x1 + augment->x1 * start->x2); + 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); }