Distance.C

Go to the documentation of this file.

/** @file Distance.C

    $Id: Distance.C 5279 2007-06-23 02:49:08Z mburo $
    $Source$
*/

// This is an ORTS file
// (c) Michael Buro  
// (c) Tim Furtak    
// licensed under the GPL

#include "Game.H"
#include "Distance.H"
#include "LinAlg.H"

using namespace std;

//===================================================================
namespace Distance {
/** Distance from a circle of radius r to a point. */

inline static real8 dist_c_p(sint4 cx, sint4 cy, sint4 r, sint4 px, sint4 py)
{
  real8 dist = sqrt(square(real8(cx-px)) + square(real8(cy-py))) - r;
  //  if (dist < 0) dist = 0;
  return dist;
}

//===================================================================

/** Distance between two points. */

inline static real8 dist_p_p(sint4 cx, sint4 cy, sint4 px, sint4 py)
{
  return sqrt(square(real8(cx-px)) + square(real8(cy-py)));
}

//===================================================================

/** Distance from a point to a line segment. */

inline static real8 dist_p_l(sint4 cx, sint4 cy,
                             sint4 x1, sint4 y1, sint4 x2, sint4 y2)
{
  sint4 ux = x2 - x1;
  sint4 uy = y2 - y1;
  sint4 vx = cx - x1;
  sint4 vy = cy - y1;

  sint4 u_v = ux * vx + uy * vy; // u dot v
  if (u_v <= 0)
    return dist_p_p(cx, cy, x1, y1);

  sint4 u_u = ux * ux + uy * uy; // u dot u
  if (u_u <= u_v)
    return dist_p_p(cx, cy, x2, y2);

  real8 b = real8(u_v) / real8(u_u);
  Vec2<real8> p(x1+b*ux - cx, y1+b*uy - cy); // (xy1 + b * u) - c
  return p.length();
}

//===================================================================

/** Distance between two line segments.
    fixme: simple but inefficient hack
*/

inline static real8 dist_l_l(const ScalarPoint &a1, const ScalarPoint &a2,
                             const ScalarPoint &b1, const ScalarPoint &b2)
{
  sint4 cross1_1 = (a2.x-a1.x) * (b1.y-a1.y) - (a2.y-a1.y) * (b1.x-a1.x);
  sint4 cross1_2 = (a2.x-a1.x) * (b2.y-a1.y) - (a2.y-a1.y) * (b2.x-a1.x);

  sint4 cross2_1 = (b2.x-b1.x) * (a1.y-b1.y) - (b2.y-b1.y) * (a1.x-b1.x);
  sint4 cross2_2 = (b2.x-b1.x) * (a2.y-b1.y) - (b2.y-b1.y) * (a2.x-b1.x);

  // the lines cross
  if ((cross1_1 * cross1_2 < 0) && (cross2_1 * cross2_2 < 0)) return -1;

  real8 d1 = dist_p_l(a1.x, a1.y, b1.x, b1.y, b2.x, b2.y);
  real8 d2 = dist_p_l(a2.x, a2.y, b1.x, b1.y, b2.x, b2.y);
  real8 d3 = dist_p_l(b1.x, b1.y, a1.x, a1.y, a2.x, a2.y);
  real8 d4 = dist_p_l(b2.x, b2.y, a1.x, a1.y, a2.x, a2.y);

  return min( min(d1,d2), min(d3,d4) );
}

//===================================================================
//===================================================================
//===================================================================

inline static real8 distance_circle_circle(const Object &e1, const Object &e2)
{
  sint4 x1, y1, x2, y2;
  e1.get_center(x1, y1);
  e2.get_center(x2, y2);

  ScalarPoint delta(x1 - x2, y1 - y2);
  Scalar r = FixedPoint::add(e1.get_radius(), e2.get_radius());
  real8 dist = delta.l2_norm() - r;
  //  dist -= 1; // slack
  //  if (dist < 0) dist = 0;
  return dist;
}

//===================================================================

inline static real8 distance_circle_point(const Object &e1, Scalar px, Scalar py)
{
  sint4 x1, y1;
  e1.get_center(x1, y1);
  return dist_c_p(x1, y1, e1.get_radius(), px, py);
}

//===================================================================

/** e1 = circle, e2 = line */

inline static real8 distance_circle_line(const Object &e1, const Object &e2)
{
  sint4 cx, cy;
  e1.get_center(cx, cy);
  Scalar r  = e1.get_radius();

  sint4 x1, y1, x2, y2;
  e2.get_p1(x1, y1);
  e2.get_p2(x2, y2);

  if (x1 == x2) {

    // vline |

    if (cy < y1) return dist_c_p(cx, cy, r, x1, y1);
    if (cy > y2) return dist_c_p(cx, cy, r, x2, y2);
    return dist_c_p(cx, cy, r, x1, cy);
    
  } else if (y1 == y2) {

    // hline --

    if (cx < x1) return dist_c_p(cx, cy, r, x1, y1);
    if (cx > x2) return dist_c_p(cx, cy, r, x2, y2);
    return dist_c_p(cx, cy, r, cx, y1);

  } else {
    
    // arbitrary line /
    
    real8 d = dist_p_l(cx, cy, x1, y1, x2, y2) - r;
    //    if (d < 0) d = 0;
    return d;
    
  }
}

//===================================================================
/*
inline static real8 distance_circle_polyline(const GameObj &e1, const GameObj &e2)
{
}
*/
//===================================================================

inline static real8 distance_line_point(const Object &e1, Scalar px, Scalar py)
{
  sint4 x1, y1, x2, y2;
  e1.get_p1(x1, y1);
  e1.get_p2(x2, y2);
  return dist_p_l(px, py, x1, y1, x2, y2);
}

//===================================================================

inline static real8 distance_line_line(const Object &e1, const Object &e2)
{
  sint4 e1_x1, e1_y1, e1_x2, e1_y2;
  sint4 e2_x1, e2_y1, e2_x2, e2_y2;
  e1.get_p1(e1_x1, e1_y1);
  e1.get_p2(e1_x2, e1_y2);
  e2.get_p1(e2_x1, e2_y1);
  e2.get_p2(e2_x2, e2_y2);

  // at least one arbitrary line
  if (((e1.get_shape() == Object::LINE) && (e1_x1 != e1_x2) && (e1_y1 != e1_y2)) ||
      ((e2.get_shape() == Object::LINE) && (e2_x1 != e2_x2) && (e2_y1 != e2_y2)))
    return dist_l_l(ScalarPoint(e1_x1, e1_y1),
                    ScalarPoint(e1_x2, e1_y2),
                    ScalarPoint(e2_x1, e2_y1),
                    ScalarPoint(e2_x2, e2_y2));

  // both vertical
  if ((e1_x1 == e1_x2) && (e2_x1 == e2_x2)) {
    sint4 y1a = e1_y1;
    sint4 y2a = e1_y2;
    sint4 y1b = e2_y1;
    sint4 y2b = e2_y2;
    if (y1a > y2a) swap(y1a, y2a);
    if (y1b > y2b) swap(y1b, y2b);
    
    if (y1a > y2b) return dist_p_p(e1_x1, y1a, e2_x1, y2b);
    if (y2a < y1b) return dist_p_p(e1_x1, y2a, e2_x1, y1b);
    return real8(abs(e1_x1 - e2_x1));
  }

  // both horizontal
  if ((e1_y1 == e1_y2) && (e2_y1 == e2_y2)) {
    sint4 x1a = e1_x1;
    sint4 x2a = e1_x2;
    sint4 x1b = e2_x1;
    sint4 x2b = e2_x2;
    if (x1a > x2a) swap(x1a, x2a);
    if (x1b > x2b) swap(x1b, x2b);
    
    if (x1a > x2b) return dist_p_p(x1a, e1_y1, x2b, e2_y1);
    if (x2a < x1b) return dist_p_p(x2a, e1_y1, x1b, e2_y1);
    return real8(abs(e1_y1 - e2_y1));
  }
  
  // mixed alignments
  
  if (e1_x1 == e1_x2) { // e1 -> vertical, e2 -> horizontal
    sint4 y1 = min(e1_y1, e1_y2);
    sint4 y2 = max(e1_y1, e1_y2);
    if (e2_y1 > y2) return dist_p_l(e1_x1, y2, e2_x1, e2_y1, e2_x2, e2_y2);
    if (e2_y1 < y1) return dist_p_l(e1_x1, y1, e2_x1, e2_y1, e2_x2, e2_y2);
    sint4 x1 = min(e2_x1, e2_x2);
    sint4 x2 = max(e2_x1, e2_x2);
    if (e1_x1 > x2) return dist_p_l(x2, e2_y1, e1_x1, y1, e1_x1, y2);
    if (e1_x1 < x1) return dist_p_l(x1, e2_y1, e1_x1, y1, e1_x1, y2);
    return -1; // the lines cross
  }

  // e2 -> vertical, e1 -> horizontal
  
  sint4 y1 = min(e2_y1, e2_y2);
  sint4 y2 = max(e2_y1, e2_y2);
  if (e1_y1 > y2) return dist_p_l(e2_x1, y2, e1_x1, e1_y1, e1_x2, e1_y2);
  if (e1_y1 < y1) return dist_p_l(e2_x1, y1, e1_x1, e1_y1, e1_x2, e1_y2);
  sint4 x1 = min(e1_x1, e1_x2);
  sint4 x2 = max(e1_x1, e1_x2);
  if (e2_x1 > x2) return dist_p_l(x2, e1_y1, e2_x1, y1, e2_x1, y2);
  if (e2_x1 < x1) return dist_p_l(x1, e1_y1, e2_x1, y1, e2_x1, y2);
  return -1; // the lines cross
}

//===================================================================

inline static real8 distance_rectangle_line(const Object &e1, const Object &e2)
{
  sint4 o1x1, o1y1, o1x2, o1y2;
  sint4 o2x1, o2y1, o2x2, o2y2;
  e1.get_p1(o1x1, o1y1);
  e1.get_p2(o1x2, o1y2);
  e2.get_p1(o2x1, o2y1);
  e2.get_p2(o2x2, o2y2);

  if (o2x1 == o2x2) {           // vline |
    
    const sint4 x = o2x1;
    sint4 y1, y2;
    if (o2y1 < o2y2) {
      y1 = o2y1;
      y2 = o2y2;
    } else {
      y1 = o2y2;
      y2 = o2y1;
    }
    
    if (o1x1 >= x) { // line is to the left of the rectangle
      if (o1y1 > y2) return dist_p_p(x, y2, o1x1, o1y1);
      if (o1y2 < y1) return dist_p_p(x, y1, o1x1, o1y2);
      return real8(o1x1 - x);
    }
    
    if (o1x2 <= x) { // line is to the right of the rectangle
      if (o1y1 > y2) return dist_p_p(x, y2, o1x2, o1y1);
      if (o1y2 < y1) return dist_p_p(x, y1, o1x2, o1y2);
      return real8(x - o1x2);
    }
    
    // the x coords of the line are within the rectangle
    if (y1 >= o1y2) return real8(y1 - o1y2);
    if (y2 <= o1y1) return real8(o1y1 - y2); 
    return -1; 
    
  } else if (o2y1 == o2y2) {    // hline --
    
    const sint4 y = o2y1;
    sint4 x1, x2;
    if (o2x1 < o2x2) {
      x1 = o2x1;
      x2 = o2x2;
    } else {
      x1 = o2x2;
      x2 = o2x1;
    }
    
    if (o1y1 >= y) { // line is above the rectangle
      if (o1x1 > x2) return dist_p_p(x2, y, o1x1, o1y1); 
      if (o1x2 < x1) return dist_p_p(x1, y, o1x2, o1y1); 
      return real8(o1y1 - y);
    }
    
    if (o1y2 <= y) { // line is below the rectangle
      if (o1x1 > x2) return dist_p_p(x2, y, o1x1, o1y2); 
      if (o1x2 < x1) return dist_p_p(x1, y, o1x2, o1y2); 
      return real8(y - o1y2); 
    }
    
    // the y coords of the line are within the rectangle
    if (x1 >= o1x2) return real8(x1 - o1x2); 
    if (x2 <= o1x1) return real8(o1x1 - x2); 
    return -1; 
    
  } else { // arbitrary line
    
    sint4 x1 = o2x1;
    sint4 x2 = o2x2;
    sint4 y1 = o2y1;
    sint4 y2 = o2y2;
    
    // if either of the two line points is inside the rectangle return -1
    if ((x1 < o1x2 && o1x1 < x1 && y1 < o1y2 && o1y1 < y1 ) ||
        (x2 < o1x2 && o1x1 < x2 && y2 < o1y2 && o1y1 < y2 )) {
      return -1;
    }
    
    //      if (x1 > x2) swap(x1, x2);
    //      if (y1 > y2) swap(y1, y2);
    
    // arbitrary line /
    ScalarPoint p(x1, y1);
    ScalarPoint q(x2, y2);
    
    ScalarPoint s1(o1x1, o1y1);
    ScalarPoint s2(o1x1, o1y2);
    ScalarPoint s3(o1x2, o1y2);
    ScalarPoint s4(o1x2, o1y1);
    
    real8 a = dist_l_l(p, q,  s1, s2);
    real8 b = dist_l_l(p, q,  s2, s3);
    real8 c = dist_l_l(p, q,  s3, s4);
    real8 d = dist_l_l(p, q,  s4, s1);
    
    return min(min(a, b),min(c, d));
    
  }

}

//===================================================================

/** e1 = circle, e2 = rectangle */

inline static real8 distance_circle_rectangle(const Object &e1, const Object &e2)
{
  sint4 cx, cy;
  e1.get_center(cx, cy);
  Scalar r = e1.get_radius();

  sint4 o2x1, o2y1, o2x2, o2y2;
  e2.get_p1(o2x1, o2y1);
  e2.get_p2(o2x2, o2y2);


  if (cx < o2x1) {
    
    if (cy < o2y1) return dist_c_p(cx, cy, r, o2x1, o2y1);
    if (cy > o2y2) return dist_c_p(cx, cy, r, o2x1, o2y2);
    return dist_c_p(cx, cy, r, o2x1, cy);
    
  }
  if (cx > o2x2) {
    
    if (cy < o2y1) return dist_c_p(cx, cy, r, o2x2, o2y1);
    if (cy > o2y2) return dist_c_p(cx, cy, r, o2x2, o2y2);
    return dist_c_p(cx, cy, r, o2x2, cy);
    
  }
  
  if (cy < o2y1) return dist_c_p(cx, cy, r, cx, o2y1);
  if (cy > o2y2) return dist_c_p(cx, cy, r, cx, o2y2);
  return -1; // inside the rectangle
}

//===================================================================

inline static real8 distance_rectangle_rectangle(const Object &e1, const Object &e2)
{
  sint4 o1x1, o1y1, o1x2, o1y2;
  sint4 o2x1, o2y1, o2x2, o2y2;
  e1.get_p1(o1x1, o1y1);
  e1.get_p2(o1x2, o1y2);
  e2.get_p1(o2x1, o2y1);
  e2.get_p2(o2x2, o2y2);

  if (o1x1 > o2x2) {
    if (o1y1 > o2y2) return dist_p_p(o1x1, o1y1, o2x2, o2y2);
    if (o1y2 < o2y1) return dist_p_p(o1x1, o1y2, o2x2, o2y1);
    return real8(o1x1 - o2x2);
  }
  
  if (o1x2 < o2x1) {
    if (o1y1 > o2y2) return dist_p_p(o1x2, o1y1, o2x1, o2y2);
    if (o1y2 < o2y1) return dist_p_p(o1x2, o1y2, o2x1, o2y1);
    return real8(o2x1 - o1x2);
  }
  
  if (o1y1 > o2y2) return real8(o1y1 - o2y2);
  if (o1y2 < o2y1) return real8(o2y1 - o1y2);
  return -1; // intersect
}

//===================================================================

inline static real8 distance_rectangle_point(const Object &e1, Scalar cx, Scalar cy)
{
  sint4 x1, y1, x2, y2;
  e1.get_p1(x1, y1);
  e1.get_p2(x2, y2);


  if (cx < x1) {
    
    if (cy < y1) return dist_p_p(cx, cy, x1, y1);
    if (cy > y2) return dist_p_p(cx, cy, x1, y2);
    return dist_p_p(cx, cy, x1, cy);
    
  }
  if (cx > x2) {
    
    if (cy < y1) return dist_p_p(cx, cy, x2, y1);
    if (cy > y2) return dist_p_p(cx, cy, x2, y2);
    return dist_p_p(cx, cy, x2, cy);
    
  }
  
  if (cy < y1) return dist_p_p(cx, cy, cx, y1);
  if (cy > y2) return dist_p_p(cx, cy, cx, y2);
  return -1; // inside the rectangle

}

//===================================================================

real8 distance(const Object &e1, const Object &e2)
{
  Object::Shape s1 = e1.get_shape();
  Object::Shape s2 = e2.get_shape();

  // circle - circle
  if (s1 == Object::CIRCLE && s2 == Object::CIRCLE)
    return distance_circle_circle(e1, e2);

  // circle - [hv]line
  if (s1 == Object::CIRCLE && s2 == Object::LINE)
    return distance_circle_line(e1, e2);
  if (s2 == Object::CIRCLE && s1 == Object::LINE)
    return distance_circle_line(e2, e1);

  // circle - rectangle
  if (s1 == Object::CIRCLE && s2 == Object::RECTANGLE)
    return distance_circle_rectangle(e1, e2);
  if (s2 == Object::CIRCLE && s1 == Object::RECTANGLE)
    return distance_circle_rectangle(e2, e1);

  // [hv]line - [hv]line
  if (s1 == Object::LINE && s2 == Object::LINE)
    return distance_line_line(e1, e2);

  // [hv]line - rectangle
  if (s1 == Object::RECTANGLE && s2 == Object::LINE)
    return distance_rectangle_line(e1, e2);
  if (s2 == Object::RECTANGLE && s1 == Object::LINE)
    return distance_rectangle_line(e2, e1);

  // rectangle - rectangle
  if (s1 == Object::RECTANGLE && s2 == Object::RECTANGLE)
    return distance_rectangle_rectangle(e1, e2);

  cout << e1.get_shape() << " " << e2.get_shape() << endl;
  //  cout << e1.bp_name() << " " << e2.bp_name() << endl;
  sint4 x1, y1, x2, y2;
  e1.get_center(x1, y1);
  e2.get_center(x2, y2);
  cout << x1 << " " << y1 << endl;
  cout << x2 << " " << y2 << endl;
  ERR("undefined shape pair");
  return 1;
}

//===================================================================

real8 distance(const Object &e1, Scalar x, Scalar y)
{
  Object::Shape s1 = e1.get_shape();

  // circle - point
  if (s1 == Object::CIRCLE)
    return distance_circle_point(e1, x, y);

  // [hv]line - point
  if (s1 == Object::LINE)
    return distance_line_point(e1, x, y);

  // rectangle - point
  if (s1 == Object::RECTANGLE)
    return distance_rectangle_point(e1, x, y);

  cout << e1.get_shape() << " point" << endl;
  //  cout << e1.bp_name() << " " << e2.bp_name() << endl;
  sint4 x1, y1;
  e1.get_center(x1, y1);
  cout << x1 << " " << y1 << endl;
  cout << x << " " << y << endl;
  ERR("undefined shape pair");
  return 1;
}


} // End of namespace Distance

//===================================================================
//===================================================================
//===================================================================

real8 Game::distance(const Object &e1, const Object &e2)
{
  return Distance::distance(e1, e2);
}

//===================================================================