mt
 All Classes Files Functions Enumerations Groups Pages
ellipse3.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Adolfo Rodriguez *
3  * adolfo.rodriguez@upc.edu *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
22 
23 // HEADER GUARD
24 #ifndef MT_ELLIPSE3_H
25 #define MT_ELLIPSE3_H
26 
27 // C++ STANDARD HEADERS
28 #include <iostream>
29 #include <limits>
30 #include <utility>
31 
32 // MT LIBRARY HEADERS
33 #include <mt/cylinder3.h>
34 #include <mt/exception.h>
35 #include <mt/line3.h>
36 #include <mt/matrix3x3.h>
37 #include <mt/transform.h>
38 #include <mt/plane3.h>
39 #include <mt/point3.h>
40 #include <mt/scalar.h>
41 #include <mt/unit3.h>
42 #include <mt/vector3.h>
43 #include <mt/relation/rel_line_plane.h>
44 
45 
47 
48 namespace mt
49 {
50 
51 
53 
56 
70 
71 
72 class Ellipse3
73 {
74 public:
75 
76 // LIFECYCLE
77 
79  Ellipse3();
80 
86  Ellipse3(const Point3& center,
87  const Vector3& axis1,
88  const Vector3& axis2);
89 
93  Ellipse3(const Plane3& plane, const Cylinder3& cylinder);
94 
95  // Compiler generated copy constructor is being used
96 
97  // Compiler generated destructor is being used
98 
99 
100 // OPERATORS
101 
102  // Compiler generated assignment operator is being used
103 
104  bool operator==(const Ellipse3& e) const;
105  bool operator!=(const Ellipse3& e) const;
106 
107 // OPERATIONS
108 
112  Point3 project(const Point3& p) const;
113 
115  Scalar distance(const Point3& p) const;
116 
117 // ACCESS
118 
119  Point3 getCenter() const;
120  Point3& getCenterRef();
121  const Point3& getCenterRef() const;
122 
123  Unit3& getAxisDirRef(const size_t i = 1);
124  const Unit3& getAxisDirRef(const size_t i = 1) const;
125 
126  Scalar& getAxisLengthRef(const size_t i = 1);
127  const Scalar& getAxisLengthRef(const size_t i = 1) const;
128 
132  Vector3 getAxis(const size_t i = 1) const;
133 
135  Plane3 getSupportPlane() const;
136 
140 
141  Unit3 getNormal() const;
142 
143  void setCenter(const Point3& center);
144 
148  void setAxis(const Vector3& axis1,
149  const Vector3& axis2);
150 
154  void setValue(const Point3& center,
155  const Vector3& axis1,
156  const Vector3& axis2);
157 
160  void setValue(const Plane3& plane,
161  const Cylinder3& cylinder);
162 
163 
164 private:
165 
166 // MEMBERS
167 
169 Point3 m_center;
170 
172 Unit3 m_axis_dir1;
173 
175 Unit3 m_axis_dir2;
176 
178 Scalar m_axis_len1;
179 
181 Scalar m_axis_len2;
182 
183 
184 // OPERATIONS
185 
213 
214 std::pair<Scalar, Scalar> project2d(const std::pair<Scalar, Scalar>& p,
215  const Scalar& tol) const;
216 
220 Scalar project2dCore(const Scalar& t,
221  const Scalar& x,
222  const Scalar& y) const;
223 };
224 
225 
227 
228 // OPERATORS
229 
230 std::ostream& operator<<(std::ostream& os,
231  const Ellipse3& e);
232 
233 
234 // FUNCTIONS
235 
237 Point3 project(const Point3& p,
238  const Ellipse3& e);
239 
241 Scalar distance(const Point3& p,
242  const Ellipse3& e);
243 
245 Scalar distance(const Ellipse3& s,
246  const Point3& e);
247 
248 
250 
251 // LIFECYCLE
252 
254 
255  m_center(0.0, 0.0, 0.0),
256  m_axis_dir1(Unit3(1.0, 0.0, 0.0)),
257  m_axis_dir2(Unit3(0.0, 1.0, 0.0)),
258  m_axis_len1(1.0),
259  m_axis_len2(1.0) {}
260 
261 
262 inline Ellipse3::Ellipse3(const Point3& center,
263  const Vector3& axis1,
264  const Vector3& axis2)
265 {
266  setValue(center, axis1, axis2);
267 }
268 
269 
270 inline Ellipse3::Ellipse3(const Plane3& plane, const Cylinder3& cylinder)
271 {
272  setValue(plane, cylinder);
273 }
274 
275 
276 // OPERATORS
277 
278 inline bool Ellipse3::operator==(const Ellipse3& e) const
279 {
280  return (m_center == e.m_center) &&
281  (m_axis_dir1 == e.m_axis_dir1 || m_axis_dir1 == -e.m_axis_dir1) &&
282  (m_axis_dir2 == e.m_axis_dir2 || m_axis_dir2 == -e.m_axis_dir2) &&
283  (m_axis_len1 == e.m_axis_len1) &&
284  (m_axis_len2 == e.m_axis_len2);
285 }
286 
287 
288 inline bool Ellipse3::operator!=(const Ellipse3& e) const
289 {
290  return !(*this == e);
291 }
292 
293 
294 // OPERATIONS
295 
296 inline Point3 Ellipse3::project(const Point3& p) const
297 {
298  // Projection of input point on ellipse support plane
299  const Plane3 sup = getSupportPlane();
300  const Point3 p_pl(sup.project(p));
301 
302  // Orthonormal basis associated to ellipse
303  Unit3 v;
304  Unit3 w;
305  orthonormalBasis(m_axis_dir1, m_axis_dir2, v, w);
306 
307  // Transformations between reference and ellipse frames
308  const Matrix3x3 M(m_axis_dir1[0], v[0], w[0],
309  m_axis_dir1[1], v[1], w[1],
310  m_axis_dir1[2], v[2], w[2]);
311 
312  const Rotation R(M);
313  const Transform Tr_inv(R, m_center);
314  const Transform Tr = Tr_inv.inverse();
315 
316  // Point proj_p in ellipse frame coordinates
317  const Point3 p_el = Tr(p_pl);
318  #ifdef MT_USE_BASIC_SCALAR
319  util::Assert((p_el[2] == 0.0), Exception("Error projecting point on ellipse."));
320  #endif
321 
322  // Projection in ellipse coordinates
323  std::pair<Scalar, Scalar> p_el_2d(p_el[0], p_el[1]);
324 
325  const Scalar tol = Scalar(100.0) * std::numeric_limits<value_t>::epsilon();
326 
327  std::pair<Scalar, Scalar> p_proj_el_2d = project2d(p_el_2d, tol);
328 
329  const Point3 p_proj_el(p_proj_el_2d.first, p_proj_el_2d.second, 0.0);
330 
331  // Projection in reference frame coordinates
332  return Tr_inv(p_proj_el);
333 }
334 
335 
336 inline Scalar Ellipse3::distance(const Point3& p) const
337 {
338  // Projection of input point on ellipse support plane
339  const Plane3 sup = getSupportPlane();
340  const Point3 proj_p(sup.project(p));
341 
342  return length(p - project(p));
343 }
344 
345 
346 inline std::pair<Scalar, Scalar>
347 Ellipse3::project2d(const std::pair<Scalar, Scalar>& p,
348  const Scalar& tol) const
349 {
350  // Point to be projected on ellipse
351  Scalar x = abs(p.first);
352  Scalar y = abs(p.second);
353 
354  // If point is inside the ellipse and contained in at least one of the
355  // x or y axis, its value is perturbated so it does not belong to a
356  // singular configuration with more than one solution
357  if (y == 0.0 && x < m_axis_len1)
358  {
359  y = tol;
360  }
361  if (x == 0.0 && y < m_axis_len2)
362  {
363  x = tol;
364  }
365 
366  // Parameter initialization for root finding
367  Scalar ta = 0.0; // Ellipse parameter (first bound)
368  Scalar tb = HALF_PI; // Ellipse parameter (second bound)
369  Scalar tc; // Ellipse parameter (evaluation point)
370  Scalar fa = project2dCore(ta, x, y); // Function evaluated at point a
371  Scalar fb = project2dCore(tb, x, y); // Function evaluated at point b
372  Scalar fc; // Function evaluated at point c (new point)
373  size_t i; // Counter
374  const size_t imax = 1000; // Maximum number of iterations
375 
376  // Rounds off fa and fb around zero (for stability of numerical method)
377  if (getValue(abs(fa)) <= getValue(tol))
378  {
379  fa = 0.0;
380  }
381  if (getValue(abs(fb)) <= getValue(tol))
382  {
383  fb = 0.0;
384  }
385 
386  // Checks that fa and fb have different signs, so a root bracketing method
387  // can be applied
388  util::Assert((getValue(fa) * getValue(fb) <= 0.0),
389  Exception("Cannot find projection of point on ellipse. \
390 Root finding conditions not met"));
391 
392  // Regula falsi root finding algorithm
393  for (i = 0 ; i < imax; ++i)
394  {
395  // Calculate new point and evaluate it in function
396  tc = tb - fb * (tb - ta) / (fb - fa);
397  fc = project2dCore(tc, x, y);
398 
399  // Update bracketing bounds
400  if (getValue(fa) * getValue(fc) < 0.0)
401  {
402  tb = tc;
403  fb = fc;
404  }
405  else
406  {
407  ta = tc;
408  fa = fc;
409  }
410 
411  // Convergence test
412  if (getValue(abs(fc)) <= getValue(tol))
413  {
414  break;
415  }
416  }
417 
418  // Maximum iterations test
419  util::Assert((i <= imax), Exception("Cannot find projection of point on \
420 ellipse. Reached iteration limit"));
421 
422  // Take care of case where the point was in another quadrant
423  const Scalar sgn_xp = sgn(p.first);
424  const Scalar sgn_yp = sgn(p.second);
425 
426  // Projection point coordinates
427  const Scalar xp = sgn_xp * m_axis_len1 * cos(tc);
428  const Scalar yp = sgn_yp * m_axis_len2 * sin(tc);
429 
430  return std::pair<Scalar, Scalar>(xp, yp);
431 }
432 
433 
434 inline Scalar Ellipse3::project2dCore(const Scalar& t,
435  const Scalar& x,
436  const Scalar& y) const
437 {
438  const Scalar ct = cos(t);
439  const Scalar st = sin(t);
440 
441  // Function whose roots are to be found
442  return (x - m_axis_len1 * ct) * m_axis_len1 * st
443  - (y - m_axis_len2 * st) * m_axis_len2 * ct;
444 }
445 
446 
447 // ACCESS
448 
449 inline Point3 Ellipse3::getCenter() const
450 {
451  return m_center;
452 }
453 
454 
455 inline Point3& Ellipse3::getCenterRef()
456 {
457  return m_center;
458 }
459 
460 
461 inline const Point3& Ellipse3::getCenterRef() const
462 {
463  return m_center;
464 }
465 
466 
467 inline Unit3& Ellipse3::getAxisDirRef(const size_t i)
468 {
469  return (i == 1) ? m_axis_dir1 : m_axis_dir2;
470 }
471 
472 
473 inline const Unit3& Ellipse3::getAxisDirRef(const size_t i) const
474 {
475  return (i == 1) ? m_axis_dir1 : m_axis_dir2;
476 }
477 
478 
479 inline Scalar& Ellipse3::getAxisLengthRef(const size_t i)
480 {
481  return (i == 1) ? m_axis_len1 : m_axis_len2;
482 }
483 
484 
485 inline const Scalar& Ellipse3::getAxisLengthRef(const size_t i) const
486 {
487  return (i == 1) ? m_axis_len1 : m_axis_len2;
488 }
489 
490 
491 inline Vector3 Ellipse3::getAxis(const size_t i) const
492 {
493  if (i == 1)
494  {
495  return m_axis_len1 * m_axis_dir1;
496  }
497  else
498  {
499  return m_axis_len2 * m_axis_dir2;
500  }
501 }
502 
503 
505 {
506  const Unit3 norm = getNormal();
507  return Plane3(norm, m_center);
508 }
509 
511 {
512  // Minimum and maximun axis
513  Scalar min_len;
514  Scalar max_len;
515  Unit3 min_dir;
516  Unit3 max_dir;
517 
518  if (m_axis_len1 < m_axis_len2)
519  {
520  min_len = m_axis_len1;
521  max_len = m_axis_len2;
522  min_dir = m_axis_dir1;
523  max_dir = m_axis_dir2;
524  }
525  else
526  {
527  min_len = m_axis_len2;
528  max_len = m_axis_len1;
529  min_dir = m_axis_dir2;
530  max_dir = m_axis_dir1;
531  }
532 
533  // Angle between cylinder and support plane
534  const Scalar angle = asin(min_len / max_len);
535 
536  // Cylinder axis
537  const Rotation rot(min_dir, angle);
538  const Unit3 dir = rot(max_dir);
539  const Line3 axis(dir, m_center);
540 
541  // Support cylinder
542  return Cylinder3(axis, min_len);
543 }
544 
545 
546 inline Unit3 Ellipse3::getNormal() const
547 {
548  return cross(m_axis_dir1, m_axis_dir2);
549 }
550 
551 
552 inline void Ellipse3::setCenter(const Point3& center)
553 {
554  m_center = center;
555 }
556 
557 
558 inline void Ellipse3::setAxis(const Vector3& axis1,
559  const Vector3& axis2)
560 {
561  // Verifies that ellipse axis are perpendicular
562  #ifdef MT_USE_BASIC_SCALAR
563  const Scalar dotp = dot(Unit3(axis1), Unit3(axis2));
564  util::Assert((dotp == 0.0),
565  Exception("Cannot construct ellipse, axis are not perpendicular"));
566  #endif
567 
568  // Updates axis-related data
569  m_axis_dir1 = axis1;
570  m_axis_dir2 = axis2;
571  m_axis_len1 = axis1.length();
572  m_axis_len2 = axis2.length();
573 }
574 
575 
576 inline void Ellipse3::setValue(const Point3& center,
577  const Vector3& axis1,
578  const Vector3& axis2)
579 {
580  setCenter(center);
581  setAxis(axis1, axis2);
582 }
583 
584 
585 inline void Ellipse3::setValue(const Plane3& plane,
586  const Cylinder3& cylinder)
587 {
588  // Cylinder parameters
589  const Line3 axis(cylinder.getAxis());
590  const Scalar radius(cylinder.getRadius());
591 
592  // Verifies that cylinder axis and plane intersect
593  const RelLinePlane rel(axis, plane);
594  const bool is_intersecting = rel.getType().test(INTERSECTING);
595  util::Assert(is_intersecting, Exception("Cannot construct ellipse, \
596  input cylinder and sphere do not intersect"));
597 
598  // Constructs ellipse
599  const Scalar ang = rel.getAngle();
600  const Unit3 Pnorm = plane.getNormal();
601  const Unit3 Ldir = axis.getDirection();
602 
603  if (rel.getType().test(PERPENDICULAR))
604  {
605  m_axis_dir1 = plane.getSupportVector();
606  }
607  else
608  {
609  m_axis_dir1 = cross(Pnorm, Ldir);
610  }
611  m_axis_dir2 = cross(m_axis_dir1, Pnorm);
612  m_axis_len1 = radius;
613  m_axis_len2 = radius / sin(ang);
614  m_center = rel.getIntersectionPoint();
615 }
616 
617 
619 
620 // OPERATORS
621 
622 inline std::ostream& operator<<(std::ostream& os,
623  const Ellipse3& e)
624 {
625  const Vector3 axis1 = e.getAxis(1);
626  const Vector3 axis2 = e.getAxis(2);
627 
628  const Unit3 axis_dir1 = axis1;
629  const Unit3 axis_dir2 = axis2;
630 
631  const Scalar axis_len1 = axis1.length();
632  const Scalar axis_len2 = axis2.length();
633 
634  return os << "center: " << e.getCenter() << ' '
635  << "axis 1: " << axis_dir1 << " length " << axis_len1 << ' '
636  << "axis 2: " << axis_dir2 << " length " << axis_len2;
637 }
638 
639 
640 // FUNCTIONS
641 
642 inline Point3 project(const Point3& p,
643  const Ellipse3& e)
644 {
645  return e.project(p);
646 }
647 
648 
649 inline Scalar distance(const Point3& p,
650  const Ellipse3& e)
651 {
652  return e.distance(p);
653 }
654 
655 
656 inline Scalar distance(const Ellipse3& e,
657  const Point3& p)
658 {
659  return e.distance(p);
660 }
661 
662 } // mt
663 
664 #endif // MT_ELLIPSE3_H
665