Changeset 17:3daa58ced492 in rrlib_geometry


Ignore:
Timestamp:
11.10.2011 18:33:24 (8 years ago)
Author:
Tobias Föhst <foehst@…>
Branch:
default
Parents:
16:a9e6bf94cfe4 (diff), 14:83a174d09c05 (diff)
Note: this is a merge changeset, the changes displayed below correspond to the merge itself.
Use the (diff) links above to see all the changes relative to each parent.
Phase:
public
Message:

Merged with changes from rrlab

Files:
22 edited

Legend:

Unmodified
Added
Removed
  • curves/tBezierCurve.h

    r14 r17  
    119119                              const tLine<Tdimension, TElement> &line) const; 
    120120 
     121  const typename tShape::tPoint GetClosestPoint(const typename tShape::tPoint &reference_point) const; 
     122 
    121123  virtual tBezierCurve &Translate(const math::tVector<Tdimension, TElement> &translation); 
    122124  virtual tBezierCurve &Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation); 
  • curves/tBezierCurve.h

    r16 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • curves/tBezierCurve.hpp

    r14 r17  
    294294 
    295295//---------------------------------------------------------------------- 
     296// tBezierCurve GetClosestPoint 
     297//---------------------------------------------------------------------- 
     298template <size_t Tdimension, typename TElement, unsigned int Tdegree> 
     299const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const 
     300{ 
     301  tSubdivision subdivision(this->GetSubdivision()); 
     302 
     303  tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.GetControlPoint(0), subdivision.first.GetControlPoint(Tdegree)); 
     304  tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.GetControlPoint(0), subdivision.second.GetControlPoint(Tdegree)); 
     305 
     306  typename tShape::tPoint first_candidate = first_segment_base_line.GetClosestPoint(reference_point); 
     307  typename tShape::tPoint second_candidate = second_segment_base_line.GetClosestPoint(reference_point); 
     308 
     309  double squared_distance_to_first_candidate = (reference_point - first_candidate).SquaredLength(); 
     310  double squared_distance_to_second_candidate = (reference_point - second_candidate).SquaredLength(); 
     311 
     312  if (squared_distance_to_first_candidate < squared_distance_to_second_candidate) 
     313  { 
     314    if (subdivision.first.GetTwist() < 1E-6) 
     315    { 
     316      return first_candidate; 
     317    } 
     318    return subdivision.first.GetClosestPoint(reference_point); 
     319  } 
     320 
     321  if (subdivision.second.GetTwist() < 1E-6) 
     322  { 
     323    return second_candidate; 
     324  } 
     325  return subdivision.second.GetClosestPoint(reference_point); 
     326} 
     327 
     328//---------------------------------------------------------------------- 
    296329// tBezierCurve Translate 
    297330//---------------------------------------------------------------------- 
  • curves/tBezierCurve.hpp

    r16 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • curves/tSplineCurve.h

    r14 r17  
    134134                        const tLine<Tdimension, TElement> &line) const; 
    135135 
     136  const typename tShape::tPoint GetClosestPoint(const typename tShape::tPoint &reference_point) const; 
     137 
    136138  virtual tSplineCurve &Translate(const math::tVector<Tdimension, TElement> &translation); 
    137139  virtual tSplineCurve &Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation); 
  • curves/tSplineCurve.h

    r16 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • curves/tSplineCurve.hpp

    r14 r17  
    224224 
    225225//---------------------------------------------------------------------- 
     226// tSplineCurve GetClosestPoint 
     227//---------------------------------------------------------------------- 
     228template <size_t Tdimension, typename TElement, unsigned int Tdegree> 
     229const typename tShape<Tdimension, TElement>::tPoint tSplineCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const 
     230{ 
     231  typename tShape::tPoint closest_point = this->GetBezierCurveForSegment(0).GetClosestPoint(reference_point); 
     232  double min_distance = (closest_point - reference_point).Length(); 
     233 
     234  for (size_t i = 1; i < this->GetNumberOfSegments(); ++i) 
     235  { 
     236    typename tShape::tPoint candidate = this->GetBezierCurveForSegment(i).GetClosestPoint(reference_point); 
     237    double distance = (candidate - reference_point).Length(); 
     238 
     239    if (distance < min_distance) 
     240    { 
     241      min_distance = distance; 
     242      closest_point = candidate; 
     243    } 
     244  } 
     245 
     246  return closest_point; 
     247} 
     248 
     249//---------------------------------------------------------------------- 
    226250// tSplineCurve Translate 
    227251//---------------------------------------------------------------------- 
  • curves/tSplineCurve.hpp

    r16 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tLine.h

    r14 r17  
    102102  const TElement GetDistanceToPoint(const typename tShape::tPoint &point) const; 
    103103 
    104   virtual const typename tShape::tPoint GetNearestPoint(const typename tShape::tPoint &reference_point) const; 
     104  virtual const typename tShape::tPoint GetClosestPoint(const typename tShape::tPoint &reference_point) const; 
    105105 
    106106  const bool GetIntersection(typename tShape::tPoint &intersection_point, const tLine &line) const; 
  • tLine.h

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tLine.hpp

    r14 r17  
    112112const TElement tLine<Tdimension, TElement>::GetDistanceToPoint(const typename tShape::tPoint &point) const 
    113113{ 
    114   return (point - this->GetNearestPoint(point)).Length(); 
    115 } 
    116  
    117 //---------------------------------------------------------------------- 
    118 // tLine GetNearestPoint 
    119 //---------------------------------------------------------------------- 
    120 template <size_t Tdimension, typename TElement> 
    121 const typename tShape<Tdimension, TElement>::tPoint tLine<Tdimension, TElement>::GetNearestPoint(const typename tShape::tPoint &reference_point) const 
     114  return (point - this->GetClosestPoint(point)).Length(); 
     115} 
     116 
     117//---------------------------------------------------------------------- 
     118// tLine GetClosestPoint 
     119//---------------------------------------------------------------------- 
     120template <size_t Tdimension, typename TElement> 
     121const typename tShape<Tdimension, TElement>::tPoint tLine<Tdimension, TElement>::GetClosestPoint(const typename tShape::tPoint &reference_point) const 
    122122{ 
    123123  //          reference_point 
     
    270270void tLine<Tdimension, TElement>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const 
    271271{ 
    272   center_of_gravity = this->GetNearestPoint(tShape::tPoint::Zero()); 
     272  center_of_gravity = this->GetClosestPoint(tShape::tPoint::Zero()); 
    273273} 
    274274 
  • tLine.hpp

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tLineSegment.h

    r14 r17  
    103103  } 
    104104 
    105   const TElement Length() const; 
     105  inline const TElement Length() const 
     106  { 
     107    return (this->Begin() - this->End()).Length(); 
     108  } 
    106109 
    107   virtual const typename tShape::tPoint GetNearestPoint(const typename tShape::tPoint &reference_point) const; 
     110  virtual const typename tShape::tPoint GetClosestPoint(const typename tShape::tPoint &reference_point) const; 
    108111 
    109112  virtual tLineSegment &Translate(const math::tVector<Tdimension, TElement> &translation); 
  • tLineSegment.h

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tLineSegment.hpp

    r14 r17  
    8989 
    9090//---------------------------------------------------------------------- 
    91 // tLineSegment GetNearestPoint 
     91// tLineSegment GetClosestPoint 
    9292//---------------------------------------------------------------------- 
    9393template <size_t Tdimension, typename TElement> 
    94 const typename tShape<Tdimension, TElement>::tPoint tLineSegment<Tdimension, TElement>::GetNearestPoint(const typename tShape::tPoint &reference_point) const 
     94const typename tShape<Tdimension, TElement>::tPoint tLineSegment<Tdimension, TElement>::GetClosestPoint(const typename tShape::tPoint &reference_point) const 
    9595{ 
    9696  math::tVector<Tdimension, TElement> from_begin_to_point(reference_point - this->Begin()); 
     
    103103  } 
    104104 
    105   // angle between direction and vecot end->point larger than 90° 
     105  // angle between direction and vector end->point larger than 90° 
    106106  if (from_end_to_point * this->Direction() > 0) 
    107107  { 
     
    109109  } 
    110110 
    111   return tLine::GetNearestPoint(reference_point); 
     111  return tLine::GetClosestPoint(reference_point); 
    112112} 
    113113 
  • tLineSegment.hpp

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tPlane.h

    r14 r17  
    103103  const TElement GetDistanceToPoint(const typename tShape::tPoint &point) const; 
    104104 
    105   virtual const typename tShape::tPoint GetNearestPoint(const typename tShape::tPoint &reference_point) const; 
     105  virtual const typename tShape::tPoint GetClosestPoint(const typename tShape::tPoint &reference_point) const; 
    106106 
    107107  virtual tPlane &Translate(const math::tVector<Tdimension, TElement> &translation); 
  • tPlane.h

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • tPlane.hpp

    r14 r17  
    122122 
    123123//---------------------------------------------------------------------- 
    124 // tPlane GetNearestPoint 
    125 //---------------------------------------------------------------------- 
    126 template <size_t Tdimension, typename TElement> 
    127 const typename tShape<Tdimension, TElement>::tPoint tPlane<Tdimension, TElement>::GetNearestPoint(const typename tShape::tPoint &reference_point) const 
     124// tPlane GetClosestPoint 
     125//---------------------------------------------------------------------- 
     126template <size_t Tdimension, typename TElement> 
     127const typename tShape<Tdimension, TElement>::tPoint tPlane<Tdimension, TElement>::GetClosestPoint(const typename tShape::tPoint &reference_point) const 
    128128{ 
    129129  return -(this->normal *(reference_point - this->support)) * this->normal + reference_point; 
     
    196196void tPlane<Tdimension, TElement>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const 
    197197{ 
    198   center_of_gravity = this->GetNearestPoint(tShape::tPoint::Zero()); 
     198  center_of_gravity = this->GetClosestPoint(tShape::tPoint::Zero()); 
    199199} 
    200200 
  • tPlane.hpp

    r15 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
  • test/test_geometries.cpp

    r14 r17  
    284284  control_points.push_back(tPoint(0.9, 0.4)); 
    285285 
    286  
    287286  tConcreteSplineCurve spline(control_points); 
    288287 
     
    380379  window.Render(); 
    381380 
    382  
     381  std::cout << std::endl << "=== Closest point to Bezier curve ===" << std::endl; 
     382 
     383  window.Clear(); 
     384  window.SetColor(0); 
     385  DrawBezierCurve(window, bezier_curve); 
     386  window.Render(); 
     387 
     388  for (int i = 0; i < 10; ++i) 
     389  { 
     390    tPoint reference_point(drand48(), drand48()); 
     391    tPoint closest_point(bezier_curve.GetClosestPoint(reference_point)); 
     392 
     393    window.SetColor(3); 
     394    DrawLineSegment(window, tLineSegment(reference_point, closest_point)); 
     395    window.SetColor(1); 
     396    DrawPoint(window, reference_point); 
     397    window.SetColor(2); 
     398    DrawPoint(window, closest_point); 
     399    window.Render(); 
     400  } 
     401 
     402  std::cout << std::endl << "=== Closest point to spline curve ===" << std::endl; 
     403 
     404  window.Clear(); 
     405  window.SetColor(0); 
     406  DrawSplineCurve(window, spline); 
     407  window.Render(); 
     408 
     409  for (int i = 0; i < 10; ++i) 
     410  { 
     411    tPoint reference_point(drand48(), drand48()); 
     412    tPoint closest_point(spline.GetClosestPoint(reference_point)); 
     413 
     414    window.SetColor(3); 
     415    DrawLineSegment(window, tLineSegment(reference_point, closest_point)); 
     416    window.SetColor(1); 
     417    DrawPoint(window, reference_point); 
     418    window.SetColor(2); 
     419    DrawPoint(window, closest_point); 
     420    window.Render(); 
     421  } 
    383422 
    384423  window.Render(); 
  • test/test_geometries.cpp

    r16 r17  
    33// Robotics Research Library 
    44// 
    5 // Copyright (C) AG Robotersysteme TU Kaiserslautern 
     5// Copyright (C) Finroc GbR (finroc.org) 
    66// 
    77// This program is free software; you can redistribute it and/or 
Note: See TracChangeset for help on using the changeset viewer.