From d2ccd44c332fbb38c92e90b80d58f91bcaa41a7e Mon Sep 17 00:00:00 2001 From: Anton Dukhovnikov Date: Fri, 8 Dec 2023 10:42:07 +1300 Subject: [PATCH] [fix] bug in vec-mat multiplication Signed-off-by: Anton Dukhovnikov --- include/rawtoaces/mathOps.h | 34 +++++++++++++--------------------- unittest/testMath.cpp | 12 ++++++------ 2 files changed, 19 insertions(+), 27 deletions(-) diff --git a/include/rawtoaces/mathOps.h b/include/rawtoaces/mathOps.h index 17c844e..b7cda48 100644 --- a/include/rawtoaces/mathOps.h +++ b/include/rawtoaces/mathOps.h @@ -409,30 +409,22 @@ T *mulVectorArray( FORI(total) data[i] = MR(i); */ - if ( dim == 3 ) + if ( dim == 3 || dim == 4 ) { for ( uint32_t i = 0; i < total; i += dim ) { - data[i] = vct[0][0] * data[i] + vct[0][1] * data[i + 1] + - vct[0][2] * data[i + 2]; - data[i + 1] = vct[1][0] * data[i] + vct[1][1] * data[i + 1] + - vct[1][2] * data[i + 2]; - data[i + 2] = vct[2][0] * data[i] + vct[2][1] * data[i + 1] + - vct[2][2] * data[i + 2]; - } - } - else if ( dim == 4 ) - { - for ( uint32_t i = 0; i < total; i += 4 ) - { - data[i] = vct[0][0] * data[i] + vct[0][1] * data[i + 1] + - vct[0][2] * data[i + 2] + vct[0][3] * data[i + 3]; - data[i + 1] = vct[1][0] * data[i] + vct[1][1] * data[i + 1] + - vct[1][2] * data[i + 2] + vct[1][3] * data[i + 3]; - data[i + 2] = vct[2][0] * data[i] + vct[2][1] * data[i + 1] + - vct[2][2] * data[i + 2] + vct[2][3] * data[i + 3]; - data[i + 3] = vct[3][0] * data[i] + vct[3][1] * data[i + 1] + - vct[3][2] * data[i + 2] + vct[3][3] * data[i + 3]; + T temp[4]; + + for ( uint8_t j = 0; j < dim; j++ ) + { + temp[j] = 0; + + for ( uint8_t k = 0; k < dim; k++ ) + temp[j] += vct[j][k] * data[i + k]; + } + + for ( uint8_t j = 0; j < dim; j++ ) + data[i + j] = temp[j]; } } diff --git a/unittest/testMath.cpp b/unittest/testMath.cpp index ffc6360..b9c16cb 100644 --- a/unittest/testMath.cpp +++ b/unittest/testMath.cpp @@ -368,15 +368,15 @@ BOOST_AUTO_TEST_CASE( Test_MulVector2 ) BOOST_AUTO_TEST_CASE( Test_MulVectorArray ) { double data[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 }; - double M[3][3] = { { 1.0000000000, 0.0000000000, 0.0000000000 }, - { 0.0000000000, 2.0000000000, 0.0000000000 }, - { 0.0000000000, 0.0000000000, 3.0000000000 } + double M[3][3] = { { 1.0000000000, 0.1000000000, 0.0100000000 }, + { 0.1000000000, 2.0000000000, 0.0100000000 }, + { 0.1000000000, 0.0100000000, 3.0000000000 } }; - double data_test[9] = { 1.0000000000, 4.0000000000, 9.0000000000, - 4.0000000000, 10.0000000000, 18.0000000000, - 7.0000000000, 16.0000000000, 27.0000000000 }; + double data_test[9] = { 1.2300000000, 4.13000000000, 9.12000000000, + 4.5600000000, 10.4600000000, 18.4500000000, + 7.8900000000, 16.7900000000, 27.7800000000 }; vector> MV( 3, vector( 3 ) ); FORIJ( 3, 3 )