1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
/**
* @file Geometry.cpp
*
* @author <a href=mailto:goehring@informatik.hu-berlin.de>Daniel Goehring</a>
* @author <a href=mailto:mellmann@informatik.hu-berlin.de>Heinrich Mellmann</a>
*
* Implementation of the class Geometry
*/

#include "Geometry.h"

double Geometry::angleTo(const Pose2D& from, const Vector2d& to)
{
  return (Pose2D(to) - from).translation.angle();
}

double Geometry::distanceTo(const Pose2D& from, const Vector2d& to)
{
  return (Pose2D(to) - from).translation.abs();
}

Vector2d Geometry::vectorTo(const Pose2D& from, const Vector2d& to)
{
  return (Pose2D(to) - from).translation;
}


Vector2d Geometry::fieldCoordToRelative(const Pose2D& robotPose, const Vector2d& fieldCoord)
{
  // this is the same as robotPose.invert()*fieldCoord or robotPose/fieldCoord
  return (fieldCoord - robotPose.translation).rotate(-robotPose.rotation);
}


Vector2d Geometry::relativeToFieldCoord(const Pose2D& robotPose, const Vector2d& relativeCoord)
{
  return robotPose * relativeCoord;
}


bool Geometry::getIntersectionPointsOfLineAndRectangle(
  const Vector2i& bottomLeft, 
  const Vector2i& topRight,
  const Math::Line& line,
  Vector2i& point1, 
  Vector2i& point2)
{
  int foundPoints=0;
  Vector2d point[2];
  if (line.getDirection().x!=0)
  {
    double y1=line.getBase().y+(bottomLeft.x-line.getBase().x)*line.getDirection().y/line.getDirection().x;
    if ((y1>=bottomLeft.y)&&(y1<=topRight.y))
    {
      point[foundPoints].x=(double) bottomLeft.x;
      point[foundPoints++].y=y1;
    }
    double y2=line.getBase().y+(topRight.x-line.getBase().x)*line.getDirection().y/line.getDirection().x;
    if ((y2>=bottomLeft.y)&&(y2<=topRight.y))
    {
      point[foundPoints].x=(double) topRight.x;
      point[foundPoints++].y=y2;
    }
  }
  if (line.getDirection().y!=0)
  {
    double x1=line.getBase().x+(bottomLeft.y-line.getBase().y)*line.getDirection().x/line.getDirection().y;
    if ((x1>=bottomLeft.x)&&(x1<=topRight.x)&&(foundPoints<2))
    {
      point[foundPoints].x=x1;
      point[foundPoints].y=(double) bottomLeft.y;
      if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
      {
        foundPoints++;
      }
    }
    double x2=line.getBase().x+(topRight.y-line.getBase().y)*line.getDirection().x/line.getDirection().y;
    if ((x2>=bottomLeft.x)&&(x2<=topRight.x)&&(foundPoints<2))
    {
      point[foundPoints].x=x2;
      point[foundPoints].y=(double) topRight.y;
      if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
      {
        foundPoints++;
      }
    }
  }
  switch (foundPoints)
  {
  case 1:
    point1.x=(int)point[0].x;
    point2.x=point1.x;
    point1.y=(int)point[0].y;
    point2.y=point1.y;
    foundPoints++;<--- Variable 'foundPoints' is modified but its new value is never used.
    return true;
  case 2:
    if ((point[1]-point[0])*line.getDirection() > 0)
    {
      point1.x=(int)point[0].x;
      point1.y=(int)point[0].y;
      point2.x=(int)point[1].x;
      point2.y=(int)point[1].y;
    }
    else
    {
      point1.x=(int)point[1].x;
      point1.y=(int)point[1].y;
      point2.x=(int)point[0].x;
      point2.y=(int)point[0].y;
    }
    return true;
  default:
    return false;
  }
}//end getIntersectionPointsOfLineAndRectangle