-
Notifications
You must be signed in to change notification settings - Fork 0
/
getdistance.cpp
104 lines (90 loc) · 3.93 KB
/
getdistance.cpp
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
#include "getdistance.h"
GetDistance::GetDistance()
{
}
MDtype GetDistance::getDx(std::shared_ptr<VertexClass> v1, std::shared_ptr<VertexClass> v2 ){
if(fabs(v2->x - v1->x) < 0.0001 && &v1 != &v2)
printf("dx: \n%lf \n%lf \n%lf\n\n", v2->x, v1->x, fabs(v2->x - v1->x));
return v2->x - v1->x;
}
MDtype GetDistance::getDy(std::shared_ptr<VertexClass> v1, std::shared_ptr<VertexClass> v2){
if(fabs(v2->x - v1->x) < 0.0001 && &v1 != &v2)
printf("dy: \n%lf \n%lf \n%lf\n\n", v2->x, v1->x, fabs(v2->x - v1->x));
return v2->y - v1->y;
}
MDtype GetDistance::
getDistanceBetweenVertex(std::shared_ptr<VertexClass> v1, std::shared_ptr<VertexClass> v2){
MDtype dx = getDx(v1, v2);
MDtype dy = getDy(v1, v2);
return sqrt(dx*dx + dy*dy);
}
MDtype GetDistance::getDistanceBetweenEdge(std::shared_ptr<VertexClass> p, std::shared_ptr<EdgeClass> line){
if(fabs(line->B->y - line->A->y) < 0.0001)
printf("Y:\n%lf\n %lf\n\n", line->B->y, line->A->y);
if(fabs(line->B->x - line->A->x) < 0.0001)
printf("X:\n%lf\n %lf\n\n", line->B->x, line->A->x);
if(fabs((line->B->y - line->A->y)*(line->A->x) - (line->A->y)*(line->B->x - line->A->x)) < 0.0001)
printf("C");
MDtype A = -(line->B->y - line->A->y);
MDtype B = line->B->x - line->A->x;
MDtype C = (line->B->y - line->A->y)*(line->A->x) - (line->A->y)*(line->B->x - line->A->x);
MDtype d = fabs(A*p->x + B*p->y + C) / sqrt(A*A + B*B);
if(fabs(A*p->x + B*p->y) < 0.0001 or fabs((A*p->x + B*p->y) + C)< 0.0001)
printf("d\n");
return d;
}
MDtype GetDistance::
whichSide(std::shared_ptr<VertexClass>A, std::shared_ptr<VertexClass> B, std::shared_ptr<VertexClass>p){
if(fabs(A->y*B->x + B->y*p->x) < 0.0001)
printf("which side1\n");
if(fabs(A->y*B->x + B->y*p->x + p->y*A->x) < 0.0001)
printf("which side2\n");
if(fabs(A->y*B->x + B->y*p->x + p->y*A->x - p->y*B->x) < 0.0001)
printf("which side3\n");
if(fabs(A->y*B->x + B->y*p->x + p->y*A->x - p->y*B->x - A->y*p->x) < 0.0001)
printf("which side4\n");
if(fabs(A->y*B->x + B->y*p->x + p->y*A->x - p->y*B->x - A->y*p->x - B->y*A->x) < 0.0001)
printf("which side5: %lf\n",
A->y*B->x + B->y*p->x + p->y*A->x - p->y*B->x - A->y*p->x - B->y*A->x);
return A->y*B->x + B->y*p->x + p->y*A->x - p->y*B->x - A->y*p->x - B->y*A->x;
}
std::shared_ptr<VertexClass> GetDistance::
projectionPointOntoLine(std::shared_ptr<EdgeClass> line, std::shared_ptr<VertexClass>p){
MDtype u =
((p->x - line->A->x)*(line->B->x - line->A->x) +
(p->y - line->A->y)*(line->B->y - line->A->y)) /
((line->A->x - line->B->x)*(line->A->x - line->B->x) +
(line->A->y - line->B->y)*(line->A->y - line->B->y));
if(fabs(p->x - line->A->x) < 0.0001)
printf("projection on line");
if(fabs(line->B->x - line->A->x) < 0.0001)
printf("projection on line");
if(fabs(p->y - line->A->y) < 0.0001)
printf("projection on line");
if(fabs(line->B->y - line->A->y) < 0.0001)
printf("projection on line");
if(fabs(line->B->y - line->A->y) < 0.0001)
printf("projection on line");
if(fabs(((p->x - line->A->x)*(line->B->x - line->A->x) +
(p->y - line->A->y)*(line->B->y - line->A->y))) < 0.0001)
printf("projection on line");
if(fabs(line->A->x - line->B->x) < 0.0001)
printf("projection on line");
if(fabs(line->A->y - line->B->y) < 0.0001)
printf("projection on line");
if(fabs(((line->B->x - line->A->x) * u) + line->A->x) < 0.0001)
printf("projection on line");
if(fabs(((line->B->y - line->A->y) * u) + line->A->y) < 0.0001)
printf("projection on line");
return std::make_shared<VertexClass>(((line->B->x - line->A->x) * u) + line->A->x,
((line->B->y - line->A->y) * u) + line->A->y);
}
bool GetDistance::isBetween(std::shared_ptr<EdgeClass> edge, std::shared_ptr<VertexClass>v){
std::shared_ptr<VertexClass> r = projectionPointOntoLine(edge, v);
if(whichSide(v, r, edge->A) * whichSide(v, r, edge->B) <= 0){
return true;
}
else{
return false;
}
}