Отказ от функций getWeightedSum в пользу appendScaled; оптимизация версоров / Replacing of getWeightedSum onto appendScaled; versor optimization
This commit is contained in:
parent
7bd9c07f17
commit
e354b2425c
13 changed files with 331 additions and 835 deletions
144
src/matrix2x2.h
144
src/matrix2x2.h
|
|
@ -345,6 +345,26 @@ static inline void dp_matrix2x2_set_column2(const double r1, const double r2, DP
|
|||
matrix->r2c2 = r2;
|
||||
}
|
||||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_matrix2x2_append_scaled(SPMatrix2x2* basic_vector, const SPMatrix2x2* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x2_append_scaled(DPMatrix2x2* basic_vector, const DPMatrix2x2* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
}
|
||||
|
||||
// ================== Addition ================== //
|
||||
|
||||
static inline void sp_matrix2x2_add(const SPMatrix2x2* matrix1, const SPMatrix2x2* matrix2, SPMatrix2x2* sum)
|
||||
|
|
@ -425,130 +445,6 @@ static inline void dp_matrix2x2_divide(const DPMatrix2x2* dividend, const double
|
|||
quotient->r2c2 = dividend->r2c2 / divisor;
|
||||
}
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
static inline void sp_matrix2x2_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix2x2* matrix1,
|
||||
const float weight2, const SPMatrix2x2* matrix2,
|
||||
SPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x2_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix2x2* matrix1,
|
||||
const double weight2, const DPMatrix2x2* matrix2,
|
||||
DPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
static inline void sp_matrix2x2_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix2x2* matrix1,
|
||||
const float weight2, const SPMatrix2x2* matrix2,
|
||||
const float weight3, const SPMatrix2x2* matrix3,
|
||||
SPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x2_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix2x2* matrix1,
|
||||
const double weight2, const DPMatrix2x2* matrix2,
|
||||
const double weight3, const DPMatrix2x2* matrix3,
|
||||
DPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
static inline void sp_matrix2x2_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix2x2* matrix1,
|
||||
const float weight2, const SPMatrix2x2* matrix2,
|
||||
const float weight3, const SPMatrix2x2* matrix3,
|
||||
const float weight4, const SPMatrix2x2* matrix4,
|
||||
SPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x2_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix2x2* matrix1,
|
||||
const double weight2, const DPMatrix2x2* matrix2,
|
||||
const double weight3, const DPMatrix2x2* matrix3,
|
||||
const double weight4, const DPMatrix2x2* matrix4,
|
||||
DPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
static inline void sp_matrix2x2_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix2x2* matrix1,
|
||||
const float weight2, const SPMatrix2x2* matrix2,
|
||||
const float weight3, const SPMatrix2x2* matrix3,
|
||||
const float weight4, const SPMatrix2x2* matrix4,
|
||||
const float weight5, const SPMatrix2x2* matrix5,
|
||||
SPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x2_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix2x2* matrix1,
|
||||
const double weight2, const DPMatrix2x2* matrix2,
|
||||
const double weight3, const DPMatrix2x2* matrix3,
|
||||
const double weight4, const DPMatrix2x2* matrix4,
|
||||
const double weight5, const DPMatrix2x2* matrix5,
|
||||
DPMatrix2x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
}
|
||||
|
||||
// ============ Left Vector Product ============= //
|
||||
|
||||
static inline void sp_matrix2x2_left_product(const SPVector2* vector, const SPMatrix2x2* matrix, SPVector2* result)
|
||||
|
|
|
|||
147
src/matrix2x3.c
147
src/matrix2x3.c
|
|
@ -1,149 +1,2 @@
|
|||
#include "matrix2x3.h"
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
SPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
|
||||
}
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
DPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
SPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
|
||||
}
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
DPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
const float weight4, const SPMatrix2x3* matrix4,
|
||||
SPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
|
||||
}
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
const double weight4, const DPMatrix2x3* matrix4,
|
||||
DPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
const float weight4, const SPMatrix2x3* matrix4,
|
||||
const float weight5, const SPMatrix2x3* matrix5,
|
||||
SPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
|
||||
}
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
const double weight4, const DPMatrix2x3* matrix4,
|
||||
const double weight5, const DPMatrix2x3* matrix5,
|
||||
DPMatrix2x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -209,6 +209,32 @@ static inline void dp_matrix2x3_set_column2(const double r1, const double r2, co
|
|||
matrix->r3c2 = r3;
|
||||
}
|
||||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_matrix2x3_append_scaled(SPMatrix2x3* basic_vector, const SPMatrix2x3* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
|
||||
basic_vector->r3c1 += scalable_vector->r3c1 * scale;
|
||||
basic_vector->r3c2 += scalable_vector->r3c2 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_matrix2x3_append_scaled(DPMatrix2x3* basic_vector, const DPMatrix2x3* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
|
||||
basic_vector->r3c1 += scalable_vector->r3c1 * scale;
|
||||
basic_vector->r3c2 += scalable_vector->r3c2 * scale;
|
||||
}
|
||||
|
||||
// ================== Addition ================== //
|
||||
|
||||
static inline void sp_matrix2x3_add(const SPMatrix2x3* matrix1, const SPMatrix2x3* matrix2, SPMatrix2x3* sum)
|
||||
|
|
@ -313,74 +339,6 @@ static inline void dp_matrix2x3_divide(const DPMatrix2x3* dividend, const double
|
|||
quotient->r3c2 = dividend->r3c2 / divisor;
|
||||
}
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
SPMatrix2x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
DPMatrix2x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
SPMatrix2x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
DPMatrix2x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
const float weight4, const SPMatrix2x3* matrix4,
|
||||
SPMatrix2x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
const double weight4, const DPMatrix2x3* matrix4,
|
||||
DPMatrix2x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix2x3_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix2x3* matrix1,
|
||||
const float weight2, const SPMatrix2x3* matrix2,
|
||||
const float weight3, const SPMatrix2x3* matrix3,
|
||||
const float weight4, const SPMatrix2x3* matrix4,
|
||||
const float weight5, const SPMatrix2x3* matrix5,
|
||||
SPMatrix2x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix2x3_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix2x3* matrix1,
|
||||
const double weight2, const DPMatrix2x3* matrix2,
|
||||
const double weight3, const DPMatrix2x3* matrix3,
|
||||
const double weight4, const DPMatrix2x3* matrix4,
|
||||
const double weight5, const DPMatrix2x3* matrix5,
|
||||
DPMatrix2x3* sum
|
||||
);
|
||||
|
||||
// ============ Left Vector Product ============= //
|
||||
|
||||
static inline void sp_matrix2x3_left_product(const SPVector3* vector, const SPMatrix2x3* matrix, SPVector2* result)
|
||||
|
|
|
|||
139
src/matrix3x2.c
139
src/matrix3x2.c
|
|
@ -1,141 +1,2 @@
|
|||
#include "matrix3x2.h"
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
SPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;
|
||||
}
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
DPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
SPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;
|
||||
}
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
DPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
const float weight4, const SPMatrix3x2* matrix4,
|
||||
SPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);
|
||||
}
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
const double weight4, const DPMatrix3x2* matrix4,
|
||||
DPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
const float weight4, const SPMatrix3x2* matrix4,
|
||||
const float weight5, const SPMatrix3x2* matrix5,
|
||||
SPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;
|
||||
}
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
const double weight4, const DPMatrix3x2* matrix4,
|
||||
const double weight5, const DPMatrix3x2* matrix5,
|
||||
DPMatrix3x2* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -175,6 +175,30 @@ static inline void dp_matrix3x2_set_column3(const double r1, const double r2, DP
|
|||
matrix->r2c3 = r2;
|
||||
}
|
||||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_matrix3x2_append_scaled(SPMatrix3x2* basic_vector, const SPMatrix3x2* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
basic_vector->r1c3 += scalable_vector->r1c3 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
basic_vector->r2c3 += scalable_vector->r2c3 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_matrix3x2_append_scaled(DPMatrix3x2* basic_vector, const DPMatrix3x2* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
basic_vector->r1c3 += scalable_vector->r1c3 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
basic_vector->r2c3 += scalable_vector->r2c3 * scale;
|
||||
}
|
||||
|
||||
// ================== Addition ================== //
|
||||
|
||||
static inline void sp_matrix3x2_add(const SPMatrix3x2* matrix1, const SPMatrix3x2* matrix2, SPMatrix3x2* sum)
|
||||
|
|
@ -271,74 +295,6 @@ static inline void dp_matrix3x2_divide(const DPMatrix3x2* dividend, const double
|
|||
quotient->r2c3 = dividend->r2c3 / divisor;
|
||||
}
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
SPMatrix3x2* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
DPMatrix3x2* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
SPMatrix3x2* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
DPMatrix3x2* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
const float weight4, const SPMatrix3x2* matrix4,
|
||||
SPMatrix3x2* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
const double weight4, const DPMatrix3x2* matrix4,
|
||||
DPMatrix3x2* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix3x2_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix3x2* matrix1,
|
||||
const float weight2, const SPMatrix3x2* matrix2,
|
||||
const float weight3, const SPMatrix3x2* matrix3,
|
||||
const float weight4, const SPMatrix3x2* matrix4,
|
||||
const float weight5, const SPMatrix3x2* matrix5,
|
||||
SPMatrix3x2* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x2_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix3x2* matrix1,
|
||||
const double weight2, const DPMatrix3x2* matrix2,
|
||||
const double weight3, const DPMatrix3x2* matrix3,
|
||||
const double weight4, const DPMatrix3x2* matrix4,
|
||||
const double weight5, const DPMatrix3x2* matrix5,
|
||||
DPMatrix3x2* sum
|
||||
);
|
||||
|
||||
// ============ Left Vector Product ============= //
|
||||
|
||||
static inline void sp_matrix3x2_left_product(const SPVector2* vector, const SPMatrix3x2* matrix, SPVector3* result)
|
||||
|
|
|
|||
176
src/matrix3x3.c
176
src/matrix3x3.c
|
|
@ -74,7 +74,7 @@ int dp_matrix3x3_invert(DPMatrix3x3* matrix)
|
|||
|
||||
// ================ Make Inverted =============== //
|
||||
|
||||
int sp_matrix3x3_make_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result)
|
||||
int sp_matrix3x3_set_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result)
|
||||
{
|
||||
const float determinant = sp_matrix3x3_get_determinant(matrix);
|
||||
|
||||
|
|
@ -109,7 +109,7 @@ int sp_matrix3x3_make_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result)
|
|||
return 1;
|
||||
}
|
||||
|
||||
int dp_matrix3x3_make_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result)
|
||||
int dp_matrix3x3_set_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result)
|
||||
{
|
||||
const double determinant = dp_matrix3x3_get_determinant(matrix);
|
||||
|
||||
|
|
@ -143,175 +143,3 @@ int dp_matrix3x3_make_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result)
|
|||
|
||||
return 1;
|
||||
}
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
SPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
|
||||
sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2;
|
||||
}
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
DPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
|
||||
sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
SPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
|
||||
sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3;
|
||||
}
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
DPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
|
||||
sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
|
||||
sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;
|
||||
|
||||
sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
|
||||
sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
|
||||
sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;
|
||||
|
||||
sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
|
||||
sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
|
||||
sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3;
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
const float weight4, const SPMatrix3x3* matrix4,
|
||||
SPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
|
||||
sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4);
|
||||
}
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
const double weight4, const DPMatrix3x3* matrix4,
|
||||
DPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
|
||||
sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4);
|
||||
}
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
const float weight4, const SPMatrix3x3* matrix4,
|
||||
const float weight5, const SPMatrix3x3* matrix5,
|
||||
SPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
|
||||
sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5;
|
||||
}
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
const double weight4, const DPMatrix3x3* matrix4,
|
||||
const double weight5, const DPMatrix3x3* matrix5,
|
||||
DPMatrix3x3* sum
|
||||
)
|
||||
{
|
||||
sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
|
||||
sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
|
||||
sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;
|
||||
|
||||
sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
|
||||
sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
|
||||
sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;
|
||||
|
||||
sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
|
||||
sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
|
||||
sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5;
|
||||
}
|
||||
|
|
|
|||
100
src/matrix3x3.h
100
src/matrix3x3.h
|
|
@ -378,6 +378,38 @@ static inline void dp_matrix3x3_set_column3(const double r1, const double r2, co
|
|||
matrix->r3c3 = r3;
|
||||
}
|
||||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_matrix3x3_append_scaled(SPMatrix3x3* basic_vector, const SPMatrix3x3* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
basic_vector->r1c3 += scalable_vector->r1c3 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
basic_vector->r2c3 += scalable_vector->r2c3 * scale;
|
||||
|
||||
basic_vector->r3c1 += scalable_vector->r3c1 * scale;
|
||||
basic_vector->r3c2 += scalable_vector->r3c2 * scale;
|
||||
basic_vector->r3c3 += scalable_vector->r3c3 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_matrix3x3_append_scaled(DPMatrix3x3* basic_vector, const DPMatrix3x3* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->r1c1 += scalable_vector->r1c1 * scale;
|
||||
basic_vector->r1c2 += scalable_vector->r1c2 * scale;
|
||||
basic_vector->r1c3 += scalable_vector->r1c3 * scale;
|
||||
|
||||
basic_vector->r2c1 += scalable_vector->r2c1 * scale;
|
||||
basic_vector->r2c2 += scalable_vector->r2c2 * scale;
|
||||
basic_vector->r2c3 += scalable_vector->r2c3 * scale;
|
||||
|
||||
basic_vector->r3c1 += scalable_vector->r3c1 * scale;
|
||||
basic_vector->r3c2 += scalable_vector->r3c2 * scale;
|
||||
basic_vector->r3c3 += scalable_vector->r3c3 * scale;
|
||||
}
|
||||
|
||||
// ================== Addition ================== //
|
||||
|
||||
static inline void sp_matrix3x3_add(const SPMatrix3x3* matrix1, const SPMatrix3x3* matrix2, SPMatrix3x3* sum)
|
||||
|
|
@ -506,74 +538,6 @@ static inline void dp_matrix3x3_divide(const DPMatrix3x3* dividend, const double
|
|||
quotient->r3c3 = dividend->r3c3 / divisor;
|
||||
}
|
||||
|
||||
// ============= Weighed Sum of two ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum2(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
SPMatrix3x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum2(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
DPMatrix3x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of three ============ //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum3(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
SPMatrix3x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum3(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
DPMatrix3x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of four ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum4(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
const float weight4, const SPMatrix3x3* matrix4,
|
||||
SPMatrix3x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum4(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
const double weight4, const DPMatrix3x3* matrix4,
|
||||
DPMatrix3x3* sum
|
||||
);
|
||||
|
||||
// ============ Weighed Sum of five ============= //
|
||||
|
||||
void sp_matrix3x3_get_weighted_sum5(
|
||||
const float weight1, const SPMatrix3x3* matrix1,
|
||||
const float weight2, const SPMatrix3x3* matrix2,
|
||||
const float weight3, const SPMatrix3x3* matrix3,
|
||||
const float weight4, const SPMatrix3x3* matrix4,
|
||||
const float weight5, const SPMatrix3x3* matrix5,
|
||||
SPMatrix3x3* sum
|
||||
);
|
||||
|
||||
void dp_matrix3x3_get_weighted_sum5(
|
||||
const double weight1, const DPMatrix3x3* matrix1,
|
||||
const double weight2, const DPMatrix3x3* matrix2,
|
||||
const double weight3, const DPMatrix3x3* matrix3,
|
||||
const double weight4, const DPMatrix3x3* matrix4,
|
||||
const double weight5, const DPMatrix3x3* matrix5,
|
||||
DPMatrix3x3* sum
|
||||
);
|
||||
|
||||
// ============ Left Vector Product ============= //
|
||||
|
||||
static inline void sp_matrix3x3_left_product(const SPVector3* vector, const SPMatrix3x3* matrix, SPVector3* result)
|
||||
|
|
|
|||
|
|
@ -206,16 +206,16 @@ static inline void dp_vector2_divide(const DPVector2* dividend, const double div
|
|||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_vector2_append_scaled(SPVector2* basic_vector, const SPVector2* scalable_summand, const float scale)
|
||||
static inline void sp_vector2_append_scaled(SPVector2* basic_vector, const SPVector2* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->x1 += scalable_summand->x1 * scale;
|
||||
basic_vector->x2 += scalable_summand->x2 * scale;
|
||||
basic_vector->x1 += scalable_vector->x1 * scale;
|
||||
basic_vector->x2 += scalable_vector->x2 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_vector2_append_scaled(DPVector2* basic_vector, const DPVector2* scalable_summand, const double scale)
|
||||
static inline void dp_vector2_append_scaled(DPVector2* basic_vector, const DPVector2* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->x1 += scalable_summand->x1 * scale;
|
||||
basic_vector->x2 += scalable_summand->x2 * scale;
|
||||
basic_vector->x1 += scalable_vector->x1 * scale;
|
||||
basic_vector->x2 += scalable_vector->x2 * scale;
|
||||
}
|
||||
|
||||
// ================== Average2 ================== //
|
||||
|
|
|
|||
|
|
@ -228,18 +228,18 @@ static inline void dp_vector3_divide(const DPVector3* dividend, const double div
|
|||
|
||||
// ================ Append scaled =============== //
|
||||
|
||||
static inline void sp_vector3_append_scaled(SPVector3* basic_vector, const SPVector3* scalable_summand, const float scale)
|
||||
static inline void sp_vector3_append_scaled(SPVector3* basic_vector, const SPVector3* scalable_vector, const float scale)
|
||||
{
|
||||
basic_vector->x1 += scalable_summand->x1 * scale;
|
||||
basic_vector->x2 += scalable_summand->x2 * scale;
|
||||
basic_vector->x3 += scalable_summand->x3 * scale;
|
||||
basic_vector->x1 += scalable_vector->x1 * scale;
|
||||
basic_vector->x2 += scalable_vector->x2 * scale;
|
||||
basic_vector->x3 += scalable_vector->x3 * scale;
|
||||
}
|
||||
|
||||
static inline void dp_vector3_append_scaled(DPVector3* basic_vector, const DPVector3* scalable_summand, const double scale)
|
||||
static inline void dp_vector3_append_scaled(DPVector3* basic_vector, const DPVector3* scalable_vector, const double scale)
|
||||
{
|
||||
basic_vector->x1 += scalable_summand->x1 * scale;
|
||||
basic_vector->x2 += scalable_summand->x2 * scale;
|
||||
basic_vector->x3 += scalable_summand->x3 * scale;
|
||||
basic_vector->x1 += scalable_vector->x1 * scale;
|
||||
basic_vector->x2 += scalable_vector->x2 * scale;
|
||||
basic_vector->x3 += scalable_vector->x3 * scale;
|
||||
}
|
||||
|
||||
// ================== Average2 ================== //
|
||||
|
|
|
|||
66
src/versor.h
66
src/versor.h
|
|
@ -117,16 +117,9 @@ static inline void sp_versor_set(const float s0, const float x1, const float x2,
|
|||
|
||||
const float square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON) {
|
||||
if (s0 > -1.0f + SP_EPSYLON) {
|
||||
return;
|
||||
}
|
||||
|
||||
sp_versor_reset(result);
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_module < 1.0f - SP_TWO_EPSYLON || 1.0f + SP_TWO_EPSYLON < square_module) {
|
||||
__sp_versor_normalize(square_module, result);
|
||||
}
|
||||
}
|
||||
|
||||
static inline void dp_versor_set(const double s0, const double x1, const double x2, const double x3, DPVersor* result)
|
||||
|
|
@ -138,16 +131,9 @@ static inline void dp_versor_set(const double s0, const double x1, const double
|
|||
|
||||
const double square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (1.0 - DP_TWO_EPSYLON <= square_module && square_module <= 1.0 + DP_TWO_EPSYLON) {
|
||||
if (s0 > -1.0 + DP_EPSYLON) {
|
||||
return;
|
||||
}
|
||||
|
||||
dp_versor_reset(result);
|
||||
return;
|
||||
}
|
||||
|
||||
if (square_module < 1.0 - DP_TWO_EPSYLON || 1.0 + DP_TWO_EPSYLON < square_module) {
|
||||
__dp_versor_normalize(square_module, result);
|
||||
}
|
||||
}
|
||||
|
||||
// ==================== Copy ==================== //
|
||||
|
|
@ -268,24 +254,40 @@ static inline void dp_versor_make_inverted(const DPVersor* versor, DPVersor* res
|
|||
|
||||
static inline void sp_versor_combine(const SPVersor* second, const SPVersor* first, SPVersor* result)
|
||||
{
|
||||
sp_versor_set(
|
||||
(second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3),
|
||||
(second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3),
|
||||
(second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1),
|
||||
(second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2),
|
||||
result
|
||||
);
|
||||
const float s0 = (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3);
|
||||
const float x1 = (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3);
|
||||
const float x2 = (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1);
|
||||
const float x3 = (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2);
|
||||
|
||||
const float square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
result->_s0 = s0;
|
||||
result->_x1 = x1;
|
||||
result->_x2 = x2;
|
||||
result->_x3 = x3;
|
||||
|
||||
if (square_module < 1.0f - SP_TWO_EPSYLON || 1.0f + SP_TWO_EPSYLON < square_module) {
|
||||
__sp_versor_normalize(square_module, result);
|
||||
}
|
||||
}
|
||||
|
||||
static inline void dp_versor_combine(const DPVersor* second, const DPVersor* first, DPVersor* result)
|
||||
{
|
||||
dp_versor_set(
|
||||
(second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3),
|
||||
(second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3),
|
||||
(second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1),
|
||||
(second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2),
|
||||
result
|
||||
);
|
||||
const double s0 = (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3);
|
||||
const double x1 = (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3);
|
||||