phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

final version

master
philipp schoenberger 10 years ago
parent
1debece111
commit
165281e612
2 changed files with 380 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 142
      lwrserv/include/Mat.h
  2. 265
      lwrserv/include/Vec.h

142
lwrserv/include/Mat.h
View File

@ -48,7 +48,14 @@ typedef Mat<double, 4> Mat4d;
typedef Mat<float, 4> MatCarthesian;
// template square matrix class for SIMPLE data types
/**
* 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
{
@ -179,6 +186,7 @@ public:
* within the array
*
* @param scalar the value to add
* @return the new calculated matrix
*/
Mat<T, SIZE> operator + (const T &scalar)
{
@ -190,10 +198,11 @@ public:
}
/**
* operator to add a simple type to each entry
* operator to subtract a simple type to each entry
* within the array
*
* @param scalar the value to add
* @param scalar the value to subtract
* @return the new calculated matrix
*/
Mat<T, SIZE> operator - (const T &scalar)
{
@ -205,10 +214,11 @@ public:
}
/**
* operator to add a simple type to each entry
* operator to multiply a simple type to each entry
* within the array
*
* @param scalar the value to add
* @param scalar the value to multiply the matrix cells
* @return the new calculated matrix
*/
Mat<T, SIZE> operator * (const T &scalar)
{
@ -219,15 +229,28 @@ public:
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 i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf(i,j) += m_aatData[i][j] / scalar;
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;
@ -237,11 +260,21 @@ public:
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 i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
m_aatData[i][j] = aatData[j+(i*4)];
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
@ -249,6 +282,14 @@ public:
// " = " + (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;
@ -258,12 +299,17 @@ public:
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)
if (SIZE == 2)
return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
@ -273,17 +319,27 @@ public:
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 << ") ";
//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 << ") ";
//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;
@ -294,14 +350,31 @@ public:
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 i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf.m_aatData[i][j] = m_aatData[j][i];
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();
@ -316,12 +389,22 @@ public:
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++)
@ -330,10 +413,19 @@ public:
}
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)
{
@ -356,33 +448,47 @@ static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
}
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

265
lwrserv/include/Vec.h
View File

