diff --git a/basic-geometry/affine3.c b/basic-geometry/affine3.c index 89b9970..22682a4 100644 --- a/basic-geometry/affine3.c +++ b/basic-geometry/affine3.c @@ -3,9 +3,18 @@ extern inline void bgc_affine3_reset_fp32(BgcAffine3FP32 * affine); extern inline void bgc_affine3_reset_fp64(BgcAffine3FP64 * affine); +extern inline void bgc_affine3_reset_distortion_fp32(BgcAffine3FP32 * affine); +extern inline void bgc_affine3_reset_distortion_fp64(BgcAffine3FP64 * affine); + +extern inline void bgc_affine3_reset_shift_fp32(BgcAffine3FP32 * affine); +extern inline void bgc_affine3_reset_shift_fp64(BgcAffine3FP64 * affine); + extern inline void bgc_affine3_make_fp32(const BgcMatrix3x3FP32 * distortion, const BgcVector3FP32 * shift, BgcAffine3FP32 * affine); extern inline void bgc_affine3_make_fp64(const BgcMatrix3x3FP64 * distortion, const BgcVector3FP64 * shift, BgcAffine3FP64 * affine); +extern inline void bgc_affine3_combine_fp32(const BgcAffine3FP32 * parent, const BgcAffine3FP32 * child, BgcAffine3FP32 * combination); +extern inline void bgc_affine3_combine_fp64(const BgcAffine3FP64 * parent, const BgcAffine3FP64 * child, BgcAffine3FP64 * combination); + extern inline void bgc_affine3_transform_point_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_point, BgcVector3FP32 * transformed_point); extern inline void bgc_affine3_transform_point_fp64(const BgcAffine3FP64 * affine, const BgcVector3FP64 * initial_point, BgcVector3FP64 * transformed_point); diff --git a/basic-geometry/affine3.h b/basic-geometry/affine3.h index 8fbcd3a..6500cb0 100644 --- a/basic-geometry/affine3.h +++ b/basic-geometry/affine3.h @@ -78,6 +78,66 @@ inline void bgc_affine3_reset_fp64(BgcAffine3FP64 * affine) #endif } +inline void bgc_affine3_reset_distortion_fp32(BgcAffine3FP32 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_set_to_identity_fp32(&affine->distortion); +#else + affine->r1c1 = 1.0f; + affine->r1c2 = 0.0f; + affine->r1c3 = 0.0f; + + affine->r2c1 = 0.0f; + affine->r2c2 = 1.0f; + affine->r2c3 = 0.0f; + + affine->r3c1 = 0.0f; + affine->r3c2 = 0.0f; + affine->r3c3 = 1.0f; +#endif +} + +inline void bgc_affine3_reset_distortion_fp64(BgcAffine3FP64 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_set_to_identity_fp64(&affine->distortion); +#else + affine->r1c1 = 1.0; + affine->r1c2 = 0.0; + affine->r1c3 = 0.0; + + affine->r2c1 = 0.0; + affine->r2c2 = 1.0; + affine->r2c3 = 0.0; + + affine->r3c1 = 0.0; + affine->r3c2 = 0.0; + affine->r3c3 = 1.0; +#endif +} + +inline void bgc_affine3_reset_shift_fp32(BgcAffine3FP32 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_vector3_reset_fp32(&affine->shift); +#else + affine->shift1 = 0.0f; + affine->shift2 = 0.0f; + affine->shift3 = 0.0f; +#endif +} + +inline void bgc_affine3_reset_shift_fp64(BgcAffine3FP64 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_vector3_reset_fp64(&affine->shift); +#else + affine->shift1 = 0.0; + affine->shift2 = 0.0; + affine->shift3 = 0.0; +#endif +} + // ==================== Make ===================== // inline void bgc_affine3_make_fp32(const BgcMatrix3x3FP32 * distortion, const BgcVector3FP32 * shift, BgcAffine3FP32 * affine) @@ -126,6 +186,90 @@ inline void bgc_affine3_make_fp64(const BgcMatrix3x3FP64 * distortion, const Bgc #endif } +// =================== Combine =================== // + +inline void bgc_affine3_combine_fp32(const BgcAffine3FP32 * parent, const BgcAffine3FP32 * child, BgcAffine3FP32 * combination) +{ +#if BGC_AFFINE_USE_MATRIX + BgcVector3FP32 child_shift; + bgc_matrix3x3_get_right_product_fp32(&parent->distortion, &child->shift, &child_shift); + bgc_matrix_product_3x3_at_3x3_fp32(&parent->distortion, &child->distortion, &combination->distortion); + bgc_vector3_add_fp32(&parent->shift, &child_shift, &combination->shift); +#else + const float r1c1 = parent->r1c1 * child->r1c1 + parent->r1c2 * child->r2c1 + parent->r1c3 * child->r3c1; + const float r1c2 = parent->r1c1 * child->r1c2 + parent->r1c2 * child->r2c2 + parent->r1c3 * child->r3c2; + const float r1c3 = parent->r1c1 * child->r1c3 + parent->r1c2 * child->r2c3 + parent->r1c3 * child->r3c3; + + const float r2c1 = parent->r2c1 * child->r1c1 + parent->r2c2 * child->r2c1 + parent->r2c3 * child->r3c1; + const float r2c2 = parent->r2c1 * child->r1c2 + parent->r2c2 * child->r2c2 + parent->r2c3 * child->r3c2; + const float r2c3 = parent->r2c1 * child->r1c3 + parent->r2c2 * child->r2c3 + parent->r2c3 * child->r3c3; + + const float r3c1 = parent->r3c1 * child->r1c1 + parent->r3c2 * child->r2c1 + parent->r3c3 * child->r3c1; + const float r3c2 = parent->r3c1 * child->r1c2 + parent->r3c2 * child->r2c2 + parent->r3c3 * child->r3c2; + const float r3c3 = parent->r3c1 * child->r1c3 + parent->r3c2 * child->r2c3 + parent->r3c3 * child->r3c3; + + const float shift1 = parent->r1c1 * child->shift1 + parent->r1c2 * child->shift2 + parent->r1c3 * child->shift3; + const float shift2 = parent->r2c1 * child->shift1 + parent->r2c2 * child->shift2 + parent->r2c3 * child->shift3; + const float shift3 = parent->r3c1 * child->shift1 + parent->r3c2 * child->shift2 + parent->r3c3 * child->shift3; + + combination->r1c1 = r1c1; + combination->r1c2 = r1c2; + combination->r1c3 = r1c3; + combination->shift1 = shift1; + + combination->r2c1 = r2c1; + combination->r2c2 = r2c2; + combination->r2c3 = r2c3; + combination->shift2 = shift2; + + combination->r3c1 = r3c1; + combination->r3c2 = r3c2; + combination->r3c3 = r3c3; + combination->shift3 = shift3; +#endif +} + +inline void bgc_affine3_combine_fp64(const BgcAffine3FP64 * parent, const BgcAffine3FP64 * child, BgcAffine3FP64 * combination) +{ +#if BGC_AFFINE_USE_MATRIX + BgcVector3FP64 child_shift; + bgc_matrix3x3_get_right_product_fp64(&parent->distortion, &child->shift, &child_shift); + bgc_matrix_product_3x3_at_3x3_fp64(&parent->distortion, &child->distortion, &combination->distortion); + bgc_vector3_add_fp64(&parent->shift, &child_shift, &combination->shift); +#else + const double r1c1 = parent->r1c1 * child->r1c1 + parent->r1c2 * child->r2c1 + parent->r1c3 * child->r3c1; + const double r1c2 = parent->r1c1 * child->r1c2 + parent->r1c2 * child->r2c2 + parent->r1c3 * child->r3c2; + const double r1c3 = parent->r1c1 * child->r1c3 + parent->r1c2 * child->r2c3 + parent->r1c3 * child->r3c3; + + const double r2c1 = parent->r2c1 * child->r1c1 + parent->r2c2 * child->r2c1 + parent->r2c3 * child->r3c1; + const double r2c2 = parent->r2c1 * child->r1c2 + parent->r2c2 * child->r2c2 + parent->r2c3 * child->r3c2; + const double r2c3 = parent->r2c1 * child->r1c3 + parent->r2c2 * child->r2c3 + parent->r2c3 * child->r3c3; + + const double r3c1 = parent->r3c1 * child->r1c1 + parent->r3c2 * child->r2c1 + parent->r3c3 * child->r3c1; + const double r3c2 = parent->r3c1 * child->r1c2 + parent->r3c2 * child->r2c2 + parent->r3c3 * child->r3c2; + const double r3c3 = parent->r3c1 * child->r1c3 + parent->r3c2 * child->r2c3 + parent->r3c3 * child->r3c3; + + const double shift1 = parent->r1c1 * child->shift1 + parent->r1c2 * child->shift2 + parent->r1c3 * child->shift3; + const double shift2 = parent->r2c1 * child->shift1 + parent->r2c2 * child->shift2 + parent->r2c3 * child->shift3; + const double shift3 = parent->r3c1 * child->shift1 + parent->r3c2 * child->shift2 + parent->r3c3 * child->shift3; + + combination->r1c1 = r1c1; + combination->r1c2 = r1c2; + combination->r1c3 = r1c3; + combination->shift1 = shift1; + + combination->r2c1 = r2c1; + combination->r2c2 = r2c2; + combination->r2c3 = r2c3; + combination->shift2 = shift2; + + combination->r3c1 = r3c1; + combination->r3c2 = r3c2; + combination->r3c3 = r3c3; + combination->shift3 = shift3; +#endif +} + // =============== Transform Point =============== // inline void bgc_affine3_transform_point_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_point, BgcVector3FP32 * transformed_point) diff --git a/basic-geometry/utilities.h b/basic-geometry/utilities.h index 03ad443..46c713f 100644 --- a/basic-geometry/utilities.h +++ b/basic-geometry/utilities.h @@ -47,7 +47,7 @@ #define BGC_AXIS_REVERSE_X3 -3 #if !defined(BGC_AFFINE_USE_MATRIX) -#define BGC_AFFINE_USE_MATRIX 1 +#define BGC_AFFINE_USE_MATRIX 0 #endif inline int bgc_is_correct_axis(const int axis)