You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
540 lines
14 KiB
540 lines
14 KiB
/**
|
|
* @addtogroup testsuite
|
|
* @{
|
|
* @addtogroup matrixtests
|
|
* @{
|
|
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
|
|
* @version 1.0
|
|
*
|
|
* @section LICENSE
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License as
|
|
* published by the Free Software Foundation; either version 2 of
|
|
* the License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* General Public License for more details at
|
|
* https://www.gnu.org/copyleft/gpl.html
|
|
*
|
|
* @section DESCRIPTION
|
|
*
|
|
* This file contains the Trajectory for a bang bang trajectory.
|
|
* The bang bang trajectory is a trajectory with linear acceleration
|
|
* phase followed by a direct de-acceleration phase
|
|
*
|
|
* The slowest joint is defining the speed of the other joints.
|
|
* By that all joints start and stop the movement synchronously
|
|
*/
|
|
#include "CppUTest/TestHarness.h"
|
|
#include "CppUTest/TestRegistry.h"
|
|
#include "CppUTest/TestOutput.h"
|
|
#include "CppUTest/TestTestingFixture.h"
|
|
|
|
#include "Mat.h"
|
|
|
|
#define TESTSIZE 4
|
|
int testMat [TESTSIZE][TESTSIZE];
|
|
int testVec [TESTSIZE];
|
|
float tolerance = 0.00001f;
|
|
|
|
/**
|
|
* testgroup for the matrix class
|
|
*/
|
|
TEST_GROUP(Matrix)
|
|
{
|
|
/**
|
|
* setup for for all test suite
|
|
* This Function is called before every Test case
|
|
*/
|
|
void setup()
|
|
{
|
|
// initializing the testmatrix
|
|
int i = 1;
|
|
int j = 1;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
testMat[x][y] = i++;
|
|
}
|
|
testVec[x] = j++;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* exit for for all test suite
|
|
* This Function is called after every Test case
|
|
*/
|
|
void teardown()
|
|
{
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Test the matrix for invert an non identity matrix
|
|
* with a x rotation
|
|
*/
|
|
TEST(Matrix, vectorInvRotX)
|
|
{
|
|
// setup the test matrix to an identity matrix
|
|
Mat<float, TESTSIZE> a;
|
|
for(int x = 0 ; x < TESTSIZE ; ++x)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; ++y)
|
|
{
|
|
float val = 0.0f;
|
|
if (x == y )
|
|
val = 1.0f;
|
|
a(x,x) = val;
|
|
}
|
|
}
|
|
|
|
// set the rotation matrix to 45 degree in z
|
|
a(0,0) = 1.0f;
|
|
a(1,1) = cos(45.0f *M_PI/180.0f);
|
|
a(1,2) = -sin(45.0f *M_PI/180.0f);
|
|
a(2,2) = cos(45.0f *M_PI/180.0f);
|
|
a(2,1) = sin(45.0f *M_PI/180.0f);
|
|
|
|
float deg = 45.0f*M_PI/180.0f;
|
|
|
|
// check the matrix after setting it up
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
|
|
DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
|
|
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
|
|
// invert the matrix
|
|
a = a.inv();
|
|
|
|
// check for the negative angle for a z rotation
|
|
deg = -45.0f*M_PI/180.0f;
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
|
|
DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
|
|
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test the matrix for invert an non identity matrix
|
|
* with a y rotation
|
|
*/
|
|
TEST(Matrix, vectorInvRotY)
|
|
{
|
|
// setup the test matrix to an identity matrix
|
|
Mat<float, TESTSIZE> a(0.0f);
|
|
|
|
float deg = 45.0f*M_PI/180.0f;
|
|
a(1,1) = 1.0f;
|
|
a(0,0) = cos(deg);
|
|
a(0,2) = sin(deg);
|
|
a(2,2) = cos(deg);
|
|
a(2,0) = -sin(deg);
|
|
a(3,3) = 1.0f;
|
|
|
|
std::cout << a;
|
|
|
|
a = a.inv();
|
|
|
|
deg = -45.0f*M_PI/180.0f;
|
|
DOUBLES_EQUAL(cos(deg),a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(sin(deg),a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(-sin(deg),a(2,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
|
|
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test the matrix for invert an identity matrix
|
|
*/
|
|
TEST(Matrix, matrixInvEye)
|
|
{
|
|
Mat<float, TESTSIZE> a(0.0f , 1.0f);
|
|
|
|
// do the invert
|
|
a = a.inv();
|
|
|
|
// check if the matrix is still an identity matrix
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
}
|
|
|
|
|
|
/**
|
|
* Test if the matrix constructor for identity matrix is working
|
|
*/
|
|
TEST(Matrix, matrixEye)
|
|
{
|
|
Mat<float, TESTSIZE> a(0.0f , 1.0f);
|
|
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an identity matrix
|
|
* This should always return an 1
|
|
*/
|
|
TEST(Matrix, matrixDeterminantEye)
|
|
{
|
|
//eye matrix
|
|
Mat<float, TESTSIZE> a(0.0f, 1.0f);
|
|
|
|
DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an identity matrix
|
|
* with only 3 dimensions.
|
|
* This should always return an 1
|
|
*/
|
|
TEST(Matrix, matrixDeterminantEye3)
|
|
{
|
|
//eye matrix
|
|
Mat<float, 3> a(0.0f, 1.0f);
|
|
|
|
DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an identity matrix
|
|
* with 1 dimension should always return the first cell
|
|
*/
|
|
TEST(Matrix, matrixDeterminantSimple)
|
|
{
|
|
Mat<float, 1> a;
|
|
a(0,0) = 50.0f;
|
|
|
|
DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an identity matrix
|
|
* with 2 dimension should always return the multiplication of the diagonal
|
|
*/
|
|
TEST(Matrix, matrixDeterminantSimple2)
|
|
{
|
|
Mat<float, 2> a;
|
|
a(0,0) = 10.0f;
|
|
a(1,1) = 5.0f;
|
|
DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an complete non zero matrix
|
|
* with 2 dimension should always return the multiplication of the diagonal
|
|
* and negative counter diagonal multiplication
|
|
*/
|
|
TEST(Matrix, matrixDeterminantSimple3)
|
|
{
|
|
Mat<float, 2> a;
|
|
a(0,0) = 10.0f;
|
|
a(0,1) = 5.0f;
|
|
a(1,0) = 10.0f;
|
|
a(1,1) = 5.0f;
|
|
DOUBLES_EQUAL(0.0f,a.determinant(), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the determinant function is working with an complete non zero matrix
|
|
* with 2 dimension should always return the multiplication of the diagonal
|
|
* and negative counter diagonal multiplication
|
|
*/
|
|
TEST(Matrix, matrixDeterminantSimple4)
|
|
{
|
|
Mat<float, 2> a;
|
|
a(0,0) = 10.0f;
|
|
a(0,1) = 5.0f;
|
|
a(1,0) = 5.0f;
|
|
a(1,1) = 10.0f;
|
|
DOUBLES_EQUAL(75.0f,a.determinant(), tolerance);
|
|
}
|
|
|
|
/**
|
|
* Test if the transpose function is working with an identity matrix
|
|
* with 4 dimension should always be the same again
|
|
*/
|
|
TEST(Matrix, matrixTransposeEye)
|
|
{
|
|
//eye matrix
|
|
Mat<float, TESTSIZE> a(0.0f,1.0f);
|
|
|
|
// check if the identity matrix is correctly created
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
|
|
// invert the identity matrix
|
|
a = a.inv();
|
|
|
|
// should still be the same
|
|
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
|
|
|
|
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
|
|
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
|
|
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
|
|
}
|
|
|
|
#if 0
|
|
/**
|
|
* Test if the transpose function is working with an identity matrix
|
|
* with 4 dimension should always be the same again
|
|
*/
|
|
TEST(Matrix, vectorMultiply)
|
|
{
|
|
Mat<int, TESTSIZE> a = testMat;
|
|
Vec<int, TESTSIZE> v = testVec;
|
|
|
|
Vec<int, TESTSIZE> b = a * v;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
int val = 0;
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
val += testMat[x][y] * testVec[x];
|
|
}
|
|
CHECK_EQUAL(val,b(x));
|
|
}
|
|
}
|
|
#endif
|
|
|
|
TEST(Matrix, scalarDivide)
|
|
{
|
|
Mat<int, TESTSIZE> a = testMat;
|
|
|
|
a = a / 5;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = testMat[x][y] / 5;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, scalarSubstract)
|
|
{
|
|
Mat<int, TESTSIZE> a = testMat;
|
|
|
|
a = a - 5;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = testMat[x][y] - 5;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, scalarAdd)
|
|
{
|
|
Mat<int, TESTSIZE> a = testMat;
|
|
|
|
a = a + 5;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = testMat[x][y] + 5;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, scalarMultiply)
|
|
{
|
|
Mat<int, TESTSIZE> a = testMat;
|
|
|
|
a = a + 5;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = testMat[x][y] + 5;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, setIndex)
|
|
{
|
|
Mat<int,TESTSIZE> a ;
|
|
Mat<int,TESTSIZE> b ;
|
|
|
|
int val = 1;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
a(x,y) = val;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
val++;
|
|
}
|
|
}
|
|
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
CHECK_EQUAL(0,b(x,y));
|
|
}
|
|
}
|
|
b = a;
|
|
val = 1;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
a(x,y) = val;
|
|
CHECK_EQUAL(val,b(x,y));
|
|
val++;
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, initAndSet)
|
|
{
|
|
Mat<int,TESTSIZE> a ;
|
|
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = (x+1)*TESTSIZE+y;
|
|
a(x,y) = val;
|
|
}
|
|
}
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
int val = (x+1)*TESTSIZE+y;
|
|
CHECK_EQUAL(val,a(x,y));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Matrix, initZeroed)
|
|
{
|
|
Mat<int,TESTSIZE> a ;
|
|
for (int x = 0; x <TESTSIZE ; x++)
|
|
{
|
|
for (int y = 0; y <TESTSIZE ; y++)
|
|
{
|
|
CHECK_EQUAL(0 , a(x,y) );
|
|
}
|
|
}
|
|
}
|