rtcGeometry2D.h

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.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 03/28/2008
 * modified ...: $Date: 2009-08-25 17:32:44 -0700 (Tue, 25 Aug 2009) $
 * changed by .: $Author: benjaminpitzer $
 * revision ...: $Revision: 893 $
 */
#ifndef GEOMETRY2D_H
#define GEOMETRY2D_H

//== INCLUDES ==================================================================
#include <rtc/rtcVec2.h>

namespace rtc {
  // euclidean distance
  float dist(const Vec2f &a, const Vec2f &b);
  // squared euclidean distance
  float dist2(const Vec2f &a, const Vec2f &b);
  // Calculates the closest point to x on a line segmente a-->b
  void dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d, Vec2f &cp);
  // Calculates the closest point to x on a line segmente a-->b
  float dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b);
  // Calculates the projected distance from x on a line segmente a-->b
  float proj_dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b);
  // Calculates the closest point to x on a line segmente a-->b
  // if the distance to closest point is smaller than d2 (squared distance), both
  // d2 and cp will be overwritten
  bool closer_on_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d2, Vec2f &cp);
  // 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);
  // 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);
  // 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);
  // Which of the four edges is point P outside of?
  long bevel_1d(const Vec2f &p);
  // Which of the four edge lines is point P outside of?
  long bevel_2d(const Vec2f &p);
  // 2D linear interpolation
  Vec2f lerp(float t, const Vec2f &a, const Vec2f &b);
  // 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);
  // 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);
  // 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);
  // 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);
  // 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);
  // 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);
  // 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);

  // calculate barycentric coordinates of the point p on triangle t1 t2 t3
  template <class T> inline void rtc_bary(const Vec2<T>& p, const Vec2<T>& t1, const Vec2<T>& t2, const Vec2<T>& t3, T &b1, T &b2, T &b3)
  {
    const T v10 = t1[0]-t3[0];
    const T v11 = t1[1]-t3[1];
    const T v20 = t2[0]-t3[0];
    const T v21 = t2[1]-t3[1];
    // ignore z
    const T d = T(1.0) / (v10*v21-v11*v20);
    const T x0 = (p[0]-t3[0]);
    const T x1 = (p[1]-t3[1]);
    b1 = (x0*v21 - x1*v20) * d;
    b2 = (v10*x1 - v11*x0) * d;
    b3 = T(1.0) - b1 - b2;
  }

  // true if poin p is inside the triangle t1 t2 t3
  template <class T> inline bool rtc_point_inside_triangle(const Vec2<T> &p, const Vec2<T>& t1, const Vec2<T>& t2, const Vec2<T>& t3)
  {
    T b1,b2,b3;
    // calculate barycentric coordinates
    rtc_bary(p, t1, t2, t3, b1, b2, b3);
    // all non-negative, the point is within the triangle
    if (b1 >= T(0) && b2 >= T(0) && b3 >= T(0))
      return true;
    else
      return false;
  }

  // Calculates the euclidian distance from a point to a another point b
  template <class T> inline T rtc_distance_point_to_point(const Vec2<T> &a, const Vec2<T> &b)
  {
    return (a-b).norm();
  }

  // Calculates the distance from a point to x to the closest point on a line segment a-->b
  template <class T> inline T rtc_distance_point_to_line(const Vec2<T> &x, const Vec2<T> &a, const Vec2<T> &b)
  {
    Vec2<T> ba(b[0]-a[0], b[1]-a[1]);
    Vec2<T> xa(x[0]-a[0], x[1]-a[1]);

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

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

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

  // Calculates the projected distance from a point to x on a line segment a-->b
  template <class T> inline T rtc_distance_point_to_line_projected(const Vec2<T> &x, const Vec2<T> &a, const Vec2<T> &b)
  {
    Vec2<T> v(b[1]-a[1], a[0]-b[0]);
    v.normalize();
    Vec2<T> xa(x[0]-a[0], x[1]-a[1]);
    return rtc_abs(v.dot(xa));
  }

  // calculates the area of an triangle (t1 t2 t3)
  template <class T> inline T  rtc_triangle_area(const Vec2<T>& t1, const Vec2<T>& t2, const Vec2<T>& t3)
  {
    return rtc_abs(rtc_triangle_area_signed(t1,t2,t3));
  }

  // calculates the signed area of an triangle (t1 t2 t3)
  template <class T> inline T rtc_triangle_area_signed(const Vec2<T>& t1, const Vec2<T>& t2, const Vec2<T>& t3)
  {
    const T a = (t2(0)-t1(0))*(t3(1)-t1(1))-(t3(0)-t1(0))*(t2(1)-t1(1));
    return T(0.5)*a;
  }

//==============================================================================
} // namespace rtc
//==============================================================================
#endif