diff --git a/basic-geometry/affine3.c b/basic-geometry/affine3.c new file mode 100644 index 0000000..89b9970 --- /dev/null +++ b/basic-geometry/affine3.c @@ -0,0 +1,13 @@ +#include "affine3.h" + +extern inline void bgc_affine3_reset_fp32(BgcAffine3FP32 * affine); +extern inline void bgc_affine3_reset_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_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); + +extern inline void bgc_affine3_transform_vector_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_vector, BgcVector3FP32 * transformed_vector); +extern inline void bgc_affine3_transform_vector_fp64(const BgcAffine3FP64 * affine, const BgcVector3FP64 * initial_vector, BgcVector3FP64 * transformed_vector); diff --git a/basic-geometry/affine3.h b/basic-geometry/affine3.h new file mode 100644 index 0000000..8fbcd3a --- /dev/null +++ b/basic-geometry/affine3.h @@ -0,0 +1,197 @@ +#ifndef _BGC_AFFINE3_H_INCLUDED_ +#define _BGC_AFFINE3_H_INCLUDED_ + +#include "utilities.h" +#include "vector3.h" +#include "matrixes.h" +#include "matrix3x3.h" + +// ==================== Types ==================== // + +typedef struct { +#if BGC_AFFINE_USE_MATRIX + BgcMatrix3x3FP32 distortion; + BgcVector3FP32 shift; +#else + float r1c1, r1c2, r1c3, shift1; + float r2c1, r2c2, r2c3, shift2; + float r3c1, r3c2, r3c3, shift3; +#endif +} BgcAffine3FP32; + +typedef struct { +#if BGC_AFFINE_USE_MATRIX + BgcMatrix3x3FP64 distortion; + BgcVector3FP64 shift; +#else + double r1c1, r1c2, r1c3, shift1; + double r2c1, r2c2, r2c3, shift2; + double r3c1, r3c2, r3c3, shift3; +#endif +} BgcAffine3FP64; + +// ==================== Reset ==================== // + +inline void bgc_affine3_reset_fp32(BgcAffine3FP32 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_set_to_identity_fp32(&affine->distortion); + bgc_vector3_reset_fp32(&affine->shift); +#else + affine->r1c1 = 1.0f; + affine->r1c2 = 0.0f; + affine->r1c3 = 0.0f; + affine->shift1 = 0.0f; + + affine->r2c1 = 0.0f; + affine->r2c2 = 1.0f; + affine->r2c3 = 0.0f; + affine->shift2 = 0.0f; + + affine->r3c1 = 0.0f; + affine->r3c2 = 0.0f; + affine->r3c3 = 1.0f; + affine->shift3 = 0.0f; +#endif +} + +inline void bgc_affine3_reset_fp64(BgcAffine3FP64 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_set_to_identity_fp64(&affine->distortion); + bgc_vector3_reset_fp64(&affine->shift); +#else + affine->r1c1 = 1.0; + affine->r1c2 = 0.0; + affine->r1c3 = 0.0; + affine->shift1 = 0.0; + + affine->r2c1 = 0.0; + affine->r2c2 = 1.0; + affine->r2c3 = 0.0; + affine->shift2 = 0.0; + + affine->r3c1 = 0.0; + affine->r3c2 = 0.0; + affine->r3c3 = 1.0; + affine->shift3 = 0.0; +#endif +} + +// ==================== Make ===================== // + +inline void bgc_affine3_make_fp32(const BgcMatrix3x3FP32 * distortion, const BgcVector3FP32 * shift, BgcAffine3FP32 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_copy_fp32(distortion, &affine->distortion); + bgc_vector3_copy_fp32(shift, &affine->shift); +#else + affine->r1c1 = distortion->r1c1; + affine->r1c2 = distortion->r1c2; + affine->r1c3 = distortion->r1c3; + affine->shift1 = shift->x1; + + affine->r2c1 = distortion->r2c1; + affine->r2c2 = distortion->r2c2; + affine->r2c3 = distortion->r2c3; + affine->shift2 = shift->x2; + + affine->r3c1 = distortion->r3c1; + affine->r3c2 = distortion->r3c2; + affine->r3c3 = distortion->r3c3; + affine->shift3 = shift->x3; +#endif +} + +inline void bgc_affine3_make_fp64(const BgcMatrix3x3FP64 * distortion, const BgcVector3FP64 * shift, BgcAffine3FP64 * affine) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_copy_fp64(distortion, &affine->distortion); + bgc_vector3_copy_fp64(shift, &affine->shift); +#else + affine->r1c1 = distortion->r1c1; + affine->r1c2 = distortion->r1c2; + affine->r1c3 = distortion->r1c3; + affine->shift1 = shift->x1; + + affine->r2c1 = distortion->r2c1; + affine->r2c2 = distortion->r2c2; + affine->r2c3 = distortion->r2c3; + affine->shift2 = shift->x2; + + affine->r3c1 = distortion->r3c1; + affine->r3c2 = distortion->r3c2; + affine->r3c3 = distortion->r3c3; + affine->shift3 = shift->x3; +#endif +} + +// =============== Transform Point =============== // + +inline void bgc_affine3_transform_point_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_point, BgcVector3FP32 * transformed_point) +{ +#if BGC_AFFINE_USE_MATRIX + BgcVector3FP32 distorted; + bgc_matrix3x3_get_right_product_fp32(&affine->distortion, initial_point, &distorted); + bgc_vector3_add_fp32(&affine->shift, &distorted, transformed_point); +#else + const float x1 = affine->r1c1 * initial_point->x1 + affine->r1c2 * initial_point->x2 + affine->r1c3 * initial_point->x3 + affine->shift1; + const float x2 = affine->r2c1 * initial_point->x1 + affine->r2c2 * initial_point->x2 + affine->r2c3 * initial_point->x3 + affine->shift2; + const float x3 = affine->r3c1 * initial_point->x1 + affine->r3c2 * initial_point->x2 + affine->r3c3 * initial_point->x3 + affine->shift3; + + transformed_point->x1 = x1; + transformed_point->x2 = x2; + transformed_point->x3 = x3; +#endif +} + +inline void bgc_affine3_transform_point_fp64(const BgcAffine3FP64 * affine, const BgcVector3FP64 * initial_point, BgcVector3FP64 * transformed_point) +{ +#if BGC_AFFINE_USE_MATRIX + BgcVector3FP64 distorted; + bgc_matrix3x3_get_right_product_fp64(&affine->distortion, initial_point, &distorted); + bgc_vector3_add_fp64(&affine->shift, &distorted, transformed_point); +#else + const double x1 = affine->r1c1 * initial_point->x1 + affine->r1c2 * initial_point->x2 + affine->r1c3 * initial_point->x3 + affine->shift1; + const double x2 = affine->r2c1 * initial_point->x1 + affine->r2c2 * initial_point->x2 + affine->r2c3 * initial_point->x3 + affine->shift2; + const double x3 = affine->r3c1 * initial_point->x1 + affine->r3c2 * initial_point->x2 + affine->r3c3 * initial_point->x3 + affine->shift3; + + transformed_point->x1 = x1; + transformed_point->x2 = x2; + transformed_point->x3 = x3; +#endif +} + +// ============== Transform Vector =============== // + +inline void bgc_affine3_transform_vector_fp32(const BgcAffine3FP32 * affine, const BgcVector3FP32 * initial_vector, BgcVector3FP32 * transformed_vector) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_get_right_product_fp32(&affine->distortion, initial_vector, transformed_vector); +#else + const float x1 = affine->r1c1 * initial_vector->x1 + affine->r1c2 * initial_vector->x2 + affine->r1c3 * initial_vector->x3; + const float x2 = affine->r2c1 * initial_vector->x1 + affine->r2c2 * initial_vector->x2 + affine->r2c3 * initial_vector->x3; + const float x3 = affine->r3c1 * initial_vector->x1 + affine->r3c2 * initial_vector->x2 + affine->r3c3 * initial_vector->x3; + + transformed_vector->x1 = x1; + transformed_vector->x2 = x2; + transformed_vector->x3 = x3; +#endif +} + +inline void bgc_affine3_transform_vector_fp64(const BgcAffine3FP64 * affine, const BgcVector3FP64 * initial_vector, BgcVector3FP64 * transformed_vector) +{ +#if BGC_AFFINE_USE_MATRIX + bgc_matrix3x3_get_right_product_fp64(&affine->distortion, initial_vector, transformed_vector); +#else + const double x1 = affine->r1c1 * initial_vector->x1 + affine->r1c2 * initial_vector->x2 + affine->r1c3 * initial_vector->x3; + const double x2 = affine->r2c1 * initial_vector->x1 + affine->r2c2 * initial_vector->x2 + affine->r2c3 * initial_vector->x3; + const double x3 = affine->r3c1 * initial_vector->x1 + affine->r3c2 * initial_vector->x2 + affine->r3c3 * initial_vector->x3; + + transformed_vector->x1 = x1; + transformed_vector->x2 = x2; + transformed_vector->x3 = x3; +#endif +} + +#endif // _BGC_AFFINE3_H_INCLUDED_ diff --git a/basic-geometry/basic-geometry.cbp b/basic-geometry/basic-geometry.cbp index 1dadfea..b906c16 100644 --- a/basic-geometry/basic-geometry.cbp +++ b/basic-geometry/basic-geometry.cbp @@ -76,6 +76,10 @@