Changeset 81:6d5259597360 in rrlib_geometry


Ignore:
Timestamp:
17.02.2017 15:49:18 (3 years ago)
Author:
Max Reichardt <max.reichardt@…>
Branch:
14.08
Phase:
public
Message:

Adds intersection method for line segments with bounding box - together with a unit test with a focus on numeric stability

Files:
1 added
3 edited

Legend:

Unmodified
Added
Removed
  • make.xml

    r57 r81  
    3838    </sources> 
    3939  </testprogram> 
     40   
     41  <program name="geometry"> 
     42    <sources> 
     43      tests/geometry.cpp 
     44    </sources> 
     45  </program> 
    4046 
    4147</targets> 
  • tLineSegment.h

    r60 r81  
    117117  virtual tLineSegment &Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation); 
    118118 
     119  /*! 
     120   * Calculates intersection of this line segment with bounding box. 
     121   * 
     122   * \param bounding_box Bounding box for intersection 
     123   * \return 'first' contains whether line intersects bounding box; 'second' the intersection (part of line_segment; has the same direction as this line_segment) 
     124   */ 
     125  std::pair<bool, tLineSegment> GetIntersection(typename tShape::tBoundingBox &bounding_box) const; 
     126  using tLine::GetIntersection; 
     127 
    119128//---------------------------------------------------------------------- 
    120129// Private fields and methods 
  • tLineSegment.hpp

    r60 r81  
    113113 
    114114//---------------------------------------------------------------------- 
     115// tLineSegment GetIntersection 
     116//---------------------------------------------------------------------- 
     117template <size_t Tdimension, typename TElement> 
     118std::pair<bool, tLineSegment<Tdimension, TElement>> tLineSegment<Tdimension, TElement>::GetIntersection(typename tShape::tBoundingBox &bounding_box) const 
     119{ 
     120  typedef typename tShape::tPoint tPoint; 
     121  typedef std::pair<double, tPoint> tIntersection;                     // Intersection point (first is relative distance to line_segment.Begin() with range: 0-1) 
     122  std::array<std::pair<double, tPoint>, Tdimension * 2> intersections; // Intersection points (cannot be more than two - corners, however, may be added multiple times) 
     123  size_t intersection_count = 0;                                       // Number of intersections 
     124  typedef std::pair<uint, tLineSegment<Tdimension, TElement>> tResult; 
     125  typedef rrlib::math::tVector<Tdimension, TElement> tVector; 
     126 
     127  // Are begin/end points already in bounding box? 
     128  if (bounding_box.Contains(Begin())) 
     129  { 
     130    intersections[intersection_count] = tIntersection(0, Begin()); 
     131    intersection_count++; 
     132  } 
     133  if (bounding_box.Contains(End())) 
     134  { 
     135    intersections[intersection_count] = tIntersection(1, End()); 
     136    intersection_count++; 
     137  } 
     138  if (intersection_count == 2) 
     139  { 
     140    return tResult(true, *this); 
     141  } 
     142 
     143  // Check whether/where line segments cross sides of bounding box 
     144  tVector diff_vector = End() - Begin(); 
     145 
     146  // To avoid numeric instabilities/asymmetries, bounding box is centered in zero 
     147  tVector box_dimension = bounding_box.Max() - bounding_box.Min(); 
     148  tVector box_center = (bounding_box.Max() + bounding_box.Min()) * 0.5; 
     149  tBoundingBox<Tdimension, TElement> centered_box; 
     150  centered_box.Add(tPoint(box_dimension * 0.5)); 
     151  centered_box.Add(tPoint(-box_dimension * 0.5)); 
     152  tPoint begin_relative_to_box(Begin() - box_center); 
     153  tPoint end_relative_to_box(End() - box_center); 
     154  tPoint interpolated; 
     155  for (size_t i = 0; i < Tdimension; i++) 
     156  { 
     157    // min side of dimension 
     158    if ((begin_relative_to_box[i] < centered_box.Min()[i] && end_relative_to_box[i] >= centered_box.Min()[i]) || 
     159        (end_relative_to_box[i] < centered_box.Min()[i] && begin_relative_to_box[i] >= centered_box.Min()[i])) 
     160    { 
     161      double distance_begin = std::fabs(centered_box.Min()[i] - begin_relative_to_box[i]); 
     162      double distance_end = std::fabs(centered_box.Min()[i] - end_relative_to_box[i]); 
     163      double relative_begin_distance = distance_begin / (distance_begin + distance_end); 
     164      if (distance_begin <= distance_end) 
     165      { 
     166        interpolated = begin_relative_to_box + (diff_vector * relative_begin_distance); 
     167      } 
     168      else 
     169      { 
     170        double relative_end_distance = distance_end / (distance_begin + distance_end); 
     171        interpolated = end_relative_to_box - (diff_vector * relative_end_distance); 
     172      } 
     173      interpolated[i] = centered_box.Min()[i]; // eliminate any numeric errors in this dimension at least 
     174      if (centered_box.Contains(interpolated)) 
     175      { 
     176        tPoint p = interpolated + box_center; 
     177        p[i] = bounding_box.Min()[i]; 
     178        intersections[intersection_count] = tIntersection(relative_begin_distance, p); 
     179        intersection_count++; 
     180      } 
     181    } 
     182    // max side of dimension 
     183    if ((begin_relative_to_box[i] > centered_box.Max()[i] && end_relative_to_box[i] <= centered_box.Max()[i]) || 
     184        (end_relative_to_box[i] > centered_box.Max()[i] && begin_relative_to_box[i] <= centered_box.Max()[i])) 
     185    { 
     186      double distance_begin = std::fabs(centered_box.Max()[i] - begin_relative_to_box[i]); 
     187      double distance_end = std::fabs(centered_box.Max()[i] - end_relative_to_box[i]); 
     188      double relative_begin_distance = distance_begin / (distance_begin + distance_end); 
     189      if (distance_begin <= distance_end) 
     190      { 
     191        interpolated = begin_relative_to_box + (diff_vector * relative_begin_distance); 
     192      } 
     193      else 
     194      { 
     195        double relative_end_distance = distance_end / (distance_begin + distance_end); 
     196        interpolated = end_relative_to_box - (diff_vector * relative_end_distance); 
     197      } 
     198      interpolated[i] = centered_box.Max()[i]; // eliminate any numeric errors in this dimension at least 
     199      if (centered_box.Contains(interpolated)) 
     200      { 
     201        tPoint p = interpolated + box_center; 
     202        p[i] = bounding_box.Max()[i]; 
     203        intersections[intersection_count] = tIntersection(relative_begin_distance, p); 
     204        intersection_count++; 
     205      } 
     206    } 
     207  } 
     208 
     209  if (!intersection_count) 
     210  { 
     211    return tResult(false, tLineSegment<Tdimension, TElement>()); 
     212  } 
     213 
     214  // Prepare result 
     215  tIntersection intersection_begin = intersections[0]; 
     216  tIntersection intersection_end = intersections[0]; 
     217  for (size_t i = 1; i < intersection_count; i++) 
     218  { 
     219    const tIntersection& intersection = intersections[i]; 
     220    if (intersection.first < intersection_begin.first) 
     221    { 
     222      intersection_begin = intersection; 
     223    } 
     224    if (intersection.first > intersection_end.first) 
     225    { 
     226      intersection_end = intersection; 
     227    } 
     228  } 
     229 
     230  return tResult(false, tLineSegment<Tdimension, TElement>(intersection_begin.second, intersection_end.second)); 
     231} 
     232 
     233//---------------------------------------------------------------------- 
    115234// tLineSegment Translate 
    116235//---------------------------------------------------------------------- 
Note: See TracChangeset for help on using the changeset viewer.