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 : : #ifndef __RDL_POINT_3_HPP__
6 : : #define __RDL_POINT_3_HPP__
7 : :
8 : : /**
9 : : * @file Point3.hpp
10 : : */
11 : :
12 : : #include <math.h>
13 : : #include <stdexcept>
14 : : #include <type_traits>
15 : : #include "rdl_dynamics/rdl_eigenmath.hpp"
16 : :
17 : : namespace RobotDynamics
18 : : {
19 : : namespace Math
20 : : {
21 : : /**
22 : : * @class Point3
23 : : * @brief A generic 3D point
24 : : */
25 : : class Point3d : public Math::TransformableGeometricObject
26 : : {
27 : : public:
28 : 16453 : Point3d(const double x, const double y, const double z)
29 : 16453 : {
30 : 16453 : point[0] = x;
31 : 16453 : point[1] = y;
32 : 16453 : point[2] = z;
33 : 16453 : }
34 : :
35 : 3021 : Point3d(const Point3d& point) : Point3d(point.x(), point.y(), point.z())
36 : : {
37 : 3021 : }
38 : :
39 : : // cppcheck-suppress noExplicitConstructor
40 : 2 : explicit Point3d(const Vector3d& vector) : Point3d(vector[0], vector[1], vector[2])
41 : : {
42 : 2 : }
43 : :
44 : 5294 : EIGEN_STRONG_INLINE Point3d() : Point3d(0., 0., 0.)
45 : : {
46 : 5294 : }
47 : :
48 : 16453 : virtual ~Point3d()
49 : 16453 : {
50 : 16453 : }
51 : :
52 : : /**
53 : : * @brief Performs in place point transform. Given a point, \f$p\f$, this performs \f$ p = -X.E X.r + X.E p \f$
54 : : * @param X
55 : : */
56 : 927 : inline void transform(const Math::SpatialTransform& X)
57 : : {
58 : 927 : set(-X.E * X.r + X.E * this->vec());
59 : 927 : }
60 : :
61 : 1 : inline Point3d transform_copy(const Math::SpatialTransform& X) const
62 : : {
63 : 1 : Point3d p = *this;
64 : 1 : p.transform(X);
65 : 1 : return p;
66 : 0 : }
67 : :
68 : : EIGEN_STRONG_INLINE void set(const std::vector<double>& vector)
69 : : {
70 : : set(vector[0], vector[1], vector[2]);
71 : : }
72 : :
73 : 2 : EIGEN_STRONG_INLINE void set(const Point3d& point)
74 : : {
75 : 2 : set(point.x(), point.y(), point.z());
76 : 2 : }
77 : :
78 : 927 : void set(const Math::Vector3d& v)
79 : : {
80 : 927 : set(v[0], v[1], v[2]);
81 : 927 : }
82 : :
83 : 3523 : void set(const double x, const double y, const double z)
84 : : {
85 : 3523 : point[0] = x;
86 : 3523 : point[1] = y;
87 : 3523 : point[2] = z;
88 : 3523 : }
89 : :
90 : : EIGEN_STRONG_INLINE void setToZero()
91 : : {
92 : : set(0., 0., 0.);
93 : : }
94 : :
95 : : /**
96 : : * Compares if two points are within epsilon of each other
97 : : * @param point
98 : : * @param epsilon
99 : : * @return true if they are within epsilon, false otherwise
100 : : */
101 : 2 : EIGEN_STRONG_INLINE bool epsilonEquals(const Point3d& point, const double epsilon) const
102 : : {
103 : 2 : return fabs(this->x() - point.x()) < epsilon && fabs(this->y() - point.y()) < epsilon && fabs(this->z() - point.z()) < epsilon;
104 : : }
105 : :
106 : : /**
107 : : * @brief clamp any values that are less than min to min
108 : : * @param min
109 : : */
110 : 9 : void clampMin(const double min)
111 : : {
112 : 9 : if (x() < min)
113 : : {
114 : 5 : x() = min;
115 : : }
116 : :
117 : 9 : if (y() < min)
118 : : {
119 : 5 : y() = min;
120 : : }
121 : :
122 : 9 : if (z() < min)
123 : : {
124 : 1 : z() = min;
125 : : }
126 : 9 : }
127 : :
128 : : /**
129 : : * @brief clamp any values that are greater than make to max
130 : : * @param max
131 : : */
132 : 8 : void clampMax(const double max)
133 : : {
134 : 8 : if (x() > max)
135 : : {
136 : 2 : x() = max;
137 : : }
138 : :
139 : 8 : if (y() > max)
140 : : {
141 : 2 : y() = max;
142 : : }
143 : :
144 : 8 : if (z() > max)
145 : : {
146 : 8 : z() = max;
147 : : }
148 : 8 : }
149 : :
150 : : /**
151 : : * @brief clamp any values greater than max to max, and any value less than min to min
152 : : * @param min
153 : : * @param max
154 : : */
155 : 6 : void clampMinMax(const double min, const double max)
156 : : {
157 : 6 : this->clampMin(min);
158 : 6 : this->clampMax(max);
159 : 6 : }
160 : :
161 : : /**
162 : : * @brief Set each element to the absolute value
163 : : */
164 : 2000 : void absoluteValue()
165 : : {
166 : 2000 : this->x() = fabs(this->x());
167 : 2000 : this->y() = fabs(this->y());
168 : 2000 : this->z() = fabs(this->z());
169 : 2000 : }
170 : :
171 : : /**
172 : : * @brief Square of the 2d distance between two points
173 : : * @param point
174 : : * @param plane, 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
175 : : * @return Distance squared
176 : : */
177 : 4 : double distance2DSquared(const Point3d& point, int plane = 2) const
178 : : {
179 : 4 : if (plane < 0 || plane > 2)
180 : : {
181 : 1 : throw std::runtime_error("Point3.distance2DSquared: Invalid plane arg. Must be 0, 1, or 2");
182 : : }
183 : :
184 : 5 : return plane == 2 ? std::pow(x() - point.x(), 2) + std::pow(y() - point.y(), 2) :
185 : 3 : plane == 1 ? std::pow(x() - point.x(), 2) + std::pow(z() - point.z(), 2) :
186 : 4 : std::pow(y() - point.y(), 2) + std::pow(z() - point.z(), 2);
187 : : }
188 : :
189 : : /**
190 : : * @brief Square of the distance between two points, \f$ x^2 + y^2 + z^2 \f$
191 : : * @param point
192 : : * @return Distance squared
193 : : */
194 : 2 : double distanceSquared(const Point3d& point) const
195 : : {
196 : 2 : return std::pow(x() - point.x(), 2) + std::pow(y() - point.y(), 2) + std::pow(z() - point.z(), 2);
197 : : }
198 : :
199 : : /**
200 : : * brief 2D Distance between two points
201 : : * @param point
202 : : * @param plane, 2 = z, 1 = y, 0 = x. Defaults to 2 which is the z = 0 plane
203 : : * @return Distance
204 : : */
205 : 4 : double distance2D(const Point3d& point, int plane = 2) const
206 : : {
207 : 4 : return std::sqrt(distance2DSquared(point, plane));
208 : : }
209 : :
210 : : /**
211 : : * brief Distance between two points, \f$ \sqrt{x^2 + y^2 + z^2} \f$
212 : : * @param point
213 : : * @return Distance
214 : : */
215 : 1 : double distance(const Point3d& point) const
216 : : {
217 : 1 : return std::sqrt(distanceSquared(point));
218 : : }
219 : :
220 : : /**
221 : : * @brief L1 norm of two points
222 : : * @param point
223 : : * @return L1 norm
224 : : */
225 : 2000 : double distanceL1(const Point3d& point) const
226 : : {
227 : 2000 : return fabs(x() - point.x()) + fabs(y() - point.y()) + fabs(z() - point.z());
228 : : }
229 : :
230 : : /**
231 : : * @brief Cross product between a point and vector
232 : : * @param v
233 : : * @return
234 : : */
235 : 43 : Vector3d cross(const Vector3d& v)
236 : : {
237 : 86 : return Vector3d(point[1] * v[2] - point[2] * v[1], point[2] * v[0] - point[0] * v[2], point[0] * v[1] - point[1] * v[0]);
238 : : }
239 : :
240 : : /**
241 : : * L-infinity norm
242 : : * @param point
243 : : * @return
244 : : */
245 : 2000 : double distanceLinf(const Point3d& point) const
246 : : {
247 : 2000 : double dx = x() - point.x();
248 : 2000 : double dy = y() - point.y();
249 : 2000 : double dz = z() - point.z();
250 : :
251 : 2000 : double tmp = fabs(dx) > fabs(dy) ? fabs(dx) : fabs(dy);
252 : :
253 : 2000 : return tmp > fabs(dz) ? tmp : fabs(dz);
254 : : }
255 : :
256 : 16102 : EIGEN_STRONG_INLINE double& x()
257 : : {
258 : 16102 : return point[0];
259 : : }
260 : :
261 : 11096 : EIGEN_STRONG_INLINE double x() const
262 : : {
263 : 11096 : return point[0];
264 : : }
265 : :
266 : 16099 : EIGEN_STRONG_INLINE double& y()
267 : : {
268 : 16099 : return point[1];
269 : : }
270 : :
271 : 11093 : EIGEN_STRONG_INLINE double y() const
272 : : {
273 : 11093 : return point[1];
274 : : }
275 : :
276 : 16100 : EIGEN_STRONG_INLINE double& z()
277 : : {
278 : 16100 : return point[2];
279 : : }
280 : :
281 : 11093 : EIGEN_STRONG_INLINE double z() const
282 : : {
283 : 11093 : return point[2];
284 : : }
285 : :
286 : 1 : EIGEN_STRONG_INLINE double* data()
287 : : {
288 : 1 : return point;
289 : : }
290 : :
291 : 3567 : EIGEN_STRONG_INLINE Math::Vector3d vec() const
292 : : {
293 : 3567 : return Math::Vector3d(point[0], point[1], point[2]);
294 : : }
295 : :
296 : 1 : Point3d& operator=(const Point3d& other)
297 : : {
298 : 1 : set(other);
299 : 1 : return *this;
300 : : }
301 : :
302 : : template <typename T>
303 : 1000 : void operator*=(const T scale)
304 : : {
305 : : static_assert(std::is_pod<T>::value, "T must be POD");
306 : 1000 : this->x() *= scale;
307 : 1000 : this->y() *= scale;
308 : 1000 : this->z() *= scale;
309 : 1000 : }
310 : :
311 : : template <typename T>
312 : : void operator/=(const T scale)
313 : : {
314 : : static_assert(std::is_pod<T>::value, "T must be POD");
315 : : this->x() /= scale;
316 : : this->y() /= scale;
317 : : this->z() /= scale;
318 : : }
319 : :
320 : 5 : bool operator==(const Point3d& rhs)
321 : : {
322 : 5 : if ((this->x() != rhs.x()) || (this->y() != rhs.y()) || (this->z() != rhs.z()))
323 : : {
324 : 1 : return false;
325 : : }
326 : :
327 : 4 : return true;
328 : : }
329 : :
330 : 2 : bool operator!=(const Point3d& rhs)
331 : : {
332 : 2 : return !this->operator==(rhs);
333 : : }
334 : :
335 : 1 : void operator+=(const Vector3d& v)
336 : : {
337 : 1 : point[0] += v[0];
338 : 1 : point[1] += v[1];
339 : 1 : point[2] += v[2];
340 : 1 : }
341 : :
342 : 1 : void operator-=(const Vector3d& v)
343 : : {
344 : 1 : point[0] -= v[0];
345 : 1 : point[1] -= v[1];
346 : 1 : point[2] -= v[2];
347 : 1 : }
348 : :
349 : : protected:
350 : : double point[3];
351 : : };
352 : :
353 : 1 : inline Point3d operator+(Point3d p, const Vector3d& v)
354 : : {
355 : 1 : p += v;
356 : 1 : return p;
357 : : }
358 : :
359 : 1 : inline Point3d operator-(Point3d p, const Vector3d& v)
360 : : {
361 : 1 : p -= v;
362 : 1 : return p;
363 : : }
364 : :
365 : : template <typename T>
366 : : inline Point3d operator*(Point3d p, const T scale)
367 : : {
368 : : static_assert(std::is_pod<T>::value, "T must be POD");
369 : : p *= scale;
370 : : return p;
371 : : }
372 : :
373 : : template <typename T>
374 : : inline Point3d operator*(const T scale, Point3d p)
375 : : {
376 : : static_assert(std::is_pod<T>::value, "T must be POD");
377 : : p *= scale;
378 : : return p;
379 : : }
380 : :
381 : 1 : inline Vector3d operator-(Point3d p1, const Point3d& p2)
382 : : {
383 : 2 : return Vector3d(p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
384 : : }
385 : :
386 : : inline std::ostream& operator<<(std::ostream& os, const Point3d& point)
387 : : {
388 : : os << "x: " << point.x() << '\n' << "y: " << point.y() << '\n' << "z: " << point.z() << "\n";
389 : : return os;
390 : : }
391 : :
392 : : static inline Matrix3d toTildeForm(const Point3d& p)
393 : : {
394 : : return Matrix3d(0., -p.z(), p.y(), p.z(), 0., -p.x(), -p.y(), p.x(), 0.);
395 : : }
396 : : typedef std::vector<Point3d, Eigen::aligned_allocator<Point3d>> Point3V;
397 : : } // namespace Math
398 : : } // namespace RobotDynamics
399 : : #endif // ifndef __RDL_POINT_3_HPP__
|
