34 #include <mt/exception.h>
43 #include <mt/relation/rel_line_plane.h>
104 bool operator==(
const Ellipse3& e)
const;
105 bool operator!=(
const Ellipse3& e)
const;
121 const Point3& getCenterRef()
const;
123 Unit3& getAxisDirRef(
const size_t i = 1);
124 const Unit3& getAxisDirRef(
const size_t i = 1)
const;
126 Scalar& getAxisLengthRef(
const size_t i = 1);
127 const Scalar& getAxisLengthRef(
const size_t i = 1)
const;
141 Unit3 getNormal()
const;
143 void setCenter(
const Point3& center);
214 std::pair<Scalar, Scalar> project2d(
const std::pair<Scalar, Scalar>& p,
215 const Scalar& tol)
const;
220 Scalar project2dCore(
const Scalar& t,
222 const Scalar& y)
const;
230 std::ostream& operator<<(std::ostream& os,
241 Scalar distance(
const Point3& p,
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)),
278 inline bool Ellipse3::operator==(
const Ellipse3& e)
const
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);
288 inline bool Ellipse3::operator!=(
const Ellipse3& e)
const
290 return !(*
this == e);
305 orthonormalBasis(m_axis_dir1, m_axis_dir2, v, w);
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]);
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."));
323 std::pair<Scalar, Scalar> p_el_2d(p_el[0], p_el[1]);
325 const Scalar tol = Scalar(100.0) * std::numeric_limits<value_t>::epsilon();
327 std::pair<Scalar, Scalar> p_proj_el_2d = project2d(p_el_2d, tol);
329 const Point3 p_proj_el(p_proj_el_2d.first, p_proj_el_2d.second, 0.0);
332 return Tr_inv(p_proj_el);
346 inline std::pair<Scalar, Scalar>
347 Ellipse3::project2d(
const std::pair<Scalar, Scalar>& p,
348 const Scalar& tol)
const
351 Scalar x = abs(p.first);
352 Scalar y = abs(p.second);
357 if (y == 0.0 && x < m_axis_len1)
361 if (x == 0.0 && y < m_axis_len2)
370 Scalar fa = project2dCore(ta, x, y);
371 Scalar fb = project2dCore(tb, x, y);
374 const size_t imax = 1000;
377 if (getValue(abs(fa)) <= getValue(tol))
381 if (getValue(abs(fb)) <= getValue(tol))
388 util::Assert((getValue(fa) * getValue(fb) <= 0.0),
389 Exception(
"Cannot find projection of point on ellipse. \
390 Root finding conditions not met"));
393 for (i = 0 ; i < imax; ++i)
396 tc = tb - fb * (tb - ta) / (fb - fa);
397 fc = project2dCore(tc, x, y);
400 if (getValue(fa) * getValue(fc) < 0.0)
412 if (getValue(abs(fc)) <= getValue(tol))
419 util::Assert((i <= imax), Exception(
"Cannot find projection of point on \
420 ellipse. Reached iteration limit"));
423 const Scalar sgn_xp = sgn(p.first);
424 const Scalar sgn_yp = sgn(p.second);
427 const Scalar xp = sgn_xp * m_axis_len1 * cos(tc);
428 const Scalar yp = sgn_yp * m_axis_len2 * sin(tc);
430 return std::pair<Scalar, Scalar>(xp, yp);
434 inline Scalar Ellipse3::project2dCore(
const Scalar& t,
436 const Scalar& y)
const
438 const Scalar ct = cos(t);
439 const Scalar st = sin(t);
442 return (x - m_axis_len1 * ct) * m_axis_len1 * st
443 - (y - m_axis_len2 * st) * m_axis_len2 * ct;
449 inline Point3 Ellipse3::getCenter()
const
455 inline Point3& Ellipse3::getCenterRef()
461 inline const Point3& Ellipse3::getCenterRef()
const
467 inline Unit3& Ellipse3::getAxisDirRef(
const size_t i)
469 return (i == 1) ? m_axis_dir1 : m_axis_dir2;
473 inline const Unit3& Ellipse3::getAxisDirRef(
const size_t i)
const
475 return (i == 1) ? m_axis_dir1 : m_axis_dir2;
479 inline Scalar& Ellipse3::getAxisLengthRef(
const size_t i)
481 return (i == 1) ? m_axis_len1 : m_axis_len2;
485 inline const Scalar& Ellipse3::getAxisLengthRef(
const size_t i)
const
487 return (i == 1) ? m_axis_len1 : m_axis_len2;
495 return m_axis_len1 * m_axis_dir1;
499 return m_axis_len2 * m_axis_dir2;
506 const Unit3 norm = getNormal();
507 return Plane3(norm, m_center);
518 if (m_axis_len1 < m_axis_len2)
520 min_len = m_axis_len1;
521 max_len = m_axis_len2;
522 min_dir = m_axis_dir1;
523 max_dir = m_axis_dir2;
527 min_len = m_axis_len2;
528 max_len = m_axis_len1;
529 min_dir = m_axis_dir2;
530 max_dir = m_axis_dir1;
534 const Scalar angle = asin(min_len / max_len);
538 const Unit3 dir = rot(max_dir);
539 const Line3 axis(dir, m_center);
546 inline Unit3 Ellipse3::getNormal()
const
548 return cross(m_axis_dir1, m_axis_dir2);
552 inline void Ellipse3::setCenter(
const Point3& center)
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"));
571 m_axis_len1 = axis1.
length();
572 m_axis_len2 = axis2.
length();
589 const Line3 axis(cylinder.getAxis());
590 const Scalar radius(cylinder.getRadius());
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"));
599 const Scalar ang = rel.getAngle();
601 const Unit3 Ldir = axis.getDirection();
603 if (rel.getType().test(PERPENDICULAR))
609 m_axis_dir1 = cross(Pnorm, Ldir);
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();
622 inline std::ostream& operator<<(std::ostream& os,
628 const Unit3 axis_dir1 = axis1;
629 const Unit3 axis_dir2 = axis2;
631 const Scalar axis_len1 = axis1.
length();
632 const Scalar axis_len2 = axis2.
length();
634 return os <<
"center: " << e.getCenter() <<
' '
635 <<
"axis 1: " << axis_dir1 <<
" length " << axis_len1 <<
' '
636 <<
"axis 2: " << axis_dir2 <<
" length " << axis_len2;
642 inline Point3 project(
const Point3& p,
649 inline Scalar distance(
const Point3& p,
652 return e.distance(p);
656 inline Scalar distance(
const Ellipse3& e,
659 return e.distance(p);
664 #endif // MT_ELLIPSE3_H