Main MRPT website > C++ reference for MRPT 1.5.7
CPoint3D.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPoint3D.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPoint2D.h>
15 #include <mrpt/poses/CPose2D.h>
16 #include <mrpt/utils/CStream.h>
17 #include <limits>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 using namespace mrpt::utils;
23 
25 
26 /** Constructor from an CPoint2D object. */ // Here instead of in the .h to avoid headers include loops.
27 CPoint3D::CPoint3D( const CPoint2D &p) { m_coords[0]=p.x(); m_coords[1]=p.y(); m_coords[2]=0; }
28 /** Constructor from an CPose2D object. */
29 CPoint3D::CPoint3D( const CPose2D &p) { m_coords[0]=p.x(); m_coords[1]=p.y(); m_coords[2]=0; }
30 
31 /** Constructor from an CPose3D object. */
32 CPoint3D::CPoint3D( const CPose3D &p) { m_coords[0]=p.x(); m_coords[1]=p.y(); m_coords[2]=p.z(); }
33 
34 
35 /*---------------------------------------------------------------
36  Implements the writing to a CStream capability of
37  CSerializable objects
38  ---------------------------------------------------------------*/
40 {
41  if (version)
42  *version = 1;
43  else
44  {
45  // The coordinates:
46  out << m_coords[0] << m_coords[1] << m_coords[2];
47  }
48 }
49 
50 /*---------------------------------------------------------------
51  Implements the reading from a CStream capability of
52  CSerializable objects
53  ---------------------------------------------------------------*/
55 {
56  switch(version)
57  {
58  case 0:
59  {
60  float f;
61  in >> f; m_coords[0]=f;
62  in >> f; m_coords[1]=f;
63  in >> f; m_coords[2]=f;
64  } break;
65  case 1:
66  {
67  // The coordinates:
68  in >> m_coords[0] >> m_coords[1] >> m_coords[2];
69  } break;
70  default:
72  };
73 }
74 
75 /*---------------------------------------------------------------
76  point3D = point3D - pose3D
77  ---------------------------------------------------------------*/
79 {
80  // JLBC: 7-FEB-2008: Why computing the whole matrix multiplication?? ;-)
81  // 5.7us -> 4.1us -> 3.1us (with optimization of HM matrices by reference)
82  // JLBC: 10-APR-2009: Usage of fixed-size 4x4 matrix, should be even faster now.
84  b.getInverseHomogeneousMatrix( B_INV );
85 
86  return CPoint3D(
87  B_INV.get_unsafe(0,0) * m_coords[0] + B_INV.get_unsafe(0,1) * m_coords[1] + B_INV.get_unsafe(0,2) * m_coords[2] + B_INV.get_unsafe(0,3),
88  B_INV.get_unsafe(1,0) * m_coords[0] + B_INV.get_unsafe(1,1) * m_coords[1] + B_INV.get_unsafe(1,2) * m_coords[2] + B_INV.get_unsafe(1,3),
89  B_INV.get_unsafe(2,0) * m_coords[0] + B_INV.get_unsafe(2,1) * m_coords[1] + B_INV.get_unsafe(2,2) * m_coords[2] + B_INV.get_unsafe(2,3) );
90 }
91 
92 /*---------------------------------------------------------------
93  point3D = point3D - point3D
94  ---------------------------------------------------------------*/
96 {
97  return CPoint3D( m_coords[0]-b.m_coords[0], m_coords[1]-b.m_coords[1], m_coords[2]-b.m_coords[2] );
98 }
99 
100 
101 /*---------------------------------------------------------------
102  point3D = point3D + point3D
103  ---------------------------------------------------------------*/
105 {
106  return CPoint3D( m_coords[0]+b.m_coords[0], m_coords[1]+b.m_coords[1], m_coords[2]+b.m_coords[2] );
107 }
108 
109 /*---------------------------------------------------------------
110  pose3D = point3D + pose3D
111  ---------------------------------------------------------------*/
113 {
114  return CPose3D( m_coords[0]+b.x(), m_coords[1]+b.y(),m_coords[2]+b.z(), b.yaw(), b.pitch(), b.roll() );
115 }
116 
118 {
119  for (int i=0;i<3;i++)
120  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
121 }
122 
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
Definition: CPoint3D.cpp:117
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CPoint3D.cpp:54
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLubyte GLubyte b
Definition: glext.h:5575
int version
Definition: mrpt_jpeglib.h:898
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CPoint3D.cpp:39
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
CPoint3D operator-(const CPose3D &b) const
Returns this point as seen from "b", i.e.
Definition: CPoint3D.cpp:78
CPoint3D(const double x=0, const double y=0, const double z=0)
Constructor for initializing point coordinates.
Definition: CPoint3D.h:42
GLuint in
Definition: glext.h:6301
CPoint3D operator+(const CPoint3D &b) const
Returns this point plus point "b", i.e.
Definition: CPoint3D.cpp:104
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019