phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
70 Commits
1 Branch
0 Tags
31 MiB
 
 
 
 
 
 
Tree: 165281e612
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '165281e612'
${ noResults }
lwr_server/lwrserv/include/Vec.h

654 lines
17 KiB
Raw Blame History

#ifndef _VEC_H_
#define _VEC_H_
#include <math.h>
#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;
/**
* Vector 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 Vec
{
public:
/**
* standard constructor
*/
Vec<T,SIZE> ()
{
// initialize all elements with zero
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = T(0);
}
/**
* construction with data value
* Initialize the Vector completely to the given value
*
* @param tData initial value for the vector
*/
Vec<T, SIZE> (const T tData)
{
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = tData;
}
/**
* construction with data array
* Initialize the Vector completely for each field with array values
*
* @param tData array of initial values for the vector
*/
Vec<T, SIZE> (const T atData[SIZE])
{
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = atData[i];
}
/**
* copy construction from an other vector
* Initialize the Vector completely for each field like the given vector
*
* @param vec of initial values for the new vector
*/
Vec<T, SIZE> (const Vec<T, SIZE> &vec)
{
if (this==&vec)
return; // nothing to do, it's me
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = vec.m_atData[i];
}
/**
* destructor which has nothing
*/
~Vec ()
{
// nothing to do here ...
}
/**
* copy the values from the array to the Vector
*
* @param atData source array for the copy process
*/
void setData (const T atData[SIZE])
{
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = atData[i];
}
/**
* copy the values from the vector to the array
*
* @param atData destination array for the copy process
*/
void getData (T atData[SIZE])
{
for (unsigned int i=0; i<SIZE; i++)
atData[i] = m_atData[i];
}
/**
* returns the dimension of the vector
*
* @retval dimension size
*/
unsigned getDimension ()
{
return SIZE;
}
/**
* assignment operator between vectors
* also capable of multi assignments
* e.g:
*
* vec1 = vec2 = vec3;
* L-Value = R-Value/L-Value = R-Value
*
* @param vec source vector which should be copied
* @retval returns the current
*/
Vec<T, SIZE> &operator = (const Vec<T, SIZE> &vec)
{
/// check if the L and R values are the same
/// do nothing in this case
if (this==&vec)
return (*this);
/// not the same values so copy content data
for (unsigned int i=0; i<SIZE; i++)
m_atData[i] = vec.m_atData[i];
// also an R-value in e.g
// vec1 = vec2 = vec3;
// L-Value = R-Value/L-Value = R-Value
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
m_atData[i] = atData[i];
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 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)
i = SIZE-1; // !!! operator clips index ...
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)
i = SIZE-1;
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);
for (unsigned int i=0; i<SIZE; i++)
buf.m_atData[i] += vec.m_atData[i];
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];
}
/**
* 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);
for (unsigned int i=0; i<SIZE; i++)
buf.m_atData[i] -= vec.m_atData[i];
return buf;
}
/**
* operator to calculate the negative vector
* of the current one
*
* @return the negative vector
*/
Vec<T, SIZE> operator - ()
{
T atBuffer[SIZE];
for (int i=0; i<SIZE; i++)
atBuffer[i] = -m_atData[i];
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);
for (int i=0; i<SIZE; i++)
dp += m_atData[i]*vec.m_atData[i];
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;
for (unsigned int i=0; i<SIZE; i++)
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;
for (unsigned int i=0; i<SIZE; i++)
vec(i) = m_atData[i]/tScale;
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;
for (unsigned int j=0; j<SIZE; j++)
for (unsigned int i=0; i<SIZE; i++)
vec(j) += m_atData[i]*mat(i,j);
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;
for (int i=0; i<SIZE; i++)
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;
for (int i=0; i<SIZE; i++)
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;
for (int i=0; i<SIZE; i++)
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;
for (int i=0; i<SIZE; i++)
buff.m_atData[i] = std::max(m_atData[i],vec.m_atData[i]);
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;
for (int i=0; i<SIZE; i++)
buff.m_atData[i] = std::abs(m_atData[i]);
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;
for (int i=0; i<SIZE; i++)
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;
for (int i=0; i<SIZE; i++)
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;
for (int i=0; i<SIZE; i++)
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 )
return 0.0f;
if ( x > 0)
return 1.0f;
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;
for (int i=0; i<SIZE; i++)
buff.m_atData[i] = sgn(m_atData[i]);
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];
// this * this
for (int i=0; i<SIZE; i++)
if (retval < m_atData[i])
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];
// this * this
for (int i=0; i<SIZE; i++)
if (retval > m_atData[i])
retval = m_atData[i];
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;
// this * this
for (int i=0; i<SIZE; i++)
retval += m_atData[i] * m_atData[i];
sqrt(retval);
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];
temp[0] = m_atData[1]*vec(2) - m_atData[2]*vec(1);
temp[1] = m_atData[2]*vec(0) - m_atData[0]*vec(2);
temp[2] = m_atData[0]*vec(1) - m_atData[1]*vec(0);
temp[3] = vec(3);
return Vec<T, SIZE> (temp);
}
private:
T m_atData[SIZE];
}; // class Vec
// some common vector classes (abbr. names)
typedef Vec<float, 2> Vec2f;
typedef Vec<float, 3> Vec3f;
typedef Vec<float, 4> Vec4f;
typedef Vec<double, 2> Vec2d;
typedef Vec<double, 3> Vec3d;
typedef Vec<double, 4> Vec4d;
typedef Vec<int, 2> Vec2i;
typedef Vec<int, 3> Vec3i;
typedef Vec<int, 4> Vec4i;
typedef Vec<bool, 2> Vec2b;
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)
{
output << "( ";
for(unsigned int i = 0 ; i< SIZE; ++i)
{
output << vec(i);
if (i<SIZE-1)
output << " , ";
else
output << " )";
}
return output;
}
/**
* @}
*/
#endif