@ -4,6 +4,35 @@
#include <ostream>
#include <cmath>
#include "Mat.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;
@ -113,7 +142,7 @@ public:
/**
* assignment operator between vectors
* also capabile of muilti asignments
* also capable of multi assignments
* e.g:
*
* vec1 = vec2 = vec3;
@ -139,6 +168,17 @@ public:
return (*this);
}
/**
* assignment operator between simple types
* also capable of multi assignments
* e.g:
*
* vec1 = vec2 = vec3;
* L-Value = R-Value/L-Value = R-Value
*
* @param atData the source data array which should be copied
* @retval returns the current
*/
Vec<T, SIZE> &operator = (const T atData[SIZE])
{
for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
@ -150,10 +190,16 @@ public:
// " = " + (L-Val = R-Val)
}
// usage of operator:
// vec(i) = var; // 0 <= i <= SIZE-1
// var = vec(i);
// vec1(i) = vec2(j);
/**
* This functions requests the cell with a defined index
* usage of operator:
* vec(i) = var; // 0 <= i <= SIZE-1
* var = vec(i);
* vec1(i) = vec2(j);
*
* @param i the index of the vector
* @retval returns the cell with the provided index
*/
T &operator () (unsigned i)
{
if (i>=SIZE)
@ -161,6 +207,16 @@ public:
return m_atData[i]; // ... so we can safely return a reference
}
/**
* This functions requests the cell with a defined index
* usage of operator:
* vec(i) = var; // 0 <= i <= SIZE-1
* var = vec(i);
* vec1(i) = vec2(j);
*
* @param i the index of the vector
* @retval returns the cell with the provided index
*/
T operator () (unsigned i) const
{
if (i>=SIZE)
@ -168,12 +224,25 @@ public:
return m_atData[i];
}
/**
* operator to adds the vector by
* an other vector and reasign it.
*
* @param vec the vector to add to the current instance
*/
void operator += (const Vec<T, SIZE> &vec)
{
for (int i=0; i<SIZE; i++)
m_atData[i] += vec.m_atData[i];
}
/**
* operator to adds the vector by
* an other vector.
*
* @param vec the vector to add to the current instance
* @return the calculated vector
*/
Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec)
{
Vec<T, SIZE> buf (m_atData);
@ -182,14 +251,25 @@ public:
return buf;
}
/**
* operator to subtract the vector by
* an other vector.and reasign it
*
* @param vec the vector to subtract to the current instance
*/
void operator -= (const Vec<T, SIZE> &vec)
{
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] -= vec.m_atData[i];
}
// binary -
// vec1 - vec2 i.e. (*this) - vec
/**
* operator to subtract the vector by
* an other vector.
*
* @param vec the vector to subtract to the current instance
* @return the calculated vector
*/
Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec)
{
Vec<T, SIZE> buf (m_atData);
@ -198,8 +278,12 @@ public:
return buf;
}
// unary -
// -vec i.e. -(*this)
/**
* operator to calculate the negative vector
* of the current one
*
* @return the negative vector
*/
Vec<T, SIZE> operator - ()
{
T atBuffer[SIZE];
@ -208,6 +292,13 @@ public:
return Vec<T, SIZE> (atBuffer);
}
/**
* operator to multiply the vector by
* an other vector.
*
* @param vec the vector to multiply to the current instance
* @return the vector product
*/
T operator * (const Vec<T, SIZE> & vec)
{
T dp = T(0);
@ -216,12 +307,25 @@ public:
return dp;
}
/**
* operator to multiply the vector by the simple type
* provided and reasign it.
*
* @param tscale the simple type
*/
void operator *= (T tScale)
{
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] *= tScale;
}
/**
* operator to multiply the vector by the simple type
* provided
*
* @param tscale the simple type
* @return the new calculated matrix
*/
Vec<T, SIZE> operator * (T tScale)
{
Vec<T, SIZE> vec;
@ -229,6 +333,14 @@ public:
vec(i) = m_atData[i]*tScale;
return vec;
}
/**
* operator to divide the vector by the simple type
* provided
*
* @param tscale the simple type
* @return the new calculated matrix
*/
Vec<T, SIZE> operator / (T tScale)
{
Vec<T, SIZE> vec;
@ -237,6 +349,15 @@ public:
return vec;
}
/**
* This functions returns from the current and
* the provided vector the vector division
* for each cell separately
*
* @info the operator/ function has to be implemented for the template type
*
* @return the vector containing the cell wise divisor
*/
Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
{
Vec<T, SIZE> vec;
@ -246,6 +367,16 @@ public:
return vec;
}
/**
* This functions returns from the current and
* the provided vector the vector division
* for each cell separately
*
* @info the operator/ function has to be implemented for the template type
*
* @pram vec the vector to divide the current vector
* @return the vector containing the cell wise divisor
*/
Vec<T, SIZE> celldivide (const Vec<T, SIZE> & vec)
{
Vec<T, SIZE> buff;
@ -253,6 +384,17 @@ public:
buff.m_atData[i] = m_atData[i]/vec.m_atData[i];
return buff;
}
/**
* This functions returns from the current and
* the provided vector the vector multiplication
* for each cell separately
*
* @info the operator* function has to be implemented for the template type
*
* @pram vec the vector to multiply the current vector
* @return the vector containing the cell wise product
*/
Vec<T, SIZE> cellmultiply (const Vec<T, SIZE> & vec)
{
Vec<T, SIZE> buff;
@ -260,6 +402,15 @@ public:
buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
return buff;
}
/**
* This functions returns from the current and
* the provided vector the minimum for each cell
*
* @info the min function has to be implemented for the template type
*
* @return the vector containing the minimum cells in each dimension
*/
Vec<T, SIZE> cellmin(const Vec<T, SIZE> & vec)
{
Vec<T, SIZE> buff;
@ -267,6 +418,15 @@ public:
buff.m_atData[i] = std::min(m_atData[i],vec.m_atData[i]);
return buff;
}
/**
* This functions returns from the current and
* the provided vector the maximum for each cell
*
* @info the max function has to be implemented for the template type
*
* @return the vector containing the maximum cells in each dimension
*/
Vec<T, SIZE> cellmax(const Vec<T, SIZE> & vec)
{
Vec<T, SIZE> buff;
@ -275,6 +435,14 @@ public:
return buff;
}
/**
* This functions calculates the absolute value
* for each cell.
*
* @info the abs function has to be implemented for the template type
*
* @return the vector containing the absolute values
*/
Vec<T, SIZE> abs ()
{
Vec<T, SIZE> buff;
@ -283,6 +451,14 @@ public:
return buff;
}
/**
* This functions calculates the square root
* for each cell.
*
* @info the sqrt function has to be implemented for the template type
*
* @return the vector containing the square root values
*/
Vec<T, SIZE> sqrt()
{
Vec<T, SIZE> buff;
@ -290,6 +466,15 @@ public:
buff.m_atData[i] = std::sqrt(m_atData[i]);
return buff;
}
/**
* This functions calculates the floor
* for each cell.
*
* @info the floor function has to be implemented for the template type
*
* @return the vector containing the floor values
*/
Vec<T, SIZE> floor ()
{
Vec<T, SIZE> buff;
@ -297,6 +482,15 @@ public:
buff.m_atData[i] = floor(m_atData[i]);
return buff;
}
/**
* This functions calculates the ceil
* for each cell.
*
* @info the ceil function has to be implemented for the template type
*
* @return the vector containing the ceil values
*/
Vec<T, SIZE> ceil ()
{
Vec<T, SIZE> buff;
@ -304,6 +498,14 @@ public:
buff.m_atData[i] = std::ceil(m_atData[i]);
return buff;
}
/**
* This functions is here for the float simple type
* to calculate the sgn
*
* @param x the float value to check
* @return the sign of the provided float
*/
float sgn(float x)
{
if ( x == 0 )
@ -314,6 +516,14 @@ public:
else
return -1.0f;
}
/**
* This functions returns the vector containing the
* sign for each cell.
*
* @info the Template type has to provide the sgn function
* @return the vector with all signs
*/
Vec<T, SIZE> sgn ()
{
Vec<T, SIZE> buff;
@ -322,6 +532,12 @@ public:
return buff;
}
/**
* This functions returns the maximum cell of the vector.
*
* @info the Template type has to provide the operator>
* @return the maximum cell
*/
T max ()
{
T retval = m_atData[0];
@ -332,6 +548,13 @@ public:
retval = m_atData[i];
return retval;
}
/**
* This functions returns the minimum cell of the vector.
*
* @info the Template type has to provide the operator>
* @return the minimum cell
*/
T min ()
{
T retval = m_atData[0];
@ -343,6 +566,12 @@ public:
return retval;
}
/**
* This functions calculates the norm of the vector
*
* @info the Template type has to provide the sqrt function
* @return the normalisation factor
*/
T norm ()
{
T retval = 0;
@ -354,6 +583,13 @@ public:
return retval;
}
/**
* This functions calculates the cross product of 2 vectors.
*
* @param vec the 2th vector for the cross product
* @return the cross product
*/
Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
{
T temp[SIZE];
@ -389,6 +625,14 @@ typedef Vec<bool, 3> Vec3b;
typedef Vec<bool, 4> Vec4b;
/**
* This function is used for the standard output stream and converts
* the vector to a string.
*
* @param output the outputstream which should be extended by the vector
* @param vec the vector 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 Vec<T,SIZE> &vec)
{
@ -404,4 +648,7 @@ static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
return output;
}
/**
* @}
*/
#endif
Write Preview
Loading…
Cancel
Save