Point3D.cpp
Go to the documentation of this file.
1 //
2 // Point3D.cpp
3 // A point in the 3-D cartesian plane. All data is stored in 64-bit-"double"-precision.
4 //
5 
6 #define _USE_MATH_DEFINES // for MSVC
7 #include <cmath>
8 #include "Point2D.hpp"
9 #include "Point3D.hpp"
10 
11 namespace datatypes
12 {
13 
14 //
15 // Some of the non-trivial member functions
16 //
17 
18 //
19 // Constructs a point from the given ScanPoint
20 //
22  : m_x(double(p.getX()))
23  , m_y(double(p.getY()))
24  , m_z(0.0)
25 {
27 }
28 
29 
35 bool Point3D::isZero() const
36 {
37  return (fuzzyCompare(m_x, 0.0) &&
38  fuzzyCompare(m_y, 0.0) &&
39  fuzzyCompare(m_z, 0.0));
40 }
41 
42 
43 double Point3D::dist( const Point3D& point ) const
44 {
45  return hypot(m_x - point.getX(), m_y - point.getY(), m_z - point.getZ());
46 }
47 
49 {
50  Point2D result(m_x, m_y);
51  return result;
52 }
53 
59 {
60  Point3D kreuz;
61 
62  kreuz.m_x = + ((v1.getY() * v2.getZ()) - (v1.getZ() * v2.getY()));
63  kreuz.m_y = -((v1.getX() * v2.getZ()) - (v1.getZ() * v2.getX()));
64  kreuz.m_z = + ((v1.getX() * v2.getY()) - (v1.getY() * v2.getX()));
65 
66  return kreuz;
67 }
68 
75 {
76  if (isZero())
77  // Vector has zero length. Such a vector will be left unchanged.
78  return;
79 
80  double len = length();
81  // To be really sure about avoiding division-by-zero, we check again here
82  if (fuzzyCompare(len, 0.0))
83  {
84  // Vector has zero length. Such a vector will be left unchanged.
85  return;
86  }
87 
88  *this /= len;
89 }
90 
91 
96 void Point3D::rotateAroundZ(double yawAngle)
97 {
98  double dCos = cos(yawAngle);
99  double dSin = sin(yawAngle);
100 
101  double x = m_x * dCos - m_y * dSin;
102  double y = m_x * dSin + m_y * dCos;
103 
104  m_x = x;
105  m_y = y;
106 }
107 
108 
113 void Point3D::rotateAroundX(double rollAngle)
114 {
115  double dCos = cos(rollAngle);
116  double dSin = sin(rollAngle);
117 
118  double y = -m_z * dSin + m_y * dCos;
119  double z = m_z * dCos + m_y * dSin;
120 
121  m_z = z;
122  m_y = y;
123 }
124 
129 void Point3D::rotateAroundY(double pitchAngle)
130 {
131  double dCos = cos(pitchAngle);
132  double dSin = sin(pitchAngle);
133 
134  double x = m_z * dSin + m_x * dCos;
135  double z = m_z * dCos - m_x * dSin;
136 
137  m_z = z;
138  m_x = x;
139 }
140 
141 
147 {
148  return hypot(m_x, m_y, m_z);
149 }
150 
155 double Point3D::length() const
156 {
157  return distFromOrigin();
158 }
159 
160 
180 {
181  double angle;
182 
183  // atan2(y,x) returns the angle against the x-axis
184  angle = std::atan2(m_y, m_x);
185 
186  return angle;
187 }
188 
208 {
209  double angle;
210 
211  // atan2(y,x) returns the angle against the x-axis
212  angle = std::atan2(m_x, m_z);
213 
214  return angle;
215 }
216 
217 
236 {
237  double angle;
238 
239  // atan2(z,y) returns the angle against the y-axis
240  angle = std::atan2(m_z, m_y);
241 
242  return angle;
243 }
244 
245 
246 
250 double Point3D::getDistanceBetweenPoints(const Point3D& pt1, const Point3D& pt2)
251 {
252  return pt1.dist(pt2);
253 }
254 
255 
256 
257 
258 
262 std::string Point3D::toString() const
263 {
264 // std::string text;
265  std::ostringstream ostr;
266  ostr << *this;
267  return ostr.str();
268 }
269 
273 std::ostream& operator<<(std::ostream& os, const Point3D& point)
274 {
275  os << "(" << point.getX() << ", " << point.getY() << ", " << point.getZ() << ")";
276  return os;
277 }
278 
279 
294  const Point3D& PlaneNormal,
295  const Point3D& VectorStartpoint,
296  const Point3D& VectorDirection)
297 {
298  Point3D intersectionPoint;
299  double nenner;
300 
301  nenner = PlaneNormal * VectorDirection;
302  if (fuzzyCompare(nenner, 0.0) == false)
303  {
304  // Calc intersection point
305  intersectionPoint = VectorStartpoint + ((PlaneNormal * (PlaneStartpoint - VectorStartpoint)) / nenner) * VectorDirection;
306  }
307  else
308  {
309  // Vector points "along" the plane, so there is no intersection point.
310  intersectionPoint.setXYZ(NaN_double, NaN_double, NaN_double);
311  }
312 
313  return intersectionPoint;
314 }
315 
316 } // namespace datatypes
void setXYZ(double x, double y, double z)
Sets the coordinates of this point to the given values.
Definition: Point3D.hpp:84
const double NaN_double
Not-a-Number in double precision.
Definition: MathToolbox.cpp:13
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
double getY() const
Returns the y-coordinate of this point.
Definition: Point3D.hpp:64
std::string toString() const
Text output for debugging.
Definition: Point3D.cpp:262
static Point3D calcIntersectionPointOfVectorWithPlane(const Point3D &PlaneStartpoint, const Point3D &PlaneNormal, const Point3D &VectorStartpoint, const Point3D &VectorDirection)
Definition: Point3D.cpp:293
double getAngleAroundZ() const
Returns the rotation angle around z (in the x-y-plane)
Definition: Point3D.cpp:179
void normalize()
Normalizes this vector (point) to length 1.0.
Definition: Point3D.cpp:74
Point2D toPoint2D() const
Returns the x/y components of this class, converted into a Point2D object.
Definition: Point3D.cpp:48
double dist(const Point3D &point) const
Calculates the distance to the given point.
Definition: Point3D.cpp:43
void rotateAroundZ(double dYawAngle)
Rotate the point around the Z-axis ("Yaw angle")
Definition: Point3D.cpp:96
double hypot(double x, double y, double z)
Definition: MathToolbox.cpp:19
double distFromOrigin() const
Dist from the point (0,0,0) to this point.
Definition: Point3D.cpp:146
bool isZero() const
Check against (near-)zero.
Definition: Point3D.cpp:35
static Point3D vectorProduct(const Point3D &v1, const Point3D &v2)
Returns the vector product ("Kreuzprodukt") of the two vectors.
Definition: Point3D.cpp:58
double getZ() const
Returns the z-coordinate of this point.
Definition: Point3D.hpp:65
double length() const
Length of the vector (identical to distFromOrigin())
Definition: Point3D.cpp:155
double getAngleAroundY() const
Returns the rotation angle around y (in the z-x-plane)
Definition: Point3D.cpp:207
double getX() const
Returns the x-coordinate of this point.
Definition: Point3D.hpp:63
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
void rotateAroundX(double rollAngle)
Rotate the point around the X-axis ("Roll angle")
Definition: Point3D.cpp:113
double getAngleAroundX() const
Returns the rotation angle around x (in the z-y-plane)
Definition: Point3D.cpp:235
static double getDistanceBetweenPoints(const Point3D &pt1, const Point3D &pt2)
Returns the distance between the two point coordinates.
Definition: Point3D.cpp:250
std::ostream & operator<<(std::ostream &os, const EvalCaseResult &result)
void rotateAroundY(double pitchAngle)
Rotate the point around the Y-axis ("Pitch angle")
Definition: Point3D.cpp:129


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33