int main(void) {
const Mesh m(IPoint(MV::fill,2,2));
Tensor<DataMesh> t(3,"aa",m ,0.0);
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
for(int k=0;k<m.Size();k++){
srand ( time(NULL) );
t(i,j)[k]=rand()/((k+1)*10);
}
}
}
Tensor<DataMesh> inv = findInverse(t);
Tensor<DataMesh> inv2 = findInverse(inv);
checkInverse(inv,inv2);
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
std::cout<<inv(i,j)-inv2(i,j)<<std::endl;
}
}
return EXIT_SUCCESS;
}
inline mat4 inverse(mat4 const& m)
{
float coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
float coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3];
float coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3];
float coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
float coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3];
float coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3];
float coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
float coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2];
float coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2];
float coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
float coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3];
float coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3];
float coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
float coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2];
float coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2];
float coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
float coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1];
float coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1];
vec4 fac0(coef00, coef00, coef02, coef03);
vec4 fac1(coef04, coef04, coef06, coef07);
vec4 fac2(coef08, coef08, coef10, coef11);
vec4 fac3(coef12, coef12, coef14, coef15);
vec4 fac4(coef16, coef16, coef18, coef19);
vec4 fac5(coef20, coef20, coef22, coef23);
vec4 v0(m[1][0], m[0][0], m[0][0], m[0][0]);
vec4 v1(m[1][1], m[0][1], m[0][1], m[0][1]);
vec4 v2(m[1][2], m[0][2], m[0][2], m[0][2]);
vec4 v3(m[1][3], m[0][3], m[0][3], m[0][3]);
vec4 inv0(v1 * fac0 - v2 * fac1 + v3 * fac2);
vec4 inv1(v0 * fac0 - v2 * fac3 + v3 * fac4);
vec4 inv2(v0 * fac1 - v1 * fac3 + v3 * fac5);
vec4 inv3(v0 * fac2 - v1 * fac4 + v2 * fac5);
vec4 signA(+1, -1, +1, -1);
vec4 signB(-1, +1, -1, +1);
mat4 inv(inv0 * signA, inv1 * signB, inv2 * signA, inv3 * signB);
vec4 row0(inv[0][0], inv[1][0], inv[2][0], inv[3][0]);
vec4 dot0(m[0] * row0);
float dot1 = (dot0.x + dot0.y) + (dot0.z + dot0.w);
float one_over_det = 1.f / dot1;
return inv * one_over_det;
}
void kalman_2D_update(kalman_filter_2D_t *kalman, vector_2_t measurement)
{
vector_2_t innovation = vsub2( measurement,
mvmul2(kalman->observation_model, kalman->state));
matrix_2x2_t innovation_covariance = madd2( mmul2( mmul2(kalman->observation_model, kalman->covariance),
trans2(kalman->observation_model)),
kalman->noise_measurement);
matrix_2x2_t kalman_gain = mmul2( mmul2(kalman->covariance, trans2(kalman->observation_model)),
inv2(innovation_covariance));
kalman->state = vadd2(kalman->state, mvmul2(kalman_gain, innovation));
kalman->covariance = mmul2( msub2(ident_2x2, mmul2(kalman_gain, kalman->observation_model)),
kalman->covariance);
}
#include "test_small_matrix.h"
#include "small_matrix.h"
#include "linear_algebra.h"
#include "print_util.h"
bool run_matrix_tests(void)
{
matrix_2x2_t m2={.v={{1, 2},{3,2}}};
print_util_print_matrix(print_util_get_debug_stream(), m2.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2t=trans2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2t.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2i=inv2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2i.v[0], 2, 2, 2); print_util_dbg_print("\n");
print_util_print_matrix(print_util_get_debug_stream(), madd2(m2, m2t).v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2_=mmul2(m2, m2i);
print_util_print_matrix(print_util_get_debug_stream(), m2_.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_3x3_t m3={.v={{1, 2, 4},{2,4,2},{4,2,1}}};
print_util_print_matrix(print_util_get_debug_stream(), m3.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3d=diag_3x3(row3(m3, 0));
print_util_print_matrix(print_util_get_debug_stream(), m3d.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3t=trans3(m3);
