GeometryUtilities.cc

Go to the documentation of this file.

#include <utl/Math.h>
#include <utl/AxialVector.h>
#include <utl/GeometryUtilities.h>
#include <utl/MathConstants.h>
#include <utl/ReferenceEllipsoid.h>
#include <utl/Test.h>
#include <utl/CoordinateSystemPtr.h>
#include <utl/PhysicalConstants.h>

#include <cmath>

using namespace std;


namespace utl {

  Line
  Intersection(const Plane& p1, const Plane& p2)
  {
    const auto& a1 = p1.GetAnchor();
    const Vector b = 0.5 * (a1 - p2.GetAnchor());
    const auto& n1 = p1.GetNormal();
    const auto& n2 = p2.GetNormal();
    const double n1b = n1 * b;
    const double n2b = n2 * b;
    const double nn = n1 * n2;
    const double det = 1 - Sqr(nn);
    const double alpha = (nn*n2b + n1b) / det;
    const double beta = - (nn*n1b + n2b) / det;
    const Point anch = a1 - b + alpha*n1 + beta*n2;
    const Vector dir = Cross(n1, n2);

    return Line(anch, dir);
  }


  Point
  Intersection(const Plane& plane, const Line& line)
  {
    const auto& al = line.GetAnchor();
    const auto& dir = line.GetDirection();
    const auto& n = plane.GetNormal();
    return al - ((n*(al - plane.GetAnchor())) / (n*dir)) * dir;
  }


  vector<Point>
  Intersection(const ReferenceEllipsoid& ellipsoid,
               const double height,
               const Line& line)
  {
    const double alpha = ellipsoid.GetPolarRadius() + height;  // minor axis
    const double beta = ellipsoid.GetEquatorialRadius() + height;  // major axis

    const double alpha2 = Sqr(alpha);
    const double beta2 = Sqr(beta);

    const auto& csECEF = CoordinateSystem::GetRootCoordinateSystem();
    const auto& direction = line.GetDirection();

    // direction in kartesian coordinate system Earth Centered Earth Fixed
    const auto uvw = direction.GetCoordinates(csECEF);
    const double& u = uvw.get<0>();
    const double& v = uvw.get<1>();
    const double& w = uvw.get<2>();

    const auto& anchor = line.GetAnchor();
    // point in kartesian coordinates
    const auto xyz = anchor.GetCoordinates(csECEF);
    const double& x = xyz.get<0>();
    const double& y = xyz.get<1>();
    const double& z = xyz.get<2>();

    // -- constants for quadratic equation --
    const double a = (Sqr(u) + Sqr(v))*alpha2 + Sqr(w*beta);
    const double b = (x*u + y*v)*alpha2 + z*w*beta2;
    const double c = (Sqr(x) + Sqr(y))*alpha2 + (Sqr(z) - alpha2)*beta2;

    // -- for quadratic equation --
    const double ba = -b/a;
    const double dis = Sqr(ba) - c/a;  // D

    vector<Point> intersectionPoints; // 0 <= intersection points <= 2

    // since 0 is hardly reached with double-division...
    if (CloseTo()(dis, 0.)) {

      // D == 0   //just touching the surface
      // point, where particle touches the ellipsoid
      intersectionPoints.push_back(anchor + ba*direction);

    } else if (dis > 0) {  // ----------------- 2 intersection points with ellipsoid

      // two solutions of quadratic equation
      const double sdis = sqrt(dis);

      // point, where particle left the ellipsoid
      intersectionPoints.push_back(anchor + (ba + sdis)*direction);
      // point, where particle hit the ellipsoid
      intersectionPoints.push_back(anchor + (ba - sdis)*direction);

    }

    return intersectionPoints;
  }


  double
  Distance(const Line& line1, const Line& line2)
  {
    const auto& s1 = line1.GetDirection();
    const auto& s2 = line2.GetDirection();
    const Vector b = line1.GetAnchor() - line2.GetAnchor();
    Vector n = Cross(s1, s2);
    const double norm2 = n.GetMag2();
    if (norm2) {
      n /= sqrt(norm2);
      return b * n;
    } else
      return sqrt(b*b - Sqr(b*s1));
  }


  double
  Distance(const Point& point, const Line& line)
  {
    const Vector a = point - line.GetAnchor();
    const auto& dir = line.GetDirection();
    const Vector b = (dir*a)*dir;
    const Vector perp = a - b;
    return perp.GetMag();
  }

}