rtcGeometry2D.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2008
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: PUMA: Probablistic Unsupervised Model Acquisition
 * file .......: Geometry2D.cpp
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 07/25/2006
 * modified ...: $Date: 2009-08-25 17:32:44 -0700 (Tue, 25 Aug 2009) $
 * changed by .: $Author: benjaminpitzer $
 * revision ...: $Revision: 893 $
 */

//== INCLUDES ==================================================================
#include <cassert>
#include "rtc/rtcGeometry2D.h"
#include <rtc/rtcRotation2D.h>

//== NAMESPACES ================================================================
namespace rtc {

//== CONSTANTS =================================================================
#define INSIDE  false
#define OUTSIDE true

float dist2(const Vec2f &a, const Vec2f &b)
{
  return (a-b).normSqr();
}

float dist(const Vec2f &a, const Vec2f &b)
{
  return (a-b).norm();
}

void dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d, Vec2f &cp)
{
  Vec2f ba(b[0]-a[0], b[1]-a[1]);
  Vec2f xa(x[0]-a[0], x[1]-a[1]);

  float xa_ba = xa.dot(ba);
  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    float nd = dist(x,a);
    cp = a;
    d = nd;
    return;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba/ba.normSqr();
  if (fact >= 1.0) {
    float nd = dist(x,b);
    cp = b;
    d = nd;
    return;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  float nd = xa.normSqr() - xa_ba*fact;
  d = sqrt(nd);
  cp[0] = a[0] + fact * ba[0];
  cp[1] = a[1] + fact * ba[1];
}

float dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b)
{
  Vec2f ba(b[0]-a[0], b[1]-a[1]);
  Vec2f xa(x[0]-a[0], x[1]-a[1]);

  float xa_ba = xa.dot(ba);
  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    return dist(x,a);
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba/ba.normSqr();
  if (fact >= 1.0) {
    return dist(x,b);
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  return sqrt(xa.normSqr() - xa_ba*fact);
}

float proj_dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b)
{
  Vec2f v(b[1]-a[1], a[0]-b[0]);
  v.normalize();
  Vec2f xa(x[0]-a[0], x[1]-a[1]);
  return abs(v.dot(xa));
}


