#ifndef _BGC_MATRIX2X2_H_ #define _BGC_MATRIX2X2_H_ #include "angle.h" #include "vector2.h" #include "matrices.h" // =================== Reset ==================== // inline void bgc_fp32_matrix2x2_reset(BGC_FP32_Matrix2x2* matrix) { matrix->row1_col1 = 0.0f; matrix->row1_col2 = 0.0f; matrix->row2_col1 = 0.0f; matrix->row2_col2 = 0.0f; } inline void bgc_fp64_matrix2x2_reset(BGC_FP64_Matrix2x2* matrix) { matrix->row1_col1 = 0.0; matrix->row1_col2 = 0.0; matrix->row2_col1 = 0.0; matrix->row2_col2 = 0.0; } // ================== Identity ================== // inline void bgc_fp32_matrix2x2_make_identity(BGC_FP32_Matrix2x2* matrix) { matrix->row1_col1 = 1.0f; matrix->row1_col2 = 0.0f; matrix->row2_col1 = 0.0f; matrix->row2_col2 = 1.0f; } inline void bgc_fp64_matrix2x2_make_identity(BGC_FP64_Matrix2x2* matrix) { matrix->row1_col1 = 1.0; matrix->row1_col2 = 0.0; matrix->row2_col1 = 0.0; matrix->row2_col2 = 1.0; } // ================ Set Diagonal ================ // inline void bgc_fp32_matrix2x2_make_diagonal(BGC_FP32_Matrix2x2* matrix, const float d1, const float d2) { matrix->row1_col1 = d1; matrix->row1_col2 = 0.0f; matrix->row2_col1 = 0.0f; matrix->row2_col2 = d2; } inline void bgc_fp64_matrix2x2_make_diagonal(BGC_FP64_Matrix2x2* matrix, const double d1, const double d2) { matrix->row1_col1 = d1; matrix->row1_col2 = 0.0; matrix->row2_col1 = 0.0; matrix->row2_col2 = d2; } // ============== Rotation Matrix =============== // inline void bgc_fp32_matrix2x2_make_for_turn(BGC_FP32_Matrix2x2* matrix, const float angle, const int angle_unit) { const float radians = bgc_fp32_angle_to_radians(angle, angle_unit); const float cosine = cosf(radians); const float sine = sinf(radians); matrix->row1_col1 = cosine; matrix->row1_col2 = -sine; matrix->row2_col1 = sine; matrix->row2_col2 = cosine; } inline void bgc_fp64_matrix2x2_make_for_turn(BGC_FP64_Matrix2x2* matrix, const double angle, const int angle_unit) { const double radians = bgc_fp64_angle_to_radians(angle, angle_unit); const double cosine = cos(radians); const double sine = sin(radians); matrix->row1_col1 = cosine; matrix->row1_col2 = -sine; matrix->row2_col1 = sine; matrix->row2_col2 = cosine; } // ================ Determinant ================= // inline float bgc_fp32_matrix2x2_get_determinant(const BGC_FP32_Matrix2x2* matrix) { return matrix->row1_col1 * matrix->row2_col2 - matrix->row1_col2 * matrix->row2_col1; } inline double bgc_fp64_matrix2x2_get_determinant(const BGC_FP64_Matrix2x2* matrix) { return matrix->row1_col1 * matrix->row2_col2 - matrix->row1_col2 * matrix->row2_col1; } // ================ Is Identity ================= // inline int bgc_fp32_matrix2x2_is_identity(const BGC_FP32_Matrix2x2* matrix) { return bgc_fp32_is_unit(matrix->row1_col1) && bgc_fp32_is_zero(matrix->row1_col2) && bgc_fp32_is_zero(matrix->row2_col1) && bgc_fp32_is_unit(matrix->row2_col2); } inline int bgc_fp64_matrix2x2_is_identity(const BGC_FP64_Matrix2x2* matrix) { return bgc_fp64_is_unit(matrix->row1_col1) && bgc_fp64_is_zero(matrix->row1_col2) && bgc_fp64_is_zero(matrix->row2_col1) && bgc_fp64_is_unit(matrix->row2_col2); } // ================ Is Singular ================= // inline int bgc_fp32_matrix2x2_is_singular(const BGC_FP32_Matrix2x2* matrix) { return bgc_fp32_is_zero(bgc_fp32_matrix2x2_get_determinant(matrix)); } inline int bgc_fp64_matrix2x2_is_singular(const BGC_FP64_Matrix2x2* matrix) { return bgc_fp64_is_zero(bgc_fp64_matrix2x2_get_determinant(matrix)); } // ================ Is Rotation ================= // inline int bgc_fp32_matrix2x2_is_rotation(const BGC_FP32_Matrix2x2* matrix) { BGC_FP32_Matrix2x2 product; product.row1_col1 = matrix->row1_col1 * matrix->row1_col1 + matrix->row1_col2 * matrix->row2_col1; product.row1_col2 = matrix->row1_col1 * matrix->row1_col2 + matrix->row1_col2 * matrix->row2_col2; product.row2_col1 = matrix->row2_col1 * matrix->row1_col1 + matrix->row2_col2 * matrix->row2_col1; product.row2_col2 = matrix->row2_col1 * matrix->row1_col2 + matrix->row2_col2 * matrix->row2_col2; return bgc_fp32_matrix2x2_is_identity(&product); } inline int bgc_fp64_matrix2x2_is_rotation(const BGC_FP64_Matrix2x2* matrix) { BGC_FP64_Matrix2x2 product; product.row1_col1 = matrix->row1_col1 * matrix->row1_col1 + matrix->row1_col2 * matrix->row2_col1; product.row1_col2 = matrix->row1_col1 * matrix->row1_col2 + matrix->row1_col2 * matrix->row2_col2; product.row2_col1 = matrix->row2_col1 * matrix->row1_col1 + matrix->row2_col2 * matrix->row2_col1; product.row2_col2 = matrix->row2_col1 * matrix->row1_col2 + matrix->row2_col2 * matrix->row2_col2; return bgc_fp64_matrix2x2_is_identity(&product); } // ==================== Copy ==================== // inline void bgc_fp32_matrix2x2_copy(BGC_FP32_Matrix2x2* destination, const BGC_FP32_Matrix2x2* source) { destination->row1_col1 = source->row1_col1; destination->row1_col2 = source->row1_col2; destination->row2_col1 = source->row2_col1; destination->row2_col2 = source->row2_col2; } inline void bgc_fp64_matrix2x2_copy(BGC_FP64_Matrix2x2* destination, const BGC_FP64_Matrix2x2* source) { destination->row1_col1 = source->row1_col1; destination->row1_col2 = source->row1_col2; destination->row2_col1 = source->row2_col1; destination->row2_col2 = source->row2_col2; } // ==================== Swap ==================== // inline void bgc_fp32_matrix2x2_swap(BGC_FP32_Matrix2x2* matrix1, BGC_FP32_Matrix2x2* matrix2) { const float row1_col1 = matrix2->row1_col1; const float row1_col2 = matrix2->row1_col2; const float row2_col1 = matrix2->row2_col1; const float row2_col2 = matrix2->row2_col2; matrix2->row1_col1 = matrix1->row1_col1; matrix2->row1_col2 = matrix1->row1_col2; matrix2->row2_col1 = matrix1->row2_col1; matrix2->row2_col2 = matrix1->row2_col2; matrix1->row1_col1 = row1_col1; matrix1->row1_col2 = row1_col2; matrix1->row2_col1 = row2_col1; matrix1->row2_col2 = row2_col2; } inline void bgc_fp64_matrix2x2_swap(BGC_FP64_Matrix2x2* matrix1, BGC_FP64_Matrix2x2* matrix2) { const double row1_col1 = matrix2->row1_col1; const double row1_col2 = matrix2->row1_col2; const double row2_col1 = matrix2->row2_col1; const double row2_col2 = matrix2->row2_col2; matrix2->row1_col1 = matrix1->row1_col1; matrix2->row1_col2 = matrix1->row1_col2; matrix2->row2_col1 = matrix1->row2_col1; matrix2->row2_col2 = matrix1->row2_col2; matrix1->row1_col1 = row1_col1; matrix1->row1_col2 = row1_col2; matrix1->row2_col1 = row2_col1; matrix1->row2_col2 = row2_col2; } // ================== Convert =================== // inline void bgc_fp64_matrix2x2_convert_to_fp32(BGC_FP32_Matrix2x2* destination, const BGC_FP64_Matrix2x2* source) { destination->row1_col1 = (float)source->row1_col1; destination->row1_col2 = (float)source->row1_col2; destination->row2_col1 = (float)source->row2_col1; destination->row2_col2 = (float)source->row2_col2; } inline void bgc_fp32_matrix2x2_convert_to_fp64(BGC_FP64_Matrix2x2* destination, const BGC_FP32_Matrix2x2* source) { destination->row1_col1 = source->row1_col1; destination->row1_col2 = source->row1_col2; destination->row2_col1 = source->row2_col1; destination->row2_col2 = source->row2_col2; } // ================ Get Inverse ================= // inline int bgc_fp32_matrix2x2_get_inverse(BGC_FP32_Matrix2x2* inverse, const BGC_FP32_Matrix2x2* matrix) { const float determinant = bgc_fp32_matrix2x2_get_determinant(matrix); if (bgc_fp32_is_zero(determinant)) { return 0; } const float row1_col1 = matrix->row2_col2; const float row1_col2 = -matrix->row1_col2; const float row2_col1 = -matrix->row2_col1; const float row2_col2 = matrix->row1_col1; const float multiplier = 1.0f / determinant; inverse->row1_col1 = row1_col1 * multiplier; inverse->row1_col2 = row1_col2 * multiplier; inverse->row2_col1 = row2_col1 * multiplier; inverse->row2_col2 = row2_col2 * multiplier; return 1; } inline int bgc_fp64_matrix2x2_get_inverse(BGC_FP64_Matrix2x2* inverse, const BGC_FP64_Matrix2x2* matrix) { const double determinant = bgc_fp64_matrix2x2_get_determinant(matrix); if (bgc_fp64_is_zero(determinant)) { return 0; } const double row1_col1 = matrix->row2_col2; const double row1_col2 = -matrix->row1_col2; const double row2_col1 = -matrix->row2_col1; const double row2_col2 = matrix->row1_col1; const double multiplier = 1.0 / determinant; inverse->row1_col1 = row1_col1 * multiplier; inverse->row1_col2 = row1_col2 * multiplier; inverse->row2_col1 = row2_col1 * multiplier; inverse->row2_col2 = row2_col2 * multiplier; return 1; } // =================== Invert =================== // inline int bgc_fp32_matrix2x2_invert(BGC_FP32_Matrix2x2* matrix) { return bgc_fp32_matrix2x2_get_inverse(matrix, matrix); } inline int bgc_fp64_matrix2x2_invert(BGC_FP64_Matrix2x2* matrix) { return bgc_fp64_matrix2x2_get_inverse(matrix, matrix); } // ================= Transpose ================== // inline void bgc_fp32_matrix2x2_transpose(BGC_FP32_Matrix2x2* matrix) { const float row1_col2 = matrix->row1_col2; matrix->row1_col2 = matrix->row2_col1; matrix->row2_col1 = row1_col2; } inline void bgc_fp64_matrix2x2_transpose(BGC_FP64_Matrix2x2* matrix) { const double row1_col2 = matrix->row1_col2; matrix->row1_col2 = matrix->row2_col1; matrix->row2_col1 = row1_col2; } // =============== Get Transpose ================ // inline void bgc_fp32_matrix2x2_get_transposed(BGC_FP32_Matrix2x2* transposed, const BGC_FP32_Matrix2x2* matrix) { const float row1_col2 = matrix->row1_col2; transposed->row1_col1 = matrix->row1_col1; transposed->row1_col2 = matrix->row2_col1; transposed->row2_col1 = row1_col2; transposed->row2_col2 = matrix->row2_col2; } inline void bgc_fp64_matrix2x2_get_transposed(BGC_FP64_Matrix2x2* transposed, const BGC_FP64_Matrix2x2* matrix) { const double row1_col2 = matrix->row1_col2; transposed->row1_col1 = matrix->row1_col1; transposed->row1_col2 = matrix->row2_col1; transposed->row2_col1 = row1_col2; transposed->row2_col2 = matrix->row2_col2; } // ================== Get Row =================== // inline void bgc_fp32_matrix2x2_get_row(BGC_FP32_Vector2* row, const BGC_FP32_Matrix2x2* matrix, const int row_number) { if (row_number == 1) { row->x1 = matrix->row1_col1; row->x2 = matrix->row1_col2; return; } if (row_number == 2) { row->x1 = matrix->row2_col1; row->x2 = matrix->row2_col2; return; } row->x1 = 0.0f; row->x2 = 0.0f; } inline void bgc_fp64_matrix2x2_get_row(BGC_FP64_Vector2* row, const BGC_FP64_Matrix2x2* matrix, const int row_number) { if (row_number == 1) { row->x1 = matrix->row1_col1; row->x2 = matrix->row1_col2; return; } if (row_number == 2) { row->x1 = matrix->row2_col1; row->x2 = matrix->row2_col2; return; } row->x1 = 0.0; row->x2 = 0.0; } // ================== Set Row =================== // inline void bgc_fp32_matrix2x2_set_row(BGC_FP32_Matrix2x2* matrix, const int row_number, const BGC_FP32_Vector2* row) { if (row_number == 1) { matrix->row1_col1 = row->x1; matrix->row1_col2 = row->x2; return; } if (row_number == 2) { matrix->row2_col1 = row->x1; matrix->row2_col2 = row->x2; } } inline void bgc_fp64_matrix2x2_set_row(BGC_FP64_Matrix2x2* matrix, const int row_number, const BGC_FP64_Vector2* row) { if (row_number == 1) { matrix->row1_col1 = row->x1; matrix->row1_col2 = row->x2; return; } if (row_number == 2) { matrix->row2_col1 = row->x1; matrix->row2_col2 = row->x2; } } // ================= Get Column ================= // inline void bgc_fp32_matrix2x2_get_column(BGC_FP32_Vector2* column, const BGC_FP32_Matrix2x2* matrix, const int column_number) { if (column_number == 1) { column->x1 = matrix->row1_col1; column->x2 = matrix->row2_col1; return; } if (column_number == 2) { column->x1 = matrix->row1_col2; column->x2 = matrix->row2_col2; return; } column->x1 = 0.0f; column->x2 = 0.0f; } inline void bgc_fp64_matrix2x2_get_column(BGC_FP64_Vector2* column, const BGC_FP64_Matrix2x2* matrix, const int column_number) { if (column_number == 1) { column->x1 = matrix->row1_col1; column->x2 = matrix->row2_col1; return; } if (column_number == 2) { column->x1 = matrix->row1_col2; column->x2 = matrix->row2_col2; return; } column->x1 = 0.0; column->x2 = 0.0; } // ================= Set Column ================= // inline void bgc_fp32_matrix2x2_set_column(BGC_FP32_Matrix2x2* matrix, const int column_number, const BGC_FP32_Vector2* column) { if (column_number == 1) { matrix->row1_col1 = column->x1; matrix->row2_col1 = column->x2; return; } if (column_number == 2) { matrix->row1_col2 = column->x1; matrix->row2_col2 = column->x2; } } inline void bgc_fp64_matrix2x2_set_column(BGC_FP64_Matrix2x2* matrix, const int column_number, const BGC_FP64_Vector2* column) { if (column_number == 1) { matrix->row1_col1 = column->x1; matrix->row2_col1 = column->x2; return; } if (column_number == 2) { matrix->row1_col2 = column->x1; matrix->row2_col2 = column->x2; } } // ==================== Add ===================== // inline void bgc_fp32_matrix2x2_add(BGC_FP32_Matrix2x2* sum, const BGC_FP32_Matrix2x2* matrix1, const BGC_FP32_Matrix2x2* matrix2) { sum->row1_col1 = matrix1->row1_col1 + matrix2->row1_col1; sum->row1_col2 = matrix1->row1_col2 + matrix2->row1_col2; sum->row2_col1 = matrix1->row2_col1 + matrix2->row2_col1; sum->row2_col2 = matrix1->row2_col2 + matrix2->row2_col2; } inline void bgc_fp64_matrix2x2_add(BGC_FP64_Matrix2x2* sum, const BGC_FP64_Matrix2x2* matrix1, const BGC_FP64_Matrix2x2* matrix2) { sum->row1_col1 = matrix1->row1_col1 + matrix2->row1_col1; sum->row1_col2 = matrix1->row1_col2 + matrix2->row1_col2; sum->row2_col1 = matrix1->row2_col1 + matrix2->row2_col1; sum->row2_col2 = matrix1->row2_col2 + matrix2->row2_col2; } // ================= Add scaled ================= // inline void bgc_fp32_matrix2x2_add_scaled(BGC_FP32_Matrix2x2* sum, const BGC_FP32_Matrix2x2* basic_matrix, const BGC_FP32_Matrix2x2* scalable_matrix, const float scale) { sum->row1_col1 = basic_matrix->row1_col1 + scalable_matrix->row1_col1 * scale; sum->row1_col2 = basic_matrix->row1_col2 + scalable_matrix->row1_col2 * scale; sum->row2_col1 = basic_matrix->row2_col1 + scalable_matrix->row2_col1 * scale; sum->row2_col2 = basic_matrix->row2_col2 + scalable_matrix->row2_col2 * scale; } inline void bgc_fp64_matrix2x2_add_scaled(BGC_FP64_Matrix2x2* sum, const BGC_FP64_Matrix2x2* basic_matrix, const BGC_FP64_Matrix2x2* scalable_matrix, const double scale) { sum->row1_col1 = basic_matrix->row1_col1 + scalable_matrix->row1_col1 * scale; sum->row1_col2 = basic_matrix->row1_col2 + scalable_matrix->row1_col2 * scale; sum->row2_col1 = basic_matrix->row2_col1 + scalable_matrix->row2_col1 * scale; sum->row2_col2 = basic_matrix->row2_col2 + scalable_matrix->row2_col2 * scale; } // ================== Subtract ================== // inline void bgc_fp32_matrix2x2_subtract(BGC_FP32_Matrix2x2* difference, const BGC_FP32_Matrix2x2* minuend, const BGC_FP32_Matrix2x2* subtrahend) { difference->row1_col1 = minuend->row1_col1 - subtrahend->row1_col1; difference->row1_col2 = minuend->row1_col2 - subtrahend->row1_col2; difference->row2_col1 = minuend->row2_col1 - subtrahend->row2_col1; difference->row2_col2 = minuend->row2_col2 - subtrahend->row2_col2; } inline void bgc_fp64_matrix2x2_subtract(BGC_FP64_Matrix2x2* difference, const BGC_FP64_Matrix2x2* minuend, const BGC_FP64_Matrix2x2* subtrahend) { difference->row1_col1 = minuend->row1_col1 - subtrahend->row1_col1; difference->row1_col2 = minuend->row1_col2 - subtrahend->row1_col2; difference->row2_col1 = minuend->row2_col1 - subtrahend->row2_col1; difference->row2_col2 = minuend->row2_col2 - subtrahend->row2_col2; } // ================== Multiply ================== // inline void bgc_fp32_matrix2x2_multiply(BGC_FP32_Matrix2x2* product, const BGC_FP32_Matrix2x2* multiplicand, const float multiplier) { product->row1_col1 = multiplicand->row1_col1 * multiplier; product->row1_col2 = multiplicand->row1_col2 * multiplier; product->row2_col1 = multiplicand->row2_col1 * multiplier; product->row2_col2 = multiplicand->row2_col2 * multiplier; } inline void bgc_fp64_matrix2x2_multiply(BGC_FP64_Matrix2x2* product, const BGC_FP64_Matrix2x2* multiplicand, const double multiplier) { product->row1_col1 = multiplicand->row1_col1 * multiplier; product->row1_col2 = multiplicand->row1_col2 * multiplier; product->row2_col1 = multiplicand->row2_col1 * multiplier; product->row2_col2 = multiplicand->row2_col2 * multiplier; } // =================== Divide =================== // inline void bgc_fp32_matrix2x2_divide(BGC_FP32_Matrix2x2* quotient, const BGC_FP32_Matrix2x2* dividend, const float divisor) { bgc_fp32_matrix2x2_multiply(quotient, dividend, 1.0f / divisor); } inline void bgc_fp64_matrix2x2_divide(BGC_FP64_Matrix2x2* quotient, const BGC_FP64_Matrix2x2* dividend, const double divisor) { bgc_fp64_matrix2x2_multiply(quotient, dividend, 1.0 / divisor); } // ================ Interpolate ================= // inline void bgc_fp32_matrix2x2_interpolate(BGC_FP32_Matrix2x2* interpolation, const BGC_FP32_Matrix2x2* first, const BGC_FP32_Matrix2x2* second, const float phase) { const float counter_phase = 1.0f - phase; interpolation->row1_col1 = first->row1_col1 * counter_phase + second->row1_col1 * phase; interpolation->row1_col2 = first->row1_col2 * counter_phase + second->row1_col2 * phase; interpolation->row2_col1 = first->row2_col1 * counter_phase + second->row2_col1 * phase; interpolation->row2_col2 = first->row2_col2 * counter_phase + second->row2_col2 * phase; } inline void bgc_fp64_matrix2x2_interpolate(BGC_FP64_Matrix2x2* interpolation, const BGC_FP64_Matrix2x2* first, const BGC_FP64_Matrix2x2* second, const double phase) { const double counter_phase = 1.0 - phase; interpolation->row1_col1 = first->row1_col1 * counter_phase + second->row1_col1 * phase; interpolation->row1_col2 = first->row1_col2 * counter_phase + second->row1_col2 * phase; interpolation->row2_col1 = first->row2_col1 * counter_phase + second->row2_col1 * phase; interpolation->row2_col2 = first->row2_col2 * counter_phase + second->row2_col2 * phase; } // ============ Right Vector Product ============ // inline void bgc_fp32_multiply_matrix2x2_by_vector2(BGC_FP32_Vector2* product, const BGC_FP32_Matrix2x2* matrix, const BGC_FP32_Vector2* vector) { const float x1 = matrix->row1_col1 * vector->x1 + matrix->row1_col2 * vector->x2; const float x2 = matrix->row2_col1 * vector->x1 + matrix->row2_col2 * vector->x2; product->x1 = x1; product->x2 = x2; } inline void bgc_fp64_multiply_matrix2x2_by_vector2(BGC_FP64_Vector2* product, const BGC_FP64_Matrix2x2* matrix, const BGC_FP64_Vector2* vector) { const double x1 = matrix->row1_col1 * vector->x1 + matrix->row1_col2 * vector->x2; const double x2 = matrix->row2_col1 * vector->x1 + matrix->row2_col2 * vector->x2; product->x1 = x1; product->x2 = x2; } // ============ Left Vector Product ============= // inline void bgc_fp32_multiply_vector2_by_matrix2x2(BGC_FP32_Vector2* product, const BGC_FP32_Vector2* vector, const BGC_FP32_Matrix2x2* matrix) { const float x1 = vector->x1 * matrix->row1_col1 + vector->x2 * matrix->row2_col1; const float x2 = vector->x1 * matrix->row1_col2 + vector->x2 * matrix->row2_col2; product->x1 = x1; product->x2 = x2; } inline void bgc_fp64_multiply_vector2_by_matrix2x2(BGC_FP64_Vector2* product, const BGC_FP64_Vector2* vector, const BGC_FP64_Matrix2x2* matrix) { const double x1 = vector->x1 * matrix->row1_col1 + vector->x2 * matrix->row2_col1; const double x2 = vector->x1 * matrix->row1_col2 + vector->x2 * matrix->row2_col2; product->x1 = x1; product->x2 = x2; } #endif