cartesian_point.hpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Ifdefs
12 *****************************************************************************/
13 
14 #ifndef ECL_GEOMETRY_CARTESIAN_POINT_HPP_
15 #define ECL_GEOMETRY_CARTESIAN_POINT_HPP_
16 
17 /*****************************************************************************
18 ** Includes
19 *****************************************************************************/
20 
21 #include <ecl/config/macros.hpp>
22 #include <ecl/exceptions/standard_exception.hpp>
23 #include <ecl/linear_algebra.hpp>
24 #include <ecl/math.hpp>
25 
26 /*****************************************************************************
27 ** Namespaces
28 *****************************************************************************/
29 
30 namespace ecl {
31 
32 /*****************************************************************************
33 ** Interface [CartesianPoint<T,N>]
34 *****************************************************************************/
45 template <typename T, unsigned int N>
46 class ECL_PUBLIC CartesianPoint {
47 public:
51  CartesianPoint(const T &value = T()) : elements(ecl::linear_algebra::Matrix<T,N,1>::Constant(value)) {}
57  CartesianPoint(const ecl::linear_algebra::Matrix<T,N,1> &point_vector) : elements(point_vector) {}
58 
59  /*********************
60  ** Assignment
61  **********************/
75  ecl::linear_algebra::CommaInitializer< ecl::linear_algebra::Matrix<T,N,1> > operator<<(const T &value) {
76  return elements.operator<<(value);
77  }
78 
86  T& operator [](const unsigned int& index) ecl_assert_throw_decl(StandardException) {
87  ecl_assert_throw( index<N, StandardException(LOC,OutOfRangeError));
88  return elements[index];
89  }
90 
98  const T& operator [](const unsigned int& index) const ecl_assert_throw_decl(StandardException) {
99  ecl_assert_throw( index<N, StandardException(LOC,OutOfRangeError));
100  return elements[index];
101  }
102 
103 
104 
105 
113  ecl::linear_algebra::Matrix<T,N,1>& positionVector() { return elements; }
121  const ecl::linear_algebra::Matrix<T,N,1>& positionVector() const { return elements; }
122 
123  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 
125 private:
126  ecl::linear_algebra::Matrix<T,N,1> elements;
127 };
128 
129 /*****************************************************************************
130 ** Interface [CartesianPoint<T,3>]
131 *****************************************************************************/
143 template<typename T>
144 class ECL_PUBLIC CartesianPoint<T,3> {
145 public:
149  CartesianPoint(const T &value = T()) : elements(ecl::linear_algebra::Matrix<T,3,1>::Constant(value)) {}
155  CartesianPoint(const ecl::linear_algebra::Matrix<T,3,1> &vec) : elements(vec) {}
163  CartesianPoint(const T &x_i, const T &y_i, const T &z_i) {
164  elements << x_i, y_i, z_i;
165  }
166 
167  virtual ~CartesianPoint() {}
168 
169  /*********************
170  ** Assignment
171  **********************/
185  ecl::linear_algebra::CommaInitializer< ecl::linear_algebra::Matrix<T,3,1> > operator<<(const T &value) {
186  return elements.operator<<(value);
187  }
188 
196  ecl::linear_algebra::Matrix<T,3,1>& positionVector() { return elements; }
197 
205  const ecl::linear_algebra::Matrix<T,3,1>& positionVector() const { return elements; }
206 
214  unsigned int size() { return 3; }
215 
221  const T& x() const { return elements[0]; }
227  const T& y() const { return elements[1]; }
233  const T& z() const { return elements[2]; }
239  T x() { return elements[0]; }
245  T y() { return elements[1]; }
251  T z() { return elements[2]; }
252 
253  void x(const T& value) { elements[0] = value; }
254  void y(const T& value) { elements[1] = value; }
255  void z(const T& value) { elements[2] = value; }
257  template <typename OutputStream, typename Type>
258  friend OutputStream& operator<<(OutputStream &ostream , const CartesianPoint<Type,3> &point);
259 
260 private:
261  ecl::linear_algebra::Matrix<T,3,1> elements;
262 };
263 
268 /*********************
269 ** Streaming
270 **********************/
278 template <typename OutputStream, typename Type>
279 OutputStream& operator<<(OutputStream &ostream , const CartesianPoint<Type,3> &point) {
280  ostream << "[ " << point.x() << " " << point.y() << " " << point.z() << " ]";
281  return ostream;
282 }
283 
284 /*****************************************************************************
285 ** Interface [CartesianPoint<T,2>]
286 *****************************************************************************/
298 template<typename T>
299 class ECL_PUBLIC CartesianPoint<T,2> {
300 public:
304  CartesianPoint(const T &value = T()) : elements(ecl::linear_algebra::Matrix<T,2,1>::Constant(value)) {}
310  CartesianPoint(const ecl::linear_algebra::Matrix<T,2,1> &vec) : elements(vec) {}
316  CartesianPoint(const CartesianPoint<T,2> &point ) : elements( point.positionVector() ) {}
317 
318  CartesianPoint(const T &x_i, const T &y_i) {
319  elements << x_i, y_i;
320  }
321 
322  virtual ~CartesianPoint() {}
323 
324  /*********************
325  ** Assignment
326  **********************/
340  ecl::linear_algebra::CommaInitializer< ecl::linear_algebra::Matrix<T,2,1> > operator<<(const T &value) {
341  return elements.operator<<(value);
342  }
343 
344  T& operator [](const unsigned int& index) ecl_assert_throw_decl(StandardException) {
346  return elements[index];
347  }
348 
349  const T operator [](const unsigned int& index) const ecl_assert_throw_decl(StandardException) {
351  return elements[index];
352  }
353 
354  const CartesianPoint<T,2> & operator = (const CartesianPoint<T,2> &v)
355  {
356  elements = v.positionVector();
357  return *this;
358  }
366  ecl::linear_algebra::Matrix<T,2,1>& positionVector() { return elements; }
367 
375  const ecl::linear_algebra::Matrix<T,2,1>& positionVector() const { return elements; }
376 
384  unsigned int size() { return 2; }
385 
391  const T& x() const { return elements[0]; }
397  const T& y() const { return elements[1]; }
403  T x() { return elements[0]; }
409  T y() { return elements[1]; }
410 
411  bool operator == (const CartesianPoint<T,2> & other ) const
412  {
413  if( other.x() != x() || other.y() != y() ) return false;
414  else return true;
415  }
416 
417  bool operator != (const CartesianPoint<T,2> & other ) const
418  {
419  if( other.x() != x() || other.y() != y() ) return true;
420  else return false;
421  }
422 
423  void rotate( const double & angle )
424  {
425  double c = cos(angle);
426  double s = sin(angle);
427  double tx = x()*c - y()*s;
428  double ty = x()*s + y()*c;
429 
430  elements[0] = tx;
431  elements[1] = ty;
432  }
433 
434  double get_angle(void)
435  {
436  return atan2( y(), x() );
437  }
438 
439  void operator += ( const CartesianPoint<T,2> & other )
440  {
441  elements += other.positionVector();
442  }
443 
444  T distance ( const CartesianPoint<T,2> & other )
445  {
446  ecl::linear_algebra::Matrix<T,2,1> temp_element = elements - other.positionVector();
447  return static_cast<T>( sqrt( temp_element[0]*temp_element[0]+temp_element[1]*temp_element[1]) );
448  }
449 
450  T Normalize(void)
451  {
452  T mag = ecl::EuclideanNorm()( x(), y() );
453 // T mag = static_cast<T>(sqrt( x()*x()+y()*y() ));
454  if ( mag == static_cast<T>(0) ) // make a unit vector in z-direction
455  {
456  elements[0] = 1.0;
457  elements[1] = 0.0;
458  } else
459  {
460  elements[0] *= (1.0/mag);
461  elements[1] *= (1.0/mag);
462  }
463  return mag;
464  }
465 
466  CartesianPoint<T,2> operator * (double d) const
467  {
468  return CartesianPoint<T,2>( elements*d );
469  }
470 
471  CartesianPoint<T,2> operator + (const CartesianPoint<T,2> &v) const
472  {
473  return CartesianPoint<T,2>( x()+v.x(), y()+v.y() );
474  }
475 
476  CartesianPoint<T,2> operator - (const CartesianPoint<T,2> &v) const
477  {
478  return CartesianPoint<T,2>( x()-v.x(), y()-v.y() );
479  }
480 
481  double Norm(void)
482  {
483  return static_cast<double>( ecl::EuclideanNorm()( x(), y() ) );
484 // return (sqrt( x()*x()+y()*y() ));
485  }
486 
487  void x(const T& value) { elements[0] = value; }
488  void y(const T& value) { elements[1] = value; }
490  template <typename OutputStream, typename Type>
491  friend OutputStream& operator<<(OutputStream &ostream , const CartesianPoint<Type,2> &point);
492 
493  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
494 
495 private:
496  ecl::linear_algebra::Matrix<T,2,1> elements;
497 };
498 
503 /*********************
504 ** Streaming
505 **********************/
513 template <typename OutputStream, typename Type>
514 OutputStream& operator<<(OutputStream &ostream , const CartesianPoint<Type,2> &point) {
515  ostream << "[ " << point.x() << " " << point.y() << " ]";
516  return ostream;
517 }
518 
519 } // namespace ecl
520 
521 #endif /* ECL_GEOMETRY_CARTESIAN_POINT_HPP_ */
const T & y() const
CartesianPoint< float, 3 > CartesianPoint3f
ecl::linear_algebra::Matrix< T, 2, 1 > & positionVector()
CartesianPoint< double, 2 > CartesianPoint2d
Specialisation for a cartesian point of dimension 2.
#define LOC
Stringify the line of code you are at.
CartesianPoint< double, 3 > CartesianPoint3d
OutputStream & operator<<(OutputStream &ostream, const Void void_object)
Output stream operator for Void objects.
Definition: void.hpp:76
#define ecl_assert_throw(expression, exception)
Debug mode throw with a logical condition check.
#define ECL_PUBLIC
Standard exception type, provides code location and error string.
OutOfRangeError
const T & x() const
CartesianPoint< int, 3 > CartesianPoint3i
Functor for euclidean norms.
Definition: norms.hpp:39
void d()
ecl_geometry_PUBLIC int size(const Trajectory2D &trajectory)
#define ecl_assert_throw_decl(exception)
Assure throw exception declaration.
Generic container storing a cartesian point of dimension N.
CartesianPoint< float, 2 > CartesianPoint2f
CartesianPoint< int, 2 > CartesianPoint2i
ecl_geometry_PUBLIC double distance(const Pose2D &pose, const Trajectory2D &trajectory)


xbot_driver
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:27:37