geometry2D.cpp

Go to the documentation of this file.

#include <dlrNumeric/geometry2D.h>
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace numeric {
    
    bool
    bilaterate(const Vector2D& point0, const Vector2D& point1,
               double range0, double range1,
               Vector2D& intersection0, Vector2D& intersection1)
    {
      // We use the traditional approach of transforming the points so
      // that point0 is at the origin and point1 is on the X axis,
      // solving this new bilateration, and then transforming back to
      // the original coordinate system.

      Vector2D baselineVector(point1 - point0);
    
      // Coordinates for the translated input points will be (0, 0), and
      // (x0, 0);
      double x0 = magnitude(baselineVector);
    
      // Compute direction vectors for the new X and Y axis.
      Vector2D e0(baselineVector / x0);
      Vector2D e1(-e0.y(), e0.x());

      // In our new coordinate system, we can write x^2 + y^2 = r0^2,
      // and (x - x0)^2 + y^2 = r1^2.
      //
      // Subtracting these two equations gives 2*x0*x - x0^2 = r0^2 -
      // r1^2, which gives us x = (r0^2 - r1^2 + x0^2) / (2*x0).
      double r0Squared = range0 * range0;
      double xValue = (r0Squared - range1 * range1 + x0 * x0) / (2 * x0);

      // Substituting this into x^2 + y^2 = r0^2, we have y = sqrt(r0^2 - x^2).
      double r0SquaredMinusXSquared = r0Squared - xValue * xValue;
      if(r0SquaredMinusXSquared < 0.0) {
        return false;
      }
      double yValue = std::sqrt(r0SquaredMinusXSquared);

      // Now we tranform back into the original coordinate system.
      Vector2D xComponent = xValue * e0;
      Vector2D yComponent = yValue * e1;
      intersection0 = point0 + xComponent + yComponent;
      intersection1 = point0 + xComponent - yComponent;
      return true;
    }
  
  } // namespace numeric

} // namespace dlr