bool closer_on_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d2, Vec2f &cp)
{
  Vec2f ba(b[0]-a[0], b[1]-a[1]);
  Vec2f xa(x[0]-a[0], x[1]-a[1]);

  float xa_ba = xa.dot(ba);
  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    float nd = dist2(x, a);
    if (nd < d2) {
      cp = a;
      d2 = nd;
      return true;
    }
    return false;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba/ba.normSqr();
  if (fact >= 1.0) {
    float nd = dist2(x,b);
    if (nd < d2) {
      cp = b;
      d2 = nd;
      return true;
    }
    return false;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  float nd = xa.normSqr() - xa_ba*fact;
  if (nd<0)
    nd=0.0f;
  if (nd < d2) {
    d2 = nd;
    cp[0] = a[0] + fact * ba[0];
    cp[1] = a[1] + fact * ba[1];
    return true;
  }
  return false;
}

// is the circle centered at b with radius r
// fully within the rectangle centered at bc, with radius br?
bool circle_within_bounds(const Vec2f &b, float r, const Vec2f &bc, float br)
{
  r -= br;
  if ((b[0]  - bc[0] <= r) ||
      (bc[0] - b[0]  <= r) ||
      (b[1]  - bc[1] <= r) ||
      (bc[1] - b[1]  <= r)) return false;
  return true;
}

// is the circle centered at b with radius r
// fully within the rectangle centered from min to max?
bool circle_within_bounds(const Vec2f &b, float r, const Vec2f &min, const Vec2f &max)
{
  if ((b[0] - min[0] <= r) ||
    (max[0] - b[0] <= r) ||
    (b[1] - min[1] <= r) ||
    (max[1] - b[1] <= r)) return false;
  return true;
}

// does the circle centered at b, with radius r,
// intersect the rectangle centered at bc, with radius br?
bool bounds_overlap_circle(const Vec2f &b, float r, const Vec2f &bc, float br)
{
  float r2 = rtc_sqr(r);
  float R_max_x,R_max_y;
  float R_min_x,R_min_y;

  /* Translate coordinates, placing the circle at the origin. */
  R_max_x = bc[0]+br-b[0];  R_max_y = bc[1]+br-b[1];
  R_min_x = bc[0]-br-b[0];  R_min_y = bc[1]-br-b[1];

  if (R_max_x < 0)      /* R to left of circle center */
    if ( R_max_y < 0)     /* R in lower left corner */
        return ((R_max_x * R_max_x + R_max_y * R_max_y) < r2);
    else if ( R_min_y > 0)  /* R in upper left corner */
        return ((R_max_x * R_max_x +  R_min_y *  R_min_y) < r2);
    else          /* R due West of circle */
        return(abs(R_max_x) < r);
   else if ( R_min_x > 0)    /* R to right of circle center */
      if ( R_max_y < 0)   /* R in lower right corner */
          return (( R_min_x *  R_min_x + R_max_y * R_max_y) < r2);
    else if ( R_min_y > 0)    /* R in upper right corner */
        return (( R_min_x *  R_min_x +  R_min_y *  R_min_y) < r2);
    else        /* R due East of circle */
        return ( R_min_x < r);
   else        /* R on circle vertical centerline */
      if ( R_max_y < 0)   /* R due South of circle */
        return (abs(R_max_y) < r);
    else if ( R_min_y > 0)    /* R due North of circle */
        return ( R_min_y < r);
    else        /* R contains circle centerpoint */
        return(true);
}

//bool bounds_overlap_circle(const Vec2f &b, float r, const Vec2f &bc, float br)
//  {
//    float sum = 0.0, tmp;
//    if        ((tmp = bc[0]-br - b[0]) > 0.0) {
//      if (tmp>r) return false; sum += tmp*tmp;
//    } else if ((tmp = b[0] - (bc[0]+br)) > 0.0) {
//      if (tmp>r) return false; sum += tmp*tmp;
//    }
//    if        ((tmp = bc[1]-br - b[1]) > 0.0) {
//      if (tmp>r) return false; sum += tmp*tmp;
//    } else if ((tmp = b[1] - (bc[1]+br)) > 0.0) {
//      if (tmp>r) return false; sum += tmp*tmp;
//    }
//    return (sum < r*r);
//  }

 /* Which of the four edges is point P outside of? */
long bevel_1d(const Vec2f &p)
{
  long outcode = 0;
  if (p[0] >  .5) outcode |= 0x01;
  if (p[0] < -.5) outcode |= 0x02;
  if (p[1] >  .5) outcode |= 0x04;
  if (p[1] < -.5) outcode |= 0x08;
  return outcode;
}

/* Which of the four corner lines is point P outside of? */
long bevel_2d(const Vec2f &p)
{
  long outcode = 0;
  if ( p[0] + p[1] > 1.0) outcode |= 0x01;
  if ( p[0] - p[1] > 1.0) outcode |= 0x02;
  if (-p[0] + p[1] > 1.0) outcode |= 0x04;
  if (-p[0] - p[1] > 1.0) outcode |= 0x08;
  return outcode;
}

// 2D linear interpolation
Vec2f lerp(float t, const Vec2f &a, const Vec2f &b)
{
  float v[2];
  float u = 1.0 - t;
  v[0]=u*a[0]+t*b[0];
  v[1]=u*a[1]+t*b[1];
  return Vec2f(v[0],v[1]);
}

/* Test the point "alpha" of the way from P1 to P2 */
/* See if it is on a edge of the rectangle         */
/* Consider only faces in "mask"                   */
bool point_on_edge(const Vec2f &p1, const Vec2f &p2, float alpha, long mask)
{
  Vec2f line_point;
  line_point = lerp(alpha, p1, p2);
  long l = bevel_1d(line_point) & mask;
  return (l==0?INSIDE:OUTSIDE);
}

/* Compute intersection of P1 --> P2 line segment with edge lines  */
/* Then test intersection point to see if it is on cube face       */
/* Consider only face planes in "outcode_diff"                     */
/* Note: Zero bits in "outcode_diff" means edge is outside of      */
bool segment_on_edge(const Vec2f &p1, const Vec2f &p2, long outcode_diff)
{
  if (0x01 & outcode_diff)
    if (point_on_edge(p1,p2,( .5-p1[0])/(p2[0]-p1[0]),0xE) == INSIDE) return INSIDE;
  if (0x02 & outcode_diff)
    if (point_on_edge(p1,p2,(-.5-p1[0])/(p2[0]-p1[0]),0xD) == INSIDE) return INSIDE;
  if (0x04 & outcode_diff)
    if (point_on_edge(p1,p2,( .5-p1[1])/(p2[1]-p1[1]),0xB) == INSIDE) return INSIDE;
  if (0x08 & outcode_diff)
    if (point_on_edge(p1,p2,(-.5-p1[1])/(p2[1]-p1[1]),0x7) == INSIDE) return INSIDE;
  return OUTSIDE;
}


// the main routine
// true if line t1,t2 is outside a rectangle
// centered at c with length of a side s,
// false if the line intersects rectangle
//
bool line_outside_of_rect(const Vec2f &c, float s, const Vec2f &t1, const Vec2f &t2)
{

  long v1_test,v2_test;

  // First compare both points tih all four rectangle edges
  // If any point is inside the rectangle, return immediately!
  Vec2f v1((t1[0]-c[0])/s, (t1[1]-c[1])/s);
  if (!(v1_test = bevel_1d(v1))) return INSIDE;
  Vec2f v2((t2[0]-c[0])/s, (t2[1]-c[1])/s);
  if (!(v2_test = bevel_1d(v2))) return INSIDE;
  // If both points were outside of one or more edges,
  // return immediately with a trivial rejection!
  if ((v1_test & v2_test) != 0) return OUTSIDE;

  // Now do the same trivial rejection test for the four corner lines
  v1_test |= bevel_2d(v1) << 8;
  v2_test |= bevel_2d(v2) << 8;
  if ((v1_test & v2_test) != 0) return OUTSIDE;

  /* If point 1 and 2, as a pair, cannot be trivially rejected    */
  /* by the above tests, then see if the v1-->v2 segment          */
  /* intersects the rectangle.                                    */
  /* Pass to the intersection algorithm the "OR" of the outcode   */
  /* bits, so that only those rectangle edges which are spanned   */
  /* by each triangle edge need be tested.                        */
  if (segment_on_edge(v1,v2,v1_test|v2_test) == INSIDE) return INSIDE;

//  if (point_on_edge(v1,v2,( .5-v1[0])/(v2[0]-v1[0])) == INSIDE) return INSIDE;
//  if (point_on_edge(v1,v2,(-.5-v1[0])/(v2[0]-v1[0])) == INSIDE) return INSIDE;
//  if (point_on_edge(v1,v2,( .5-v1[1])/(v2[1]-v1[1])) == INSIDE) return INSIDE;
//  if (point_on_edge(v1,v2,(-.5-v1[1])/(v2[1]-v1[1])) == INSIDE) return INSIDE;

//  /* No line touched the rectangle                                   */
//  /* We're done...there was no intersection.                         */
  return OUTSIDE;
}

// true if line p is outside a rectangle
// centered at c with length of a side s,
// false if the line intersects rectangle
bool point_outside_of_rect(const Vec2f &c, float s, const Vec2f &p)
{
  Vec2f v1((p[0]-c[0])/s, (p[1]-c[1])/s);
  if (!bevel_1d(v1)) return INSIDE;
  return OUTSIDE;
}

// true if line p is outside a rectangle
// centered at c with length of x side xlen and length of y side ylen,
// rotated rot_angle degree,
// false if the line intersects rectangle
bool point_outside_of_rect(const Vec2f &c, float xlen, float ylen, float rot_angle, const Vec2f &p)
{
  Vec2f v(p[0]-c[0], p[1]-c[1]);
  Rotation2D<float> r(rot_angle);
  v=r*v;
  v.x[0]/=xlen;v.x[1]/=ylen;
  if (!bevel_1d(v)) return INSIDE;
  return OUTSIDE;
}

// true if rectangle 1 intersects rectangle 2
// c == center
// theta == angle
// w == width
// l == length
bool rect_rect_X(const Vec2f &c1, float theta1, float w1, float l1,
                 const Vec2f &c2, float theta2, float w2, float l2) {

  float x,y,sintheta2,costheta2;
  float tx, ty, scale_x, scale_y,sintheta1,costheta1;
  assert(l1);assert(w1);
  assert(l2);assert(w2);

  sintheta2 = sin(theta2);
  costheta2 = cos(theta2);

  // calculate the coordinates of all four corners of rectangle 2
  float rect2_x[4];
  float rect2_y[4];

  x = -l2/2.0; y = -w2/2.0;
  rect2_x[0] = costheta2*(x) - sintheta2*(y) + c2[0];
  rect2_y[0] = sintheta2*(x) + costheta2*(y) + c2[1];

  x = l2/2.0; y = -w2/2.0;
  rect2_x[1] = costheta2*(x) - sintheta2*(y) + c2[0];
  rect2_y[1] = sintheta2*(x) + costheta2*(y) + c2[1];

  x = l2/2.0; y = w2/2.0;
  rect2_x[2] = costheta2*(x) - sintheta2*(y) + c2[0];
  rect2_y[2] = sintheta2*(x) + costheta2*(y) + c2[1];

  x = -l2/2.0; y = w2/2.0;
  rect2_x[3] = costheta2*(x) - sintheta2*(y) + c2[0];
  rect2_y[3] = sintheta2*(x) + costheta2*(y) + c2[1];

  // normalize all coordinates so that rectangle 1 ist axis aligned and of length 1
  tx = c1[0];
  ty = c1[1];
  scale_x = l1;
  scale_y = w1;
  sintheta1 = sin(-theta1);
  costheta1 = cos(-theta1);

  for(int i=0;i<4;++i) {
    // transform coordinates of rectangle 2
    x = rect2_x[i] - tx;
    y = rect2_y[i] - ty;
    rect2_x[i] = (costheta1*(x) - sintheta1*(y))/scale_x;
    rect2_y[i] = (sintheta1*(x) + costheta1*(y))/scale_y;
  }

  Vec2f t0(rect2_x[0], rect2_y[0]);
  Vec2f t1(rect2_x[1], rect2_y[1]);
  Vec2f t2(rect2_x[2], rect2_y[2]);
  Vec2f t3(rect2_x[3], rect2_y[3]);

  // perform line rectangle intersection for all four edges of rectangle 2
  Vec2f c((float)0);
  bool boutside;

  boutside = line_outside_of_rect(c, 1, t0, t1);
  if(!boutside) return true;

  boutside = line_outside_of_rect(c, 1, t1, t2);
  if(!boutside) return true;

  boutside = line_outside_of_rect(c, 1, t2, t3);
  if(!boutside) return true;

  boutside = line_outside_of_rect(c, 1, t3, t0);
  if(!boutside) return true;

  return false;
}

// true if line intersects rectangle
// line t1-->t2
// c == center
// theta == angle
// w == width
// l == length
bool line_rect_X(const Vec2f &t1, const Vec2f &t2,
                 const Vec2f &c, float theta, float w, float l) {
  float x,y,sintheta,costheta;
  float tx, ty, scale_x, scale_y;
  Vec2f t1n,t2n;
  assert(l);assert(w);

  // normalize all coordinates so that rectangle 1 ist axis aligned and of length 1
  tx = c[0];
  ty = c[1];
  scale_x = l;
  scale_y = w;
  sintheta = sin(-theta);
  costheta = cos(-theta);

  // transform coordinates of t1
  x = t1[0] - tx;
  y = t1[1] - ty;
  t1n[0] = (costheta*(x) - sintheta*(y)) / scale_x;
  t1n[1] = (sintheta*(x) + costheta*(y)) / scale_y;

  // transform coordinates of t2
  x = t2[0] - tx;
  y = t2[1] - ty;
  t2n[0] = (costheta*(x) - sintheta*(y)) / scale_x;
  t2n[1] = (sintheta*(x) + costheta*(y)) / scale_y;

  // perform line rectangle intersection for all four edges of rectangle 2
  Vec2f cn((float)0);
  bool boutside = line_outside_of_rect(cn, 1, t1n, t2n);
  if(!boutside) return true;
  return false;
}

//==============================================================================
} // namespace rtc
//==============================================================================