Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // RDL - Robot Dynamics Library
3 : : // Licensed under the zlib license. See LICENSE for more details.
4 : :
5 : : /**
6 : : * @file FramePoint.hpp
7 : : * @brief File containing the FramePoint<T> object definition
8 : : */
9 : :
10 : : #ifndef __RDL_FRAME_POINT_HPP__
11 : : #define __RDL_FRAME_POINT_HPP__
12 : :
13 : : #include "rdl_dynamics/FrameObject.hpp"
14 : : #include "rdl_dynamics/FrameVector.hpp"
15 : : #include "rdl_dynamics/Point3.hpp"
16 : : #include "rdl_dynamics/FrameExceptions.hpp"
17 : :
18 : : /**
19 : : * @page frame_point Frame Point
20 : : *
21 : : * A RobotDynamics::Math::FramePoint is a 3d point in Cartesian space with an associated reference frame
22 : : */
23 : :
24 : : namespace RobotDynamics
25 : : {
26 : : namespace Math
27 : : {
28 : : /**
29 : : * @class FramePoint
30 : : * @ingroup reference_frame
31 : : * @brief A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a
32 : : * FramePoint is expressed in, you may call the inhereted FrameObject::changeFrame method and supply it a
33 : : * pointer to the ReferenceFrame you wish to have the FramePoint expressed in. This class and its
34 : : * implementation are an adaptation of FramePoint.java by <a href="http://robots.ihmc.us/">Jerry Pratt and the IHMC
35 : : **Robotics Group</a>.
36 : : */
37 : : class FramePoint : public FrameObject, public Math::Point3d
38 : : {
39 : : public:
40 : : /**
41 : : * @brief Constructor
42 : : * @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
43 : : * @param x The x-component of the point
44 : : * @param y The y-component of the point
45 : : * @param z The z-component of the point
46 : : */
47 : 72 : FramePoint(ReferenceFramePtr referenceFrame, const double x, const double y, const double z) : FrameObject(referenceFrame), Math::Point3d(x, y, z)
48 : : {
49 : 72 : }
50 : :
51 : : /**
52 : : * @brief Constructor
53 : : * @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
54 : : * @param v A Vector3d that will be used to set the components of this FramePoint
55 : : */
56 : 25 : FramePoint(ReferenceFramePtr referenceFrame, Math::Vector3d v) : FrameObject(referenceFrame), Math::Point3d(v[0], v[1], v[2])
57 : : {
58 : 25 : }
59 : :
60 : : /**
61 : : * @brief Constructor
62 : : * @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
63 : : * @param point A Math::Point3 that will be used to set the components of this FramePoint
64 : : */
65 : : FramePoint(ReferenceFramePtr referenceFrame, const Math::Point3d& point) : FrameObject(referenceFrame), Math::Point3d(point)
66 : : {
67 : : }
68 : :
69 : : /**
70 : : * @brief Copy constructor
71 : : * @param framePoint A FramePoint to copy
72 : : */
73 : 9 : FramePoint(const FramePoint& framePoint) : FrameObject(framePoint.getReferenceFrame()), Math::Point3d(framePoint.x(), framePoint.y(), framePoint.z())
74 : : {
75 : 9 : }
76 : :
77 : : /**
78 : : * @brief Constructor that initializes to (x,y,z) = (0,0,0)
79 : : * @param referenceFrame A pointer to the ReferenceFrame the point will be expressed in
80 : : */
81 : : explicit FramePoint(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Math::Point3d()
82 : : {
83 : : }
84 : :
85 : : /**
86 : : * @brief Empty constructor that creates a point with ReferencFrame=nullptr and (x,y,z)=(0,0,0)
87 : : */
88 : 2291 : FramePoint() : FrameObject(nullptr), Math::Point3d()
89 : : {
90 : 2291 : }
91 : :
92 : 17 : FramePoint& operator=(const FramePoint& p)
93 : : {
94 : 17 : this->referenceFrame = p.getReferenceFrame();
95 : 17 : this->x() = p.x();
96 : 17 : this->y() = p.y();
97 : 17 : this->z() = p.z();
98 : 17 : return *this;
99 : : }
100 : :
101 : : /**
102 : : * @brief Destructor
103 : : */
104 : 2409 : ~FramePoint()
105 : 2397 : {
106 : 2409 : }
107 : :
108 : : /**
109 : : * @brief Get as point3d
110 : : * @return Point as Point3d type
111 : : */
112 : : inline Math::Point3d point() const
113 : : {
114 : : return Math::Point3d(x(), y(), z());
115 : : }
116 : :
117 : : /**
118 : : * @brief Return a pointer to this as base class type Math::TransformableGeometricObject. See
119 : : **FrameObject::changeFrame for how this method is used
120 : : * @return Pointer to this object as type Math::TransformableGeometricObject
121 : : */
122 : 922 : Math::TransformableGeometricObject* getTransformableGeometricObject()
123 : : {
124 : 922 : return this;
125 : : }
126 : :
127 : : /**
128 : : * @brief copy into new frame point and change the frame of that
129 : : * @param referenceFrame
130 : : * @return
131 : : */
132 : 2 : FramePoint changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
133 : : {
134 : 2 : FramePoint p = *this;
135 : 2 : p.changeFrame(referenceFrame);
136 : 2 : return p;
137 : 0 : }
138 : :
139 : : /**
140 : : * @brief Set both the ReferenceFrame this object is expressed in as well as the (x,y,z) coordinates of the point
141 : : * @param v Vector3d that this point will be set to
142 : : * @param referenceFrame Pointer to the ReferenceFrame this object will be expressed in
143 : : */
144 : 2584 : EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d& v, ReferenceFramePtr referenceFrame)
145 : : {
146 : 2584 : setIncludingFrame(v(0), v(1), v(2), referenceFrame);
147 : 2584 : }
148 : :
149 : : /**
150 : : * @brief Set both the ReferenceFrame the point is expressed in as well as the (x,y,z) coordinates
151 : : * @param x The x coordinate
152 : : * @param y The y coordinate
153 : : * @param z The z coordinate
154 : : * @param referenceFrame The ReferenceFrame this point is to be expressed in
155 : : */
156 : 2595 : void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
157 : : {
158 : 2595 : if (!referenceFrame)
159 : : {
160 : 1 : throw ReferenceFrameException("Reference frame is nullptr!");
161 : : }
162 : :
163 : 2594 : this->set(x, y, z);
164 : 2594 : this->referenceFrame = referenceFrame;
165 : 2594 : }
166 : :
167 : : /**
168 : : * @brief Set both the ReferenceFrame the point is expressed in as well as the (x,y,z) coordinates
169 : : * @param point Math::Point3d to set this point to
170 : : * @param referenceFrame Pointer to ReferenceFrame this point will be expressed in
171 : : */
172 : : void setIncludingFrame(const Math::Point3d& point, ReferenceFramePtr referenceFrame)
173 : : {
174 : : if (!referenceFrame)
175 : : {
176 : : throw ReferenceFrameException("Reference frame cannot be nullptr!");
177 : : }
178 : :
179 : : this->x() = point.x();
180 : : this->y() = point.y();
181 : : this->z() = point.z();
182 : : this->referenceFrame = referenceFrame;
183 : : }
184 : :
185 : : /**
186 : : * @brief Calculate the distance squared between two FramePoints. \f$\Delta_x^2+\Delta_y^2+\Delta_z^2\f$
187 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
188 : : * @param point FramePoint to calculate squared distance to
189 : : * @return Distance squared
190 : : */
191 : 2 : double distanceSquared(const FramePoint& point) const
192 : : {
193 : 2 : checkReferenceFramesMatch(&point);
194 : :
195 : 2 : double dx = this->x() - point.x();
196 : 2 : double dy = this->y() - point.y();
197 : 2 : double dz = this->z() - point.z();
198 : 2 : return dx * dx + dy * dy + dz * dz;
199 : : }
200 : :
201 : : /**
202 : : * @brief Calculate the 2D distance squared between two FramePoints
203 : : * @param point FramePoint to calculate squared distance to
204 : : * @param plane, 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
205 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
206 : : * @throws std::runtime_error if the plane argument is less than zero or greater than 2
207 : : * @return Distance squared
208 : : */
209 : 4 : double distance2DSquared(const FramePoint& point, int plane = 2) const
210 : : {
211 : 4 : checkReferenceFramesMatch(&point);
212 : 4 : if (plane < 0 || plane > 2)
213 : : {
214 : 1 : throw std::runtime_error("FramePoint.distance2DSquared: Plane argument must be either 0, 1, or 2");
215 : : }
216 : :
217 : 3 : double dx = plane == 0 ? 0. : this->x() - point.x();
218 : 3 : double dy = plane == 1 ? 0. : this->y() - point.y();
219 : 3 : double dz = plane == 2 ? 0. : this->z() - point.z();
220 : 3 : return dx * dx + dy * dy + dz * dz;
221 : : }
222 : :
223 : : /**
224 : : * @brief Calculate the distance squared from this point to a Point3d
225 : : * @param point Point3d to calculate squared distance to
226 : : * @param plane, 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
227 : : * @throws std::runtime_error if the plane argument is less than zero or greater than 2
228 : : * @return Distance squared
229 : : */
230 : 4 : double distance2DSquared(const Point3d& point, int plane = 2) const
231 : : {
232 : 4 : if (plane < 0 || plane > 2)
233 : : {
234 : 1 : throw std::runtime_error("FramePoint.distance2DSquared: Plane argument must be either 0, 1, or 2");
235 : : }
236 : :
237 : 3 : double dx = plane == 0 ? 0. : this->x() - point.x();
238 : 3 : double dy = plane == 1 ? 0. : this->y() - point.y();
239 : 3 : double dz = plane == 2 ? 0. : this->z() - point.z();
240 : 3 : return dx * dx + dy * dy + dz * dz;
241 : : }
242 : :
243 : : /**
244 : : * @brief Calculate the distance squared from this point to a Point3d
245 : : * @param point Point3d to calculate squared distance to
246 : : * @return Distance squared
247 : : */
248 : 1 : double distanceSquared(const Point3d& point) const
249 : : {
250 : 1 : double dx = this->x() - point.x();
251 : 1 : double dy = this->y() - point.y();
252 : 1 : double dz = this->z() - point.z();
253 : 1 : return dx * dx + dy * dy + dz * dz;
254 : : }
255 : :
256 : : /**
257 : : * @brief Calculate the distance between two FramePoints. \f$\sqrt{\Delta_x^2+\Delta_y^2+\Delta_z^2}\f$
258 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
259 : : * @param point FramePoint to calculate distance to
260 : : * @return Distance between points as template type T
261 : : */
262 : 4 : double distance2D(const FramePoint& point, int plane = 2) const
263 : : {
264 : 4 : checkReferenceFramesMatch(&point);
265 : :
266 : 4 : return sqrt(distance2DSquared(point, plane));
267 : : }
268 : :
269 : : /**
270 : : * @brief Calculate the distance between two FramePoints. \f$\sqrt{\Delta_x^2+\Delta_y^2+\Delta_z^2}\f$
271 : : * @param plane 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
272 : : * @param point FramePoint to calculate distance to
273 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
274 : : * @return Distance between points
275 : : */
276 : 4 : double distance(const FramePoint& point) const
277 : : {
278 : 4 : checkReferenceFramesMatch(&point);
279 : :
280 : 2 : return sqrt(distanceSquared(point));
281 : : }
282 : :
283 : : /**
284 : : * @brief Calculate the distance to a Point3d
285 : : * @param point Point3d to calculate distance to
286 : : * @param plane 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
287 : : * @return Distance between points
288 : : */
289 : 4 : double distance2D(const Point3d& point, int plane = 2) const
290 : : {
291 : 4 : return sqrt(distance2DSquared(point, plane));
292 : : }
293 : :
294 : : /**
295 : : * @brief Calculate the distance to a Point3d
296 : : * @param point Point3d to calculate distance to
297 : : * @return Distance between points
298 : : */
299 : 1 : double distance(const Point3d& point) const
300 : : {
301 : 1 : return sqrt(distanceSquared(point));
302 : : }
303 : :
304 : : /**
305 : : * @brief Calculate the L1 distance between two FramePoints by \f$|\Delta_x| + |\Delta_y| + |\Delta_z|\f$
306 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
307 : : * @param point FramePoint to calculate distance to
308 : : * @return Distance between points as template type T
309 : : */
310 : 6 : double distanceL1(const FramePoint& point) const
311 : : {
312 : 6 : checkReferenceFramesMatch(&point);
313 : :
314 : 2 : return fabs(this->x() - point.x()) + fabs(this->y() - point.y()) + fabs(this->z() - point.z());
315 : : }
316 : :
317 : : /**
318 : : * @brief Calculate the LInfinity distance between two FramePoints by \f$max(|\Delta_x|,|\Delta_y|,|\Delta_z|)\f$
319 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
320 : : * @param point FramePoint to calculate distance to
321 : : * @return Distance between points as template type T
322 : : */
323 : 2 : double distanceLinf(const FramePoint& point) const
324 : : {
325 : 2 : checkReferenceFramesMatch(&point);
326 : :
327 : 2 : double dx = this->x() - point.x();
328 : 2 : double dy = this->y() - point.y();
329 : 2 : double dz = this->z() - point.z();
330 : :
331 : 2 : double tmp = fabs(dx) > fabs(dy) ? fabs(dx) : fabs(dy);
332 : :
333 : 2 : return tmp > fabs(dz) ? tmp : fabs(dz);
334 : : }
335 : :
336 : : /**
337 : : * @brief Return true FramePoint argument is within epsilon of this, false otherwise
338 : : * @param point The FramePoint to be compared
339 : : * @param epsilon The tolerance of the comparison check
340 : : * @throws ReferenceFrameException If both points are not expressed in the same ReferenceFrame
341 : : *
342 : : * @deprecated prefer isApprox instead
343 : : */
344 : : [[deprecated("Use isApprox(FramePoint, double) instead.")]] bool epsilonEquals(const FramePoint& point, const double epsilon) const
345 : : {
346 : : checkReferenceFramesMatch(&point);
347 : : return std::abs(this->x() - point.x()) < epsilon && std::abs(this->y() - point.y()) < epsilon && std::abs(this->z() - point.z()) < epsilon;
348 : : }
349 : :
350 : : /**
351 : : * @brief Return true FramePoint argument is within epsilon of this and they have the same reference frame, false otherwise
352 : : * @param point The FramePoint to be compared
353 : : * @param epsilon The tolerance of the comparison check
354 : : */
355 : 3 : bool isApprox(const FramePoint& point, const double epsilon) const
356 : : {
357 : 8 : return referenceFrame == point.getReferenceFrame() && std::abs(this->x() - point.x()) < epsilon && std::abs(this->y() - point.y()) < epsilon &&
358 : 5 : std::abs(this->z() - point.z()) < epsilon;
359 : : }
360 : :
361 : : /**
362 : : * @brief Overloaded *= operator, performs this = this*scala
363 : : * @param scale Scalar to scale each element of this FramePoint by
364 : : * @return FramePoint representing this = this*scale
365 : : */
366 : : template <typename T>
367 : 1 : void operator*=(const T scale)
368 : : {
369 : 1 : this->x() *= scale;
370 : 1 : this->y() *= scale;
371 : 1 : this->z() *= scale;
372 : 1 : }
373 : :
374 : : /**
375 : : * @brief Overloaded /= operator, performs this = this*scale
376 : : * @param scale Scalar to divide each element of this FramePoint by
377 : : * @return FramePoint representing this = this/scale
378 : : */
379 : : template <typename T>
380 : : void operator/=(const T scale)
381 : : {
382 : : this->x() /= scale;
383 : : this->y() /= scale;
384 : : this->z() /= scale;
385 : : }
386 : :
387 : 1 : void operator+=(const FrameVector& v)
388 : : {
389 : 1 : checkReferenceFramesMatch(&v);
390 : 1 : this->x() += v.x();
391 : 1 : this->y() += v.y();
392 : 1 : this->z() += v.z();
393 : 1 : }
394 : :
395 : 1 : void operator-=(const FrameVector& v)
396 : : {
397 : 1 : checkReferenceFramesMatch(&v);
398 : 1 : this->x() -= v.x();
399 : 1 : this->y() -= v.y();
400 : 1 : this->z() -= v.z();
401 : 1 : }
402 : : };
403 : :
404 : 1 : inline FramePoint operator+(FramePoint p, const FrameVector& v)
405 : : {
406 : 1 : p += v;
407 : 1 : return p;
408 : : }
409 : :
410 : 1 : inline FramePoint operator-(FramePoint p, const FrameVector& v)
411 : : {
412 : 1 : p -= v;
413 : 1 : return p;
414 : : }
415 : :
416 : : template <typename T>
417 : : inline FrameVector operator*(const T scale, FramePoint p)
418 : : {
419 : : p *= scale;
420 : : return FrameVector(p.getReferenceFrame(), p.x(), p.y(), p.z());
421 : : }
422 : :
423 : : template <typename T>
424 : : inline FrameVector operator*(FramePoint p, const T scale)
425 : : {
426 : : p *= scale;
427 : : return FrameVector(p.getReferenceFrame(), p.x(), p.y(), p.z());
428 : : }
429 : :
430 : : /**
431 : : * @brief Check if two FramePoints are equal
432 : : * @param lhs
433 : : * @param rhs
434 : : * @throws ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame
435 : : * @return bool true if equal, false if not equal
436 : : */
437 : : inline bool operator==(const FramePoint& lhs, const FramePoint& rhs)
438 : : {
439 : : lhs.checkReferenceFramesMatch(&rhs);
440 : :
441 : : if (lhs.x() != rhs.x())
442 : : {
443 : : return false;
444 : : }
445 : :
446 : : if (lhs.y() != rhs.y())
447 : : {
448 : : return false;
449 : : }
450 : :
451 : : if (lhs.z() != rhs.z())
452 : : {
453 : : return false;
454 : : }
455 : :
456 : : return true;
457 : : }
458 : :
459 : : /**
460 : : * @brief Subtract two FramePoints and return result in newly created FramePoint
461 : : * @param p1
462 : : * @param p2
463 : : * @throws ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame
464 : : * @return A FramePoint that is the difference of the two argument FramePoints, i.e. p1-=p2
465 : : */
466 : 2 : inline FrameVector operator-(FramePoint p1, const FramePoint& p2)
467 : : {
468 : 4 : p1.getReferenceFrame()->checkReferenceFramesMatch(p2.getReferenceFrame());
469 : 2 : return FrameVector(p1.getReferenceFrame(), p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
470 : : }
471 : :
472 : : /**
473 : : * @brief Check if two FramePoints are not equal
474 : : * @param lhs
475 : : * @param rhs
476 : : * @throws ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame
477 : : * @return bool false if equal, true if not equal
478 : : */
479 : : inline bool operator!=(const FramePoint& lhs, const FramePoint& rhs)
480 : : {
481 : : return !operator==(lhs, rhs);
482 : : }
483 : :
484 : : inline std::ostream& operator<<(std::ostream& output, const FramePoint& framePoint)
485 : : {
486 : : output << "ReferenceFrame = " << framePoint.getReferenceFrame()->getName() << std::endl;
487 : : output << "x = " << framePoint.x() << " y = " << framePoint.y() << " z = " << framePoint.z() << std::endl;
488 : : return output;
489 : : }
490 : : typedef std::vector<FramePoint, Eigen::aligned_allocator<FramePoint>> FramePointV;
491 : : } // namespace Math
492 : : } // namespace RobotDynamics
493 : : #endif // ifndef __RDL_FRAME_POINT_HPP__
|
