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
|