Main MRPT website > C++ reference for MRPT 1.4.0
geometry.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, 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#ifndef GEO_H
10#define GEO_H
11
14#include <mrpt/poses/CPose3D.h>
16
17#include <mrpt/math/math_frwds.h> // forward declarations
18#include <mrpt/math/wrap2pi.h>
19
20/*---------------------------------------------------------------
21 Class
22 ---------------------------------------------------------------*/
23namespace mrpt
24{
25 namespace math
26 {
27 using std::vector;
28
29 /** \addtogroup geometry_grp Geometry: lines, planes, intersections, SLERP, "lightweight" point & pose classes
30 * \ingroup mrpt_base_grp
31 * @{ */
32
33 /**
34 * Global epsilon to overcome small precision errors
35 */
36 extern double BASE_IMPEXP geometryEpsilon;
37
38 /**
39 * Slightly heavyweight type to speed-up calculations with polygons in 3D
40 * \sa TPolygon3D,TPlane
41 */
43 public:
44 /**
45 * Actual polygon.
46 */
48 /**
49 * Plane containing the polygon.
50 */
52 /**
53 * Plane's pose.
54 * \sa inversePose
55 */
57 /**
58 * Plane's inverse pose.
59 * \sa pose
60 */
62 /**
63 * Polygon, after being projected to the plane using inversePose.
64 * \sa inversePose
65 */
67 /**
68 * Constructor. Takes a polygon and computes each parameter.
69 */
71 /**
72 * Basic constructor. Needed to create containers.
73 * \sa TPolygonWithPlane(const TPolygon3D &)
74 */
76 /**
77 * Static method for vectors. Takes a set of polygons and creates every TPolygonWithPlane
78 */
79 static void getPlanes(const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys);
80 };
81
82 /** @name Simple intersection operations, relying basically on geometrical operations.
83 @{
84 */
85 /** Gets the intersection between two 3D segments. Possible outcomes:
86 * - Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
87 * - Segments don't intersect & are parallel: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment "in between" both segments.
88 * - Segments don't intersect & aren't parallel: Return=false.
89 * \sa TObject3D
90 */
91 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TSegment3D &s2,TObject3D &obj);
92
93 /** Gets the intersection between a 3D segment and a plane. Possible outcomes:
94 * - Don't intersect: Return=false
95 * - s1 is within the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
96 * - s1 intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
97 * \sa TObject3D
98 */
99 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TPlane &p2,TObject3D &obj);
100
101 /** Gets the intersection between a 3D segment and a 3D line. Possible outcomes:
102 * - They don't intersect : Return=false
103 * - s1 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
104 * - s1 intersects the line at a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
105 * \sa TObject3D
106 */
107 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TLine3D &r2,TObject3D &obj);
108
109 /** Gets the intersection between a plane and a 3D segment. Possible outcomes:
110 * - Don't intersect: Return=false
111 * - s2 is within the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
112 * - s2 intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
113 * \sa TObject3D
114 */
115 inline bool intersect(const TPlane &p1,const TSegment3D &s2,TObject3D &obj) {
116 return intersect(s2,p1,obj);
117 }
118
119 /** Gets the intersection between two planes. Possible outcomes:
120 * - Planes are parallel: Return=false
121 * - Planes intersect into a line: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE
122 * \sa TObject3D
123 */
124 bool BASE_IMPEXP intersect(const TPlane &p1,const TPlane &p2,TObject3D &obj);
125
126 /** Gets the intersection between a plane and a 3D line. Possible outcomes:
127 * - Line is parallel to plane but not within it: Return=false
128 * - Line is contained in the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE
129 * - Line intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
130 * \sa TObject3D
131 */
132 bool BASE_IMPEXP intersect(const TPlane &p1,const TLine3D &p2,TObject3D &obj);
133
134 /** Gets the intersection between a 3D line and a 3D segment. Possible outcomes:
135 * - They don't intersect : Return=false
136 * - s2 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
137 * - s2 intersects the line at a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
138 * \sa TObject3D
139 */
140 inline bool intersect(const TLine3D &r1,const TSegment3D &s2,TObject3D &obj) {
141 return intersect(s2,r1,obj);
142 }
143
144 /** Gets the intersection between a 3D line and a plane. Possible outcomes:
145 * - Line is parallel to plane but not within it: Return=false
146 * - Line is contained in the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE
147 * - Line intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
148 * \sa TObject3D
149 */
150 inline bool intersect(const TLine3D &r1,const TPlane &p2,TObject3D &obj) {
151 return intersect(p2,r1,obj);
152 }
153
154 /** Gets the intersection between two 3D lines. Possible outcomes:
155 * - Lines do not intersect: Return=false
156 * - Lines are parallel and do not coincide: Return=false
157 * - Lines coincide (are the same): Return=true, obj.getType()=GEOMETRIC_TYPE_LINE
158 * - Lines intesect in a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
159 * \sa TObject3D
160 */
161 bool BASE_IMPEXP intersect(const TLine3D &r1,const TLine3D &r2,TObject3D &obj);
162
163 /** Gets the intersection between two 2D lines. Possible outcomes:
164 * - Lines do not intersect: Return=false
165 * - Lines are parallel and do not coincide: Return=false
166 * - Lines coincide (are the same): Return=true, obj.getType()=GEOMETRIC_TYPE_LINE
167 * - Lines intesect in a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
168 * \sa TObject2D
169 */
170 bool BASE_IMPEXP intersect(const TLine2D &r1,const TLine2D &r2,TObject2D &obj);
171
172 /** Gets the intersection between a 2D line and a 2D segment. Possible outcomes:
173 * - They don't intersect: Return=false
174 * - s2 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
175 * - Both intersects in one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
176 * \sa TObject2D
177 */
178 bool BASE_IMPEXP intersect(const TLine2D &r1,const TSegment2D &s2,TObject2D &obj);
179
180 /** Gets the intersection between a 2D line and a 2D segment. Possible outcomes:
181 * - They don't intersect: Return=false
182 * - s1 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT
183 * - Both intersects in one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
184 * \sa TObject2D
185 */
186 inline bool intersect(const TSegment2D &s1,const TLine2D &r2,TObject2D &obj) {
187 return intersect(r2,s1,obj);
188 }
189
190 /** Gets the intersection between two 2D segments. Possible outcomes:
191 * - Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
192 * - Segments don't intersect & are parallel: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment "in between" both segments.
193 * - Segments don't intersect & aren't parallel: Return=false.
194 * \sa TObject2D
195 */
196 bool BASE_IMPEXP intersect(const TSegment2D &s1,const TSegment2D &s2,TObject2D &obj);
197
198 /** @}
199 */
200
201 /** @name Angle retrieval methods. Methods which use TSegments will automatically use TLines' implicit constructors.
202 @{
203 */
204 /**
205 * Computes the angle between two planes.
206 */
207 double BASE_IMPEXP getAngle(const TPlane &p1,const TPlane &p2);
208 /**
209 * Computes the angle between a plane and a 3D line or segment (implicit constructor will be used if passing a segment instead of a line).
210 */
211 double BASE_IMPEXP getAngle(const TPlane &p1,const TLine3D &r2);
212 /**
213 * Computes the angle between a 3D line or segment and a plane (implicit constructor will be used if passing a segment instead of a line).
214 */
215 inline double getAngle(const TLine3D &r1,const TPlane &p2) {
216 return getAngle(p2,r1);
217 }
218 /**
219 * Computes the angle between two 3D lines or segments (implicit constructor will be used if passing a segment instead of a line).
220 */
221 double BASE_IMPEXP getAngle(const TLine3D &r1,const TLine3D &r2);
222 /**
223 * Computes the angle between two 2D lines or segments (implicit constructor will be used if passing a segment instead of a line).
224 */
225 double BASE_IMPEXP getAngle(const TLine2D &r1,const TLine2D &r2);
226 /** @}
227 */
228
229 /** @name Creation of lines from poses.
230 @{
231 */
232 /**
233 * Gets a 3D line corresponding to the X axis in a given pose. An implicit constructor is used if a TPose3D is given.
234 * \sa createFromPoseY,createFromPoseZ,createFromPoseAndVector
235 */
237 /**
238 * Gets a 3D line corresponding to the Y axis in a given pose. An implicit constructor is used if a TPose3D is given.
239 * \sa createFromPoseX,createFromPoseZ,createFromPoseAndVector
240 */
242 /**
243 * Gets a 3D line corresponding to the Z axis in a given pose. An implicit constructor is used if a TPose3D is given.
244 * \sa createFromPoseX,createFromPoseY,createFromPoseAndVector
245 */
247 /**
248 * Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose. An implicit constructor is used if a TPose3D is given.
249 * \sa createFromPoseX,createFromPoseY,createFromPoseZ
250 */
251 void BASE_IMPEXP createFromPoseAndVector(const mrpt::poses::CPose3D &p,const double (&vector)[3],TLine3D &r);
252 /**
253 * Gets a 2D line corresponding to the X axis in a given pose. An implicit constructor is used if a CPose2D is given.
254 * \sa createFromPoseY,createFromPoseAndVector
255 */
257 /**
258 * Gets a 2D line corresponding to the Y axis in a given pose. An implicit constructor is used if a CPose2D is given.
259 * \sa createFromPoseX,createFromPoseAndVector
260 */
262 /**
263 * Gets a 2D line corresponding to any arbitrary vector, in the base given the given pose. An implicit constructor is used if a CPose2D is given.
264 * \sa createFromPoseY,createFromPoseAndVector
265 */
266 void BASE_IMPEXP createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r);
267 /** @}
268 */
269
270 /** @name Other line or plane related methods.
271 @{
272 */
273 /**
274 * Checks whether this polygon or set of points acceptably fits a plane.
275 * \sa TPolygon3D,geometryEpsilon
276 */
277 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points);
278 /**
279 * Checks whether this polygon or set of points acceptably fits a plane, and if it's the case returns it in the second argument.
280 * \sa TPolygon3D,geometryEpsilon
281 */
282 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points,TPlane &p);
283 /**
284 * Checks whether this set of points acceptably fits a 2D line.
285 * \sa geometryEpsilon
286 */
287 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points);
288 /**
289 * Checks whether this set of points acceptably fits a 2D line, and if it's the case returns it in the second argument.
290 * \sa geometryEpsilon
291 */
292 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points,TLine2D &r);
293 /**
294 * Checks whether this set of points acceptably fits a 3D line.
295 * \sa geometryEpsilon
296 */
297 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points);
298 /**
299 * Checks whether this set of points acceptably fits a 3D line, and if it's the case returns it in the second argument.
300 */
301 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points,TLine3D &r);
302 /** @}
303 */
304
305 /** @name Projections
306 @{
307 */
308 /**
309 * Uses the given pose 3D to project a point into a new base.
310 */
311 inline void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose,TPoint3D &newPoint) {
312 newXYpose.composePoint(point.x,point.y,point.z,newPoint.x,newPoint.y,newPoint.z);
313 }
314 /**
315 * Uses the given pose 3D to project a segment into a new base.
316 */
317 inline void project3D(const TSegment3D &segment,const mrpt::poses::CPose3D &newXYpose,TSegment3D &newSegment) {
318 project3D(segment.point1,newXYpose,newSegment.point1);
319 project3D(segment.point2,newXYpose,newSegment.point2);
320 }
321 /**
322 * Uses the given pose 3D to project a line into a new base.
323 */
324 void BASE_IMPEXP project3D(const TLine3D &line,const mrpt::poses::CPose3D &newXYpose,TLine3D &newLine);
325 /**
326 * Uses the given pose 3D to project a plane into a new base.
327 */
328 void BASE_IMPEXP project3D(const TPlane &plane,const mrpt::poses::CPose3D &newXYpose,TPlane &newPlane);
329 /**
330 * Uses the given pose 3D to project a polygon into a new base.
331 */
332 void BASE_IMPEXP project3D(const TPolygon3D &polygon,const mrpt::poses::CPose3D &newXYpose,TPolygon3D &newPolygon);
333 /**
334 * Uses the given pose 3D to project any 3D object into a new base.
335 */
336 void BASE_IMPEXP project3D(const TObject3D &object,const mrpt::poses::CPose3D &newXYPose,TObject3D &newObject);
337
338 /**
339 * Projects any 3D object into the plane's base, using its inverse pose. If the object is exactly inside the plane, this projection will zero its Z coordinates.
340 */
341 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,T &newObj) {
343 TPlane(newXYPlane).getAsPose3D(pose);
344 project3D(obj,-pose,newObj);
345 }
346
347 /**
348 * Projects any 3D object into the plane's base, using its inverse pose and forcing the position of the new coordinates origin. If the object is exactly inside the plane, this projection will zero its Z coordinates.
349 */
350 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,const TPoint3D &newOrigin,T &newObj) {
352 //TPlane(newXYPlane).getAsPose3DForcingOrigin(newOrigin,pose);
353 TPlane(newXYPlane).getAsPose3D(pose);
354 project3D(obj,-pose,newObj);
355 }
356
357 /**
358 * Projects a set of 3D objects into the plane's base.
359 */
360 template<class T> void project3D(const std::vector<T> &objs,const mrpt::poses::CPose3D &newXYpose,std::vector<T> &newObjs) {
361 size_t N=objs.size();
362 newObjs.resize(N);
363 for (size_t i=0;i<N;i++) project3D(objs[i],newXYpose,newObjs[i]);
364 }
365
366 /**
367 * Uses the given pose 2D to project a point into a new base.
368 */
369 void BASE_IMPEXP project2D(const TPoint2D &point,const mrpt::poses::CPose2D &newXpose,TPoint2D &newPoint);
370
371 /**
372 * Uses the given pose 2D to project a segment into a new base.
373 */
374 inline void project2D(const TSegment2D &segment,const mrpt::poses::CPose2D &newXpose,TSegment2D &newSegment) {
375 project2D(segment.point1,newXpose,newSegment.point1);
376 project2D(segment.point2,newXpose,newSegment.point2);
377 }
378
379 /**
380 * Uses the given pose 2D to project a line into a new base.
381 */
382 void BASE_IMPEXP project2D(const TLine2D &line,const mrpt::poses::CPose2D &newXpose,TLine2D &newLine);
383 /**
384 * Uses the given pose 2D to project a polygon into a new base.
385 */
386 void BASE_IMPEXP project2D(const TPolygon2D &polygon,const mrpt::poses::CPose2D &newXpose,TPolygon2D &newPolygon);
387 /**
388 * Uses the given pose 2D to project any 2D object into a new base.
389 */
390 void BASE_IMPEXP project2D(const TObject2D &object,const mrpt::poses::CPose2D &newXpose,TObject2D &newObject);
391
392 /**
393 * Projects any 2D object into the line's base, using its inverse pose. If the object is exactly inside the line, this projection will zero its Y coordinate.
394 * \tparam CPOSE2D set to mrpt::poses::CPose2D
395 */
396 template<class T,class CPOSE2D> void project2D(const T &obj,const TLine2D &newXLine,T &newObj) {
397 CPOSE2D pose;
398 newXLine.getAsPose2D(pose);
399 project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
400 }
401
402 /**
403 * Projects any 2D object into the line's base, using its inverse pose and forcing the position of the new coordinate origin. If the object is exactly inside the line, this projection will zero its Y coordinate.
404 * \tparam CPOSE2D set to mrpt::poses::CPose2D
405 */
406 template<class T,class CPOSE2D> void project2D(const T &obj,const TLine2D &newXLine,const TPoint2D &newOrigin,T &newObj) {
407 CPOSE2D pose;
408 newXLine.getAsPose2DForcingOrigin(newOrigin,pose);
409 project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
410 }
411
412 /**
413 * Projects a set of 2D objects into the line's base.
414 */
415 template<class T> void project2D(const std::vector<T> &objs,const mrpt::poses::CPose2D &newXpose,std::vector<T> &newObjs) {
416 size_t N=objs.size();
417 newObjs.resize(N);
418 for (size_t i=0;i<N;i++) project2D(objs[i],newXpose,newObjs[i]);
419 }
420 /** @}
421 */
422
423 /** @name Polygon intersections. These operations rely more on spatial reasoning than in raw numerical operations.
424 @{
425 */
426 /**
427 * Gets the intersection between a 2D polygon and a 2D segment.
428 * \sa TObject2D
429 */
430 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj);
431 /**
432 * Gets the intersection between a 2D polygon and a 2D line.
433 * \sa TObject2D
434 */
435 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj);
436 /**
437 * Gets the intersection between two 2D polygons.
438 * \sa TObject2D
439 */
440 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TPolygon2D &p2,TObject2D &obj);
441 /**
442 * Gets the intersection between a 2D segment and a 2D polygon.
443 * \sa TObject2D
444 */
445 inline bool intersect(const TSegment2D &s1,const TPolygon2D &p2,TObject2D &obj) {
446 return intersect(p2,s1,obj);
447 }
448 /**
449 * Gets the intersection between a 2D line and a 2D polygon.
450 * \sa TObject2D
451 */
452 inline bool intersect(const TLine2D &r1,const TPolygon2D &p2,TObject2D &obj) {
453 return intersect(p2,r1,obj);
454 }
455 /**
456 * Gets the intersection between a 3D polygon and a 3D segment.
457 * \sa TObject3D
458 */
459 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj);
460 /**
461 * Gets the intersection between a 3D polygon and a 3D line.
462 * \sa TObject3D
463 */
464 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj);
465 /**
466 * Gets the intersection between a 3D polygon and a plane.
467 * \sa TObject3D
468 */
469 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj);
470 /**
471 * Gets the intersection between two 3D polygons.
472 * \sa TObject3D
473 */
474 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj);
475 /**
476 * Gets the intersection between a 3D segment and a 3D polygon.
477 * \sa TObject3D
478 */
479 inline bool intersect(const TSegment3D &s1,const TPolygon3D &p2,TObject3D &obj) {
480 return intersect(p2,s1,obj);
481 }
482 /**
483 * Gets the intersection between a 3D line and a 3D polygon.
484 * \sa TObject3D
485 */
486 inline bool intersect(const TLine3D &r1,const TPolygon3D &p2,TObject3D &obj) {
487 return intersect(p2,r1,obj);
488 }
489 /**
490 * Gets the intersection between a plane and a 3D polygon.
491 * \sa TObject3D
492 */
493 inline bool intersect(const TPlane &p1,const TPolygon3D &p2,TObject3D &obj) {
494 return intersect(p2,p1,obj);
495 }
496
497 /**
498 * Gets the intersection between two sets of 3D polygons. The intersection is returned as an sparse matrix with each pair of polygons' intersections, and the return value is the amount of intersections found.
499 * \sa TObject3D,CSparseMatrixTemplate
500 */
501 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs);
502 /**
503 * Gets the intersection between two sets of 3D polygons. The intersection is returned as a vector with every intersection found, and the return value is the amount of intersections found.
504 * \sa TObject3D
505 */
506 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
507 /** @}
508 */
509
510 /** @name Other intersections
511 @{
512 */
513 /**
514 * Gets the intersection between vectors of geometric objects and returns it in a sparse matrix of either TObject2D or TObject3D.
515 * \sa TObject2D,TObject3D,CSparseMatrix
516 */
517 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,CSparseMatrixTemplate<O> &objs) {
518 size_t M=v1.size(),N=v2.size();
519 O obj;
520 objs.clear();
521 objs.resize(M,N);
522 for (size_t i=0;i<M;i++) for (size_t j=0;j<M;j++) if (intersect(v1[i],v2[j],obj)) objs(i,j)=obj;
523 return objs.getNonNullElements();
524 }
525
526 /**
527 * Gets the intersection between vectors of geometric objects and returns it in a vector of either TObject2D or TObject3D.
528 * \sa TObject2D,TObject3D
529 */
530 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,std::vector<O> objs) {
531 objs.resize(0);
532 O obj;
533 for (typename std::vector<T>::const_iterator it1=v1.begin();it1!=v1.end();++it1) {
534 const T &elem1=*it1;
535 for (typename std::vector<U>::const_iterator it2=v2.begin();it2!=v2.end();++it2) if (intersect(elem1,*it2,obj)) objs.push_back(obj);
536 }
537 return objs.size();
538 }
539
540 /**
541 * Gets the intersection between any pair of 2D objects.
542 */
543 bool BASE_IMPEXP intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj);
544 /**
545 * Gets the intersection between any pair of 3D objects.
546 */
547 bool BASE_IMPEXP intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj);
548 /** @}
549 */
550
551 /** @name Distances
552 @{
553 */
554 /**
555 * Gets the distance between two points in a 2D space.
556 */
557 double BASE_IMPEXP distance(const TPoint2D &p1,const TPoint2D &p2);
558 /**
559 * Gets the distance between two points in a 3D space.
560 */
561 double BASE_IMPEXP distance(const TPoint3D &p1,const TPoint3D &p2);
562 /**
563 * Gets the distance between two lines in a 2D space.
564 */
565 double BASE_IMPEXP distance(const TLine2D &r1,const TLine2D &r2);
566 /**
567 * Gets the distance between two lines in a 3D space.
568 */
569 double BASE_IMPEXP distance(const TLine3D &r1,const TLine3D &r2);
570 /**
571 * Gets the distance between two planes. It will be zero if the planes are not parallel.
572 */
573 double BASE_IMPEXP distance(const TPlane &p1,const TPlane &p2);
574
575 /**
576 * Gets the distance between two polygons in a 2D space.
577 */
578 double BASE_IMPEXP distance(const TPolygon2D &p1,const TPolygon2D &p2);
579 /**
580 * Gets the distance between a polygon and a segment in a 2D space.
581 */
582 double BASE_IMPEXP distance(const TPolygon2D &p1,const TSegment2D &s2);
583 /**
584 * Gets the distance between a segment and a polygon in a 2D space.
585 */
586 inline double distance(const TSegment2D &s1,const TPolygon2D &p2) {
587 return distance(p2,s1);
588 }
589 /**
590 * Gets the distance between a polygon and a line in a 2D space.
591 */
592 double BASE_IMPEXP distance(const TPolygon2D &p1,const TLine2D &l2);
593 inline double distance(const TLine2D &l1,const TPolygon2D &p2) {
594 return distance(p2,l1);
595 }
596 /**
597 * Gets the distance between two polygons in a 3D space.
598 */
599 double BASE_IMPEXP distance(const TPolygon3D &p1,const TPolygon3D &p2);
600 /**
601 * Gets the distance between a polygon and a segment in a 3D space.
602 */
603 double BASE_IMPEXP distance(const TPolygon3D &p1,const TSegment3D &s2);
604 /**
605 * Gets the distance between a segment and a polygon in a 3D space.
606 */
607 inline double distance(const TSegment3D &s1,const TPolygon3D &p2) {
608 return distance(p2,s1);
609 }
610 /**
611 * Gets the distance between a polygon and a line in a 3D space.
612 */
613 double BASE_IMPEXP distance(const TPolygon3D &p1,const TLine3D &l2);
614 /**
615 * Gets the distance between a line and a polygon in a 3D space.
616 */
617 inline double distance(const TLine3D &l1,const TPolygon3D &p2) {
618 return distance(p2,l1);
619 }
620 /**
621 * Gets the distance between a polygon and a plane.
622 */
623 double BASE_IMPEXP distance(const TPolygon3D &po,const TPlane &pl);
624 /**
625 * Gets the distance between a plane and a polygon.
626 */
627 inline double distance(const TPlane &pl,const TPolygon3D &po) {
628 return distance(po,pl);
629 }
630 /** @}
631 */
632
633 /** @name Bound checkers
634 @{
635 */
636 /**
637 * Gets the rectangular bounds of a 2D polygon or set of 2D points.
638 */
639 void BASE_IMPEXP getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax);
640 /**
641 * Gets the prism bounds of a 3D polygon or set of 3D points.
642 */
643 void BASE_IMPEXP getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax);
644 /** @}
645 */
646
647 /** @name Creation of planes from poses
648 @{
649 */
650 /**
651 * Given a pose, creates a plane orthogonal to its Z vector.
652 * \sa createPlaneFromPoseXZ,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
653 */
655 /**
656 * Given a pose, creates a plane orthogonal to its Y vector.
657 * \sa createPlaneFromPoseXY,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
658 */
660 /**
661 * Given a pose, creates a plane orthogonal to its X vector.
662 * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseAndNormal
663 */
665 /**
666 * Given a pose and any vector, creates a plane orthogonal to that vector in the pose's coordinates.
667 * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseYZ
668 */
669 void BASE_IMPEXP createPlaneFromPoseAndNormal(const mrpt::poses::CPose3D &pose,const double (&normal)[3],TPlane &plane);
670 /**
671 * Creates a rotation matrix so that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the vector.
672 */
673 void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double (&vec)[3],char coord,CMatrixDouble &matrix);
674 /** @}
675 */
676
677 /** @name Linear regression methods
678 @{
679 */
680 /**
681 * Using eigenvalues, gets the best fitting line for a set of 2D points. Returns an estimation of the error.
682 * \sa spline, leastSquareLinearFit
683 */
684 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint2D> &points,TLine2D &line);
685 /**
686 * Using eigenvalues, gets the best fitting line for a set of 3D points. Returns an estimation of the error.
687 * \sa spline, leastSquareLinearFit
688 */
689 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint3D> &points,TLine3D &line);
690 /**
691 * Using eigenvalues, gets the best fitting plane for a set of 3D points. Returns an estimation of the error.
692 * \sa spline, leastSquareLinearFit
693 */
694 double BASE_IMPEXP getRegressionPlane(const std::vector<TPoint3D> &points,TPlane &plane);
695 /** @}
696 */
697
698 /** @name Miscellaneous Geometry methods
699 @{
700 */
701 /**
702 * Tries to assemble a set of segments into a set of closed polygons.
703 */
704 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys);
705 /**
706 * Tries to assemble a set of segments into a set of closed polygons, returning the unused segments as another out parameter.
707 */
708 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
709 /**
710 * Extracts all the polygons, including those formed from segments, from the set of objects.
711 */
712 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
713 /**
714 * Extracts all the polygons, including those formed from segments, from the set of objects.
715 */
716 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
717 /**
718 * Extracts all the polygons, including those formed from segments, from the set of objects.
719 */
720 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
721
722 /**
723 * Changes the value of the geometric epsilon.
724 * \sa geometryEpsilon,getEpsilon
725 */
726 inline void setEpsilon(double nE) {
728 }
729 /**
730 * Gets the value of the geometric epsilon.
731 * \sa geometryEpsilon,setEpsilon
732 */
733 inline double getEpsilon() {
734 return geometryEpsilon;
735 }
736
737 /**
738 * Splits a 2D polygon into convex components.
739 */
740 bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly,vector<TPolygon2D> &components);
741 /**
742 * Splits a 3D polygon into convex components.
743 * \throw std::logic_error if the polygon can't be fit into a plane.
744 */
745 bool BASE_IMPEXP splitInConvexComponents(const TPolygon3D &poly,vector<TPolygon3D> &components);
746
747 /**
748 * Gets the bisector of a 2D segment.
749 */
751 /**
752 * Gets the bisector of a 3D segment.
753 */
755 /**
756 * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
757 */
758 void BASE_IMPEXP getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis);
759 /**
760 * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
761 * \throw std::logic_error if the lines do not fit in a single plane.
762 */
763 void BASE_IMPEXP getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis);
764
765 /**
766 * Fast ray tracing method using polygons' properties.
767 * \sa CRenderizable::rayTrace
768 */
769 bool BASE_IMPEXP traceRay(const vector<TPolygonWithPlane> &vec,const mrpt::poses::CPose3D &pose,double &dist);
770 /**
771 * Fast ray tracing method using polygons' properties.
772 * \sa CRenderizable::rayTrace
773 */
774 inline bool traceRay(const vector<TPolygon3D> &vec,const mrpt::poses::CPose3D &pose,double &dist) {
775 std::vector<TPolygonWithPlane> pwp;
777 return traceRay(pwp,pose,dist);
778 }
779
780 /** Computes the cross product of two 3D vectors, returning a vector normal to both.
781 * It uses the simple implementation:
782
783 \f[ v_out = \left(
784 \begin{array}{c c c}
785 \hat{i} ~ \hat{j} ~ \hat{k} \\
786 x0 ~ y0 ~ z0 \\
787 x1 ~ y1 ~ z1 \\
788 \end{array} \right)
789 \f]
790 */
791 template<class T,class U,class V>
792 inline void crossProduct3D(const T &v0,const U &v1,V& vOut) {
793 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
794 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
795 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
796 }
797
798 //! \overload
799 template<class T>
800 inline void crossProduct3D(
801 const std::vector<T> &v0,
802 const std::vector<T> &v1,
803 std::vector<T> &v_out )
804 {
805 ASSERT_(v0.size()==3)
806 ASSERT_(v1.size()==3);
807 v_out.resize(3);
808 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
809 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
810 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
811 }
812
813 //! \overload (returning a vector of size 3 by value).
814 template<class VEC1,class VEC2>
815 inline Eigen::Matrix<double,3,1> crossProduct3D(const VEC1 &v0,const VEC2 &v1) {
816 Eigen::Matrix<double,3,1> vOut;
817 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
818 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
819 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
820 return vOut;
821 }
822
823 /** Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
824 * \f[ M([x ~ y ~ z]^\top) = \left(
825 * \begin{array}{c c c}
826 * 0 & -z & y \\
827 * z & 0 & -x \\
828 * -y & x & 0
829 * \end{array} \right)
830 * \f]
831 */
832 template<class VECTOR,class MATRIX>
833 inline void skew_symmetric3(const VECTOR &v,MATRIX& M) {
834 ASSERT_(v.size()==3)
835 M.setSize(3,3);
836 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
837 M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
838 M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
839 }
840 //! \overload
841 template<class VECTOR>
844 skew_symmetric3(v,M);
845 return M;
846 }
847
848 /** Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array:
849 * \f[ -M([x ~ y ~ z]^\top) = \left(
850 * \begin{array}{c c c}
851 * 0 & z & -y \\
852 * -z & 0 & x \\
853 * y & -x & 0
854 * \end{array} \right)
855 * \f]
856 */
857 template<class VECTOR,class MATRIX>
858 inline void skew_symmetric3_neg(const VECTOR &v,MATRIX& M) {
859 ASSERT_(v.size()==3)
860 M.setSize(3,3);
861 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
862 M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
863 M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
864 }
865 //! \overload
866 template<class VECTOR>
870 return M;
871 }
872
873
874 /**
875 * Returns true if two 2D vectors are parallel. The arguments may be points, arrays, etc.
876 */
877 template<class T,class U> inline bool vectorsAreParallel2D(const T &v1,const U &v2) {
878 return abs(v1[0]*v2[1]-v2[0]*v1[1])<geometryEpsilon;
879 }
880
881 /**
882 * Returns true if two 3D vectors are parallel. The arguments may be points, arrays, etc.
883 */
884 template<class T,class U>
885 inline bool vectorsAreParallel3D(const T &v1,const U &v2) {
886 if (abs(v1[0]*v2[1]-v2[0]*v1[1])>=geometryEpsilon) return false;
887 if (abs(v1[1]*v2[2]-v2[1]*v1[2])>=geometryEpsilon) return false;
888 return abs(v1[2]*v2[0]-v2[2]*v1[0])<geometryEpsilon;
889 }
890
891 /** Computes the closest point from a given point to a segment, and returns that minimum distance.
892 */
894 const double & Px,
895 const double & Py,
896 const double & x1,
897 const double & y1,
898 const double & x2,
899 const double & y2,
900 double & out_x,
901 double & out_y);
902
903 /** Computes the closest point from a given point to a segment, and returns that minimum distance.
904 */
906 const double & Px,
907 const double & Py,
908 const double & x1,
909 const double & y1,
910 const double & x2,
911 const double & y2,
912 float & out_x,
913 float & out_y);
914
915
916 /** Computes the closest point from a given point to a segment.
917 * \sa closestFromPointToLine
918 */
920 const double & Px,
921 const double & Py,
922 const double & x1,
923 const double & y1,
924 const double & x2,
925 const double & y2,
926 double &out_x,
927 double &out_y);
928
929 /** Computes the closest point from a given point to a (infinite) line.
930 * \sa closestFromPointToSegment
931 */
933 const double & Px,
934 const double & Py,
935 const double & x1,
936 const double & y1,
937 const double & x2,
938 const double & y2,
939 double &out_x,
940 double &out_y);
941
942 /** Returns the square distance from a point to a line.
943 */
945 const double & Px,
946 const double & Py,
947 const double & x1,
948 const double & y1,
949 const double & x2,
950 const double & y2 );
951
952
953 /** Returns the distance between 2 points in 2D. */
954 template <typename T>
955 T distanceBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
956 return std::sqrt( square(x1-x2)+square(y1-y2) );
957 }
958
959 /** Returns the distance between 2 points in 3D. */
960 template <typename T>
961 T distanceBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
962 return std::sqrt( square(x1-x2)+square(y1-y2)+square(z1-z2) );
963 }
964
965 /** Returns the square distance between 2 points in 2D. */
966 template <typename T>
967 T distanceSqrBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
968 return square(x1-x2)+square(y1-y2);
969 }
970
971 /** Returns the square distance between 2 points in 3D. */
972 template <typename T>
973 T distanceSqrBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
974 return square(x1-x2)+square(y1-y2)+square(z1-z2);
975 }
976
977 /** Returns the intersection point, and if it exists, between two segments.
978 */
980 const double & x1,const double & y1,
981 const double & x2,const double & y2,
982 const double & x3,const double & y3,
983 const double & x4,const double & y4,
984 double &ix,double &iy);
985
986 /** Returns the intersection point, and if it exists, between two segments.
987 */
989 const double & x1,const double & y1,
990 const double & x2,const double & y2,
991 const double & x3,const double & y3,
992 const double & x4,const double & y4,
993 float &ix,float &iy);
994
995 /** Returns true if the 2D point (px,py) falls INTO the given polygon.
996 * \sa pointIntoQuadrangle
997 */
998 bool BASE_IMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
999
1000 /** Specialized method to check whether a point (x,y) falls into a quadrangle.
1001 * \sa pointIntoPolygon2D
1002 */
1003 template <typename T>
1005 T x, T y,
1006 T v1x, T v1y,
1007 T v2x, T v2y,
1008 T v3x, T v3y,
1009 T v4x, T v4y )
1010 {
1011 using mrpt::utils::sign;
1012
1013 const T a1 = atan2( v1y - y , v1x - x );
1014 const T a2 = atan2( v2y - y , v2x - x );
1015 const T a3 = atan2( v3y - y , v3x - x );
1016 const T a4 = atan2( v4y - y , v4x - x );
1017
1018 // The point is INSIDE iff all the signs of the angles between each vertex
1019 // and the next one are equal.
1020 const T da1 = mrpt::math::wrapToPi( a2-a1 );
1021 const T da2 = mrpt::math::wrapToPi( a3-a2 );
1022 if (sign(da1)!=sign(da2)) return false;
1023
1024 const T da3 = mrpt::math::wrapToPi( a4-a3 );
1025 if (sign(da2)!=sign(da3)) return false;
1026
1027 const T da4 = mrpt::math::wrapToPi( a1-a4 );
1028 return (sign(da3)==sign(da4) && (sign(da4)==sign(da1)));
1029 }
1030
1031 /** Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygon or its perimeter.
1032 */
1033 double BASE_IMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
1034
1035 /** Calculates the minimum distance between a pair of lines.
1036 The lines are given by:
1037 - Line 1 = P1 + f (P2-P1)
1038 - Line 2 = P3 + f (P4-P3)
1039 The Euclidean distance is returned in "dist", and the mid point between the lines in (x,y,z)
1040 \return It returns false if there is no solution, i.e. lines are (almost, up to EPS) parallel.
1041 */
1043 const double & p1_x, const double & p1_y, const double & p1_z,
1044 const double & p2_x, const double & p2_y, const double & p2_z,
1045 const double & p3_x, const double & p3_y, const double & p3_z,
1046 const double & p4_x, const double & p4_y, const double & p4_z,
1047 double &x, double &y, double &z,
1048 double &dist);
1049
1050 /** Returns whether two rotated rectangles intersect.
1051 * The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max).
1052 * The second rectangle is given is a similar way, but it is internally rotated according
1053 * to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative
1054 * to the coordinates system of rectangle 1.
1055 */
1057 const double & R1_x_min, const double & R1_x_max,
1058 const double & R1_y_min, const double & R1_y_max,
1059 const double & R2_x_min, const double & R2_x_max,
1060 const double & R2_y_min, const double & R2_y_max,
1061 const double & R2_pose_x,
1062 const double & R2_pose_y,
1063 const double & R2_pose_phi );
1064
1065 /** Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of them.
1066 * NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!
1067 *
1068 * If \f$ d = [ dx ~ dy ~ dz ] \f$ is the input vector, then this function returns a matrix \f$ M \f$ such as:
1069 *
1070 \f[ M = \left(
1071 \begin{array}{c c c}
1072 v^1_x ~ v^2_x ~ v^3_x \\
1073 v^1_y ~ v^2_y ~ v^3_y \\
1074 v^1_z ~ v^2_z ~ v^3_z
1075 \end{array} \right)
1076 \f]
1077 *
1078 * And the three normal vectors are computed as:
1079
1080 \f[ v^1 = \frac{d}{|d|} \f]
1081
1082 If (dx!=0 or dy!=0):
1083 \f[ v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}} \f]
1084 otherwise (the direction vector is vertical):
1085 \f[ v^2 = [1 ~ 0 ~ 0] \f]
1086
1087 And finally, the third vector is the cross product of the others:
1088
1089 \f[ v^3 = v^1 \times v^2 \f]
1090 *
1091 * \return The 3x3 matrix (CMatrixTemplateNumeric<T>), containing one vector per column.
1092 * \except Throws an std::exception on invalid input (i.e. null direction vector)
1093 *
1094 * (JLB @ 18-SEP-2007)
1095 */
1096 template<class T>
1098 {
1100
1101 if (dx==0 && dy==0 && dz==0)
1102 THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
1103
1105
1106 // 1st vector:
1107 T n_xy = square(dx)+square(dy);
1108 T n = sqrt(n_xy+square(dz));
1109 n_xy = sqrt(n_xy);
1110 P(0,0) = dx / n;
1111 P(1,0) = dy / n;
1112 P(2,0) = dz / n;
1113
1114 // 2nd perpendicular vector:
1115 if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
1116 {
1117 P(0,1) = -dy / n_xy;
1118 P(1,1) = dx / n_xy;
1119 P(2,1) = 0;
1120 }
1121 else
1122 {
1123 // Any vector in the XY plane will work:
1124 P(0,1) = 1;
1125 P(1,1) = 0;
1126 P(2,1) = 0;
1127 }
1128
1129 // 3rd perpendicular vector: cross product of the two last vectors:
1130 P.col(2) = crossProduct3D(P.col(0),P.col(1));
1131
1132 return P;
1133 MRPT_END
1134 }
1135
1136
1137 /** Compute a rotation exponential using the Rodrigues Formula.
1138 * The rotation axis is given by \f$\vec{w}\f$, and the rotation angle must
1139 * be computed using \f$ \theta = |\vec{w}|\f$. This is provided as a separate
1140 * function primarily to allow fast and rough matrix exponentials using fast
1141 * and rough approximations to \e A and \e B.
1142 *
1143 * \param w Vector about which to rotate.
1144 * \param A \f$\frac{\sin \theta}{\theta}\f$
1145 * \param B \f$\frac{1 - \cos \theta}{\theta^2}\f$
1146 * \param R Matrix to hold the return value.
1147 * \sa CPose3D
1148 * \note Method from TooN (C) Tom Drummond (GNU GPL)
1149 */
1150 template <typename VECTOR_LIKE, typename Precision, typename MATRIX_LIKE>
1151 inline void rodrigues_so3_exp(const VECTOR_LIKE& w, const Precision A,const Precision B,MATRIX_LIKE & R)
1152 {
1153 ASSERT_EQUAL_(w.size(),3)
1154 ASSERT_EQUAL_(R.getColCount(),3)
1155 ASSERT_EQUAL_(R.getRowCount(),3)
1156 {
1157 const Precision wx2 = (Precision)w[0]*w[0];
1158 const Precision wy2 = (Precision)w[1]*w[1];
1159 const Precision wz2 = (Precision)w[2]*w[2];
1160 R(0,0) = 1.0 - B*(wy2 + wz2);
1161 R(1,1) = 1.0 - B*(wx2 + wz2);
1162 R(2,2) = 1.0 - B*(wx2 + wy2);
1163 }
1164 {
1165 const Precision a = A*w[2];
1166 const Precision b = B*(w[0]*w[1]);
1167 R(0,1) = b - a;
1168 R(1,0) = b + a;
1169 }
1170 {
1171 const Precision a = A*w[1];
1172 const Precision b = B*(w[0]*w[2]);
1173 R(0,2) = b + a;
1174 R(2,0) = b - a;
1175 }
1176 {
1177 const Precision a = A*w[0];
1178 const Precision b = B*(w[1]*w[2]);
1179 R(1,2) = b - a;
1180 R(2,1) = b + a;
1181 }
1182 }
1183
1184 /** @} */ // end of misc. geom. methods
1185
1186 /** @} */ // end of grouping
1187
1188 } // End of namespace
1189
1190} // End of namespace
1191#endif
A numeric matrix of compile-time fixed size.
A sparse matrix container (with cells of any type), with iterators.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
void clear()
Completely removes all elements, although maintaining the matrix's size.
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
Slightly heavyweight type to speed-up calculations with polygons in 3D.
Definition: geometry.h:42
TPolygon3D poly
Actual polygon.
Definition: geometry.h:47
TPolygonWithPlane(const TPolygon3D &p)
Constructor.
TPolygonWithPlane()
Basic constructor.
Definition: geometry.h:75
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:66
mrpt::poses::CPose3D pose
Plane's pose.
Definition: geometry.h:56
TPlane plane
Plane containing the polygon.
Definition: geometry.h:51
mrpt::poses::CPose3D inversePose
Plane's inverse pose.
Definition: geometry.h:61
static void getPlanes(const vector< TPolygon3D > &oldPolys, vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
Definition: geometry.h:877
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
void BASE_IMPEXP createPlaneFromPoseXY(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
Definition: geometry.h:1097
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
void BASE_IMPEXP project2D(const TPoint2D &point, const mrpt::poses::CPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors.
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
Definition: geometry.h:833
void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], char coord, CMatrixDouble &matrix)
Creates a rotation matrix so that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the...
bool BASE_IMPEXP pointIntoPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns true if the 2D point (px,py) falls INTO the given polygon.
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
Definition: geometry.h:967
double BASE_IMPEXP closestSquareDistanceFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2)
Returns the square distance from a point to a line.
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
bool BASE_IMPEXP RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
double BASE_IMPEXP distancePointToPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygo...
void BASE_IMPEXP getRectangleBounds(const std::vector< TPoint2D > &poly, TPoint2D &pMin, TPoint2D &pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
bool BASE_IMPEXP minDistBetweenLines(const double &p1_x, const double &p1_y, const double &p1_z, const double &p2_x, const double &p2_y, const double &p2_z, const double &p3_x, const double &p3_y, const double &p3_z, const double &p4_x, const double &p4_y, const double &p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
void BASE_IMPEXP createFromPoseY(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
void setEpsilon(double nE)
Changes the value of the geometric epsilon.
Definition: geometry.h:726
void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:311
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
void BASE_IMPEXP getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
bool pointIntoQuadrangle(T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
Specialized method to check whether a point (x,y) falls into a quadrangle.
Definition: geometry.h:1004
void BASE_IMPEXP createFromPoseZ(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
double BASE_IMPEXP getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
void BASE_IMPEXP getAngleBisector(const TLine2D &l1, const TLine2D &l2, TLine2D &bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
void BASE_IMPEXP assemblePolygons(const std::vector< TSegment3D > &segms, std::vector< TPolygon3D > &polys)
Tries to assemble a set of segments into a set of closed polygons.
void BASE_IMPEXP createPlaneFromPoseYZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
Definition: geometry.h:885
void BASE_IMPEXP createPlaneFromPoseXZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
void BASE_IMPEXP createFromPoseAndVector(const mrpt::poses::CPose3D &p, const double(&vector)[3], TLine3D &r)
Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose.
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array:
Definition: geometry.h:858
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
Definition: geometry.h:955
void BASE_IMPEXP createPlaneFromPoseAndNormal(const mrpt::poses::CPose3D &pose, const double(&normal)[3], TPlane &plane)
Given a pose and any vector, creates a plane orthogonal to that vector in the pose's coordinates.
void rodrigues_so3_exp(const VECTOR_LIKE &w, const Precision A, const Precision B, MATRIX_LIKE &R)
Compute a rotation exponential using the Rodrigues Formula.
Definition: geometry.h:1151
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
double getEpsilon()
Gets the value of the geometric epsilon.
Definition: geometry.h:733
void BASE_IMPEXP closestFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:792
double BASE_IMPEXP minimumDistanceFromPointToSegment(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment, and returns that minimum distance.
bool BASE_IMPEXP SegmentsIntersection(const double &x1, const double &y1, const double &x2, const double &y2, const double &x3, const double &y3, const double &x4, const double &y4, double &ix, double &iy)
Returns the intersection point, and if it exists, between two segments.
void BASE_IMPEXP closestFromPointToSegment(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
void BASE_IMPEXP createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
#define ASSERT_EQUAL_(__A, __B)
Definition: mrpt_macros.h:264
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_END
Definition: mrpt_macros.h:353
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: bits.h:91
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
2D line without bounds, represented by its equation .
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
void getAsPose2DForcingOrigin(const TPoint2D &origin, mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
3D line, represented by a base point and a director vector.
Standard type for storing any lightweight 2D type.
Standard object for storing any 3D lightweight object.
3D Plane, represented by its equation
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
Lightweight 2D point.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Lightweight 2D pose.
2D segment, consisting of two points.
TPoint2D point2
Destiny point.
TPoint2D point1
Origin point.
3D segment, consisting of two points.
TPoint3D point1
Origin point.
TPoint3D point2
Destiny point.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 04:35:51 UTC 2023