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.
494 lines
14 KiB
494 lines
14 KiB
#ifndef _MAT_H_
|
|
#define _MAT_H_
|
|
#include <iostream>
|
|
#include <math.h>
|
|
#include "Vec.h"
|
|
/**
|
|
* @addtogroup math
|
|
* @{
|
|
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
|
|
* @version 2.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 linear trajectory
|
|
* The linear trajectory only uses the max speed and ignores
|
|
* the acceleration parameter.
|
|
* The movement is a strict hard acceleration and deacceration.
|
|
*
|
|
* The slowest joint is defining the speed of the other joints.
|
|
* By that all joints start and stop the movement synchronously
|
|
*/
|
|
|
|
template <class T, unsigned SIZE> class Vec;
|
|
|
|
template <class T, unsigned SIZE> class Mat;
|
|
// some common vector classes (abbr. names)
|
|
typedef Mat<float, 2> Mat2f;
|
|
typedef Mat<float, 3> Mat3f;
|
|
typedef Mat<float, 4> Mat4f;
|
|
|
|
typedef Mat<double, 2> Mat2d;
|
|
typedef Mat<double, 3> Mat3d;
|
|
typedef Mat<double, 4> Mat4d;
|
|
|
|
typedef Mat<float, 4> MatCarthesian;
|
|
|
|
|
|
/**
|
|
* Matrix class for class and simple data types
|
|
* template with type and size of the vector
|
|
*
|
|
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
|
|
* @copyright 2012 philipp schoenberger All rights reserved.
|
|
* @license This project is released under the GNU Public License.
|
|
*/
|
|
template <class T, unsigned SIZE>
|
|
class Mat
|
|
{
|
|
public:
|
|
|
|
/**
|
|
* standard constructor which is initialising all vales to zero
|
|
*/
|
|
Mat<T, SIZE> ()
|
|
{
|
|
for (unsigned int x=0; x<SIZE; x++)
|
|
{
|
|
for (unsigned int y=0; y<SIZE; y++)
|
|
{
|
|
m_aatData[x][y] = T(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* default deconstructor which has to do nothing
|
|
*/
|
|
~Mat<T, SIZE> ()
|
|
{
|
|
// nothing to do here ...
|
|
}
|
|
|
|
/**
|
|
* copy constructor for flat arrays of the simple type
|
|
*
|
|
* @param srcData source data for the new matrix
|
|
*/
|
|
Mat<T, SIZE> (const T srcData[SIZE][SIZE])
|
|
{
|
|
for (unsigned int x=0; x<SIZE; x++)
|
|
{
|
|
for (unsigned int y=0; y<SIZE; y++)
|
|
{
|
|
m_aatData[x][y] = srcData[x][y];
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* constructor for an identity matrix from left top to right bottom
|
|
*
|
|
* @param initVal the value in each field beside the diagonal entries
|
|
* @param eyeVal the value on each filed exactly on the diagonal
|
|
*/
|
|
Mat<T, SIZE> (const T initVal,const T eyeVal)
|
|
{
|
|
for (unsigned int x=0; x<SIZE; x++)
|
|
{
|
|
for (unsigned int y=0; y<SIZE; y++)
|
|
{
|
|
if (x == y)
|
|
m_aatData[x][y] = eyeVal;
|
|
else
|
|
m_aatData[x][y] = initVal;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* constructor for an matrix with a defined value in each cell
|
|
*
|
|
* @param initVal the value for each field
|
|
*/
|
|
Mat<T, SIZE> (const T initVal)
|
|
{
|
|
for (unsigned int x=0; x<SIZE; x++)
|
|
{
|
|
for (unsigned int y=0; y<SIZE; y++)
|
|
{
|
|
m_aatData[x][y] = initVal;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* copy constructor for matrix-matrix copy
|
|
*
|
|
* @param srcMat the value for each field
|
|
*/
|
|
Mat<T, SIZE> (const Mat<T, SIZE> &srcMat)
|
|
{
|
|
for (unsigned int x=0; x<SIZE; x++)
|
|
{
|
|
for (unsigned int y=0; y<SIZE; y++)
|
|
{
|
|
m_aatData[x][y] = srcMat.m_aatData[x][y];
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* operator for cell access and assignment
|
|
*
|
|
* @param x x axis position within the matrix
|
|
* @param y y axis position within the matrix
|
|
*/
|
|
T &operator () (unsigned x, unsigned y)
|
|
{
|
|
if (y>=SIZE)
|
|
y = SIZE-1;
|
|
if (x>=SIZE)
|
|
x = SIZE-1;
|
|
return m_aatData[x][y];
|
|
}
|
|
|
|
/**
|
|
* operator for cell access without writing to it
|
|
*
|
|
* @param x x axis position within the matrix
|
|
* @param y y axis position within the matrix
|
|
*/
|
|
T operator () (unsigned x, unsigned y) const
|
|
{
|
|
if (x>=SIZE)
|
|
x = SIZE-1;
|
|
if (y>=SIZE)
|
|
y = SIZE-1;
|
|
return m_aatData[x][y];
|
|
}
|
|
|
|
/**
|
|
* operator to add a simple type to each entry
|
|
* within the array
|
|
*
|
|
* @param scalar the value to add
|
|
* @return the new calculated matrix
|
|
*/
|
|
Mat<T, SIZE> operator + (const T &scalar)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int x=0; x<SIZE; x++) // row
|
|
for (unsigned int y=0; y<SIZE; y++) // column
|
|
buf(x,y) += m_aatData[x][y] + scalar;
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* operator to subtract a simple type to each entry
|
|
* within the array
|
|
*
|
|
* @param scalar the value to subtract
|
|
* @return the new calculated matrix
|
|
*/
|
|
Mat<T, SIZE> operator - (const T &scalar)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int x=0; x<SIZE; x++) // row
|
|
for (unsigned int y=0; y<SIZE; y++) // column
|
|
buf(x,y) += m_aatData[x][y] - scalar;
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* operator to multiply a simple type to each entry
|
|
* within the array
|
|
*
|
|
* @param scalar the value to multiply the matrix cells
|
|
* @return the new calculated matrix
|
|
*/
|
|
Mat<T, SIZE> operator * (const T &scalar)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++)
|
|
buf(row,column) += m_aatData[row][column] * scalar;
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* operator to divide a simple type to each entry
|
|
* within the array
|
|
*
|
|
* @param scalar the value to divide the matrix cells
|
|
* @return the new calculated matrix
|
|
*/
|
|
Mat<T, SIZE> operator / (const T &scalar)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++)
|
|
buf(row,column) += m_aatData[row][column] / scalar;
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* operator to multiply tow different matrices
|
|
*
|
|
* @param mat matrix to multiply
|
|
* @return the new calculated matrix
|
|
*/
|
|
Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int i=0; i<SIZE; i++) // row i
|
|
for (unsigned int j=0; j<SIZE; j++) // column j
|
|
for (unsigned int a=0; a<SIZE; a++) // a
|
|
buf(i,j) += m_aatData[i][a] * mat(a,j);
|
|
return buf;
|
|
}
|
|
/**
|
|
* This functions is used for the assigning operator.
|
|
* for a simple array for the right hand side.
|
|
* The array has to be column wise coded.
|
|
*
|
|
* @info the template type has to provide the abs function
|
|
*
|
|
* @param aatData the right hand side array
|
|
* @return the new matrix
|
|
*/
|
|
Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE])
|
|
{
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++) // column j
|
|
m_aatData[row][column] = aatData[column+(row*4)];
|
|
|
|
return (*this); // also an R-value in e.g.
|
|
// vec1 = vec2 + (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
|
|
// L-Val = R-Val + R-Val
|
|
// " = " + (L-Val = R-Val)
|
|
}
|
|
|
|
/**
|
|
* This functions calculates the matrix containing only the
|
|
* absolute values of the current one.
|
|
*
|
|
* @info the template type has to provide the abs function
|
|
*
|
|
* @return the matrix with absolute values
|
|
*/
|
|
Mat<T,SIZE> abs()
|
|
{
|
|
Mat<T,SIZE> buf;
|
|
for (unsigned int j=0; j<SIZE; j++) // column j
|
|
for (unsigned int i=0; i<SIZE; i++) // row i
|
|
buf.m_aatData[i][j] = abs(m_aatData[i][j]);
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* This functions calculates the determinant of a matrix
|
|
*
|
|
* @return the determinant
|
|
*/
|
|
T determinant()
|
|
{
|
|
T buf = 0;
|
|
if (SIZE == 1)
|
|
return m_aatData[0][0];
|
|
if (SIZE == 2)
|
|
return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
|
|
|
|
|
|
for (unsigned int i=0; i<SIZE; i++) // column j
|
|
{
|
|
T multBuff;
|
|
multBuff = m_aatData[0][i];
|
|
multBuff *= m_aatData[1][(i+1)%SIZE];
|
|
multBuff *= m_aatData[2][(i+2)%SIZE];
|
|
|
|
//std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";
|
|
|
|
buf += multBuff;
|
|
multBuff = m_aatData[0][i];
|
|
multBuff *= m_aatData[1][(i+SIZE-1)%SIZE];
|
|
multBuff *= m_aatData[2][(i+SIZE-2)%SIZE];
|
|
|
|
//std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";
|
|
|
|
buf -= multBuff;
|
|
}
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* This functions calculates the norm of the matrix.
|
|
*
|
|
* @info the Template type has to provide the sqrt and pow function
|
|
* @return the normalisation factor
|
|
*/
|
|
Mat<T,SIZE> norm()
|
|
{
|
|
T buf;
|
|
for (unsigned int row=0; row < SIZE; row++)
|
|
for (unsigned int column=0; column < SIZE; column++)
|
|
buf += pow(abs(m_aatData[row][column]),2);
|
|
sqrt(buf);
|
|
return buf;
|
|
}
|
|
|
|
/**
|
|
* This functions generates the transposed matrix for the
|
|
* current one.
|
|
* @return the transposed matrix
|
|
*
|
|
* @see transpose
|
|
* @see determinant
|
|
*/
|
|
Mat<T,SIZE> transpose()
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++)
|
|
buf.m_aatData[row][column] = m_aatData[row][column];
|
|
return buf;
|
|
}
|
|
/**
|
|
* This functions generates the inverse matrix for the
|
|
* current one.
|
|
* This is done via the determinant and the transposed.,
|
|
* @return the inverted matrix
|
|
*
|
|
* @see transpose
|
|
* @see determinant
|
|
*/
|
|
Mat<T,SIZE> inv()
|
|
{
|
|
T det = determinant();
|
|
|
|
if (det== 0)
|
|
{
|
|
return *this;
|
|
}
|
|
|
|
Mat<float,4> thisTransposed = this->transpose();
|
|
thisTransposed/det;
|
|
|
|
return thisTransposed;
|
|
}
|
|
/**
|
|
* This functions copies the vales from the matrix to a flat list
|
|
* of simple types. The order will be row wise.
|
|
* @param atData the flat destination array
|
|
*/
|
|
void getData (T atData[SIZE*SIZE])
|
|
{
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++)
|
|
atData[row+column*SIZE] = m_aatData[row][column];
|
|
}
|
|
/**
|
|
* This functions copies the vales from a flat list of simple types
|
|
* to the matrix. The order has to be rowwise.
|
|
* @param atData the flat source array
|
|
*/
|
|
void setData (const T atData[SIZE*SIZE])
|
|
{
|
|
for (unsigned int row=0; row<SIZE; row++)
|
|
for (unsigned int column=0; column<SIZE; column++)
|
|
m_aatData[row][column] = atData[row+column*SIZE];
|
|
}
|
|
|
|
private:
|
|
/// the private container for the matrix cells
|
|
T m_aatData[SIZE][SIZE];
|
|
};
|
|
|
|
|
|
/**
|
|
* This function is used for the standard output stream and converts
|
|
* the Matrix to a string.
|
|
*
|
|
* @param output the outputstream which should be extended by the matrix print
|
|
* @param mat the matrix which should be printed to the output stream
|
|
* @return the changed output stream to be capable of something like "cout<<"test" << matrix;"
|
|
*/
|
|
template <class T, unsigned SIZE>
|
|
static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
|
|
{
|
|
output << "( ";
|
|
for(unsigned int j =0; j< SIZE; ++j)
|
|
{
|
|
output << "( ";
|
|
for(unsigned int i =0; i< SIZE; ++i)
|
|
{
|
|
output << mat(j,i);
|
|
if (i<4-1)
|
|
output << " , ";
|
|
else
|
|
output << " )";
|
|
}
|
|
if (j<SIZE-1)
|
|
output << " ) ; ";
|
|
else
|
|
output << " ) )";
|
|
}
|
|
return output;
|
|
}
|
|
|
|
/**
|
|
* This function creates a rotation matrix for a x-y-z rotation
|
|
*
|
|
* @param x rotation for the x axis in [ degree ]
|
|
* @param y rotation for the x axis in [ degree ]
|
|
* @param x rotation for the x axis in [ degree ]
|
|
* @return a homogeneous cartesian matrix
|
|
*/
|
|
static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
|
|
{
|
|
Mat<float,4> tempx,tempy,tempz;
|
|
|
|
// create x rotation
|
|
float temp_x[4][4] = { { 1, 0, 0, 0},
|
|
{ 0, cos(x_angle), -sin(x_angle), 0},
|
|
{ 0, sin(x_angle), cos(x_angle), 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
|
|
// create y rotation
|
|
float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0},
|
|
{ 0, 1, 0, 0},
|
|
{ sin(y_angle), 0, cos(y_angle), 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
|
|
// create z rotation
|
|
float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0},
|
|
{ sin(z_angle), cos(z_angle), 0, 0},
|
|
{ 0, 0, 1, 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
|
|
// combine all rotations to a matrix class
|
|
tempx=temp_x;
|
|
tempy=temp_y;
|
|
tempz=temp_z;
|
|
return tempz*tempy*tempx;
|
|
}
|
|
/**
|
|
* @}
|
|
*/
|
|
#endif
|