PROJET_IAR/test_angle.py at master · lucasschott/PROJET_IAR · GitHub

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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
# -*- coding: utf-8 -*-
"""
Created on Sat Nov 30 13:29:19 2019

@author: Hector
"""


import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np

env_size = 512
view_distance = 200

fig, ax=plt.subplots()

UP = 0
DOWN = 1
LEFT = 2
RIGHT = 3

def test2(point1, point2):
    diffs = point1 - point2
    new_point = np.array([point2[0], point2[1]])

    if (point1[0] > env_size / 2 and diffs[0] > 0 and abs(diffs[0]) > env_size / 2):
        new_point[0] += env_size

    elif point1[0] < env_size / 2 and diffs[0] < 0 and abs(diffs[0]) > env_size / 2:
        new_point[0] -= env_size

    if (point1[1] > env_size / 2 and diffs[1] > 0 and abs(diffs[1]) > env_size / 2):
        new_point[1] += env_size

    elif point1[1] < env_size / 2 and diffs[1] < 0 and abs(diffs[1]) > env_size / 2:
        new_point[1] -= env_size


    return new_point

def distance_torus(point1, point2):
    diffs = np.abs(point1 - point2)

    if diffs[0] > env_size / 2:
        diffs[0] = env_size - diffs[0]

    if diffs[1] > env_size / 2:
        diffs[1] = env_size - diffs[1]

    return np.linalg.norm(diffs)

def test(point1, point2):
    diffs = point1 - point2
    new_point = np.array([point2[0], point2[1]])

    if (point1[0] > env_size / 2 and diffs[0] > env_size / 2):
        new_point[0] += env_size

    elif point1[0] < env_size / 2 and diffs[0] < -1 * env_size / 2:
        new_point[0] -= env_size

    if (point1[1] > env_size / 2 and diffs[1] > env_size / 2):
        new_point[1] += env_size

    elif point1[1] < env_size / 2 and diffs[1] < -1 * env_size / 2:
        new_point[1] -= env_size

    return new_point

def get_new_point(point1, point2):

    flags_1 = [point1[1] > env_size - view_distance,
               point1[1] < view_distance,
               point1[0] < view_distance,
               point1[0] > env_size - view_distance]

    flags_2 = [point2[1] > env_size - view_distance,
               point2[1] < view_distance,
               point2[0] < view_distance,
               point2[0] > env_size - view_distance]

    new_point = [point2[0], point2[1]]

    if flags_1[LEFT] and new_point[0] > env_size - (view_distance - point_1[0]):
        new_point[0] -= env_size

    elif flags_1[RIGHT] and new_point[0] + env_size - point_1[0] < view_distance:
        new_point[0] += env_size

    if flags_1[DOWN] and new_point[1] > env_size - (view_distance - point_1[1]):
        new_point[1] -= env_size

    elif flags_1[UP] and new_point[1] + env_size - point_1[1] < view_distance:
        new_point[1] += env_size

    return np.array(new_point)

point_1 = np.random.rand(2) * env_size
point_2_ori = np.random.rand(2) * env_size
point_2 = get_new_point(point_1, point_2_ori)
point_2 = test(point_1, point_2_ori)

print("torus distance : ", distance_torus(point_1, point_2_ori))

direction_1 = np.random.rand() * 2 * np.pi
direction_2 = 0

def compute_max_view_point(point, view_distance, direction):
    v_x = (point_1[0] + view_distance * np.cos(direction_1))
    v_y = (point_1[1] + view_distance * np.sin(direction_1))

    v_x = min(env_size, max(0, v_x))
    v_y = min(env_size, max(0, v_y))

    return v_x, v_y

def plot_line(point1, point2):
    x = [point1[0], point2[0]]
    y = [point1[1], point2[1]]
    plt.plot(x, y, marker="o")

def points_to_vec(p1, p2):
    return p2 - p1

def get_angle_vec_3(o_to_view, view_to_p, o_to_p):

    """
    b = o_to_view
    a = view_to_p
    c = o_to_p
    """

    b = np.linalg.norm(o_to_view)
    a = np.linalg.norm(view_to_p)
    c = np.linalg.norm(o_to_p)

    cos_angle =  (b ** 2 + c ** 2 - a ** 2) / (2 * c * b)

    orientation = -o_to_view[0] * o_to_p[1] + o_to_view[1] * o_to_p[0]

    print(np.sign(orientation))

    return np.arccos(cos_angle)

max_view_point = compute_max_view_point(point_1, view_distance, direction_1)

ax.plot(point_1[0], point_1[1], "r+")
ax.annotate("Point_1", (point_1[0], point_1[1]))

ax.plot(point_2_ori[0], point_2_ori[1], "r+")
ax.annotate("Point_2_ori", (point_2_ori[0], point_2_ori[1]))

ax.plot(point_2[0], point_2[1], "r+")
ax.annotate("Point_2_repos", (point_2[0], point_2[1]))

ax.plot(max_view_point[0], max_view_point[1], "r+")
ax.annotate("View_1", (max_view_point[0], max_view_point[1]))


plot_line(point_1, max_view_point)
plot_line(point_1, point_2)
plot_line(max_view_point, point_2)


vec1 = points_to_vec(point_1, max_view_point)
vec3 = points_to_vec(point_1, point_2)
vec2 = points_to_vec(max_view_point, point_2)

# Create a Rectangle patch
rect = patches.Rectangle((0,0),env_size,env_size,linewidth=1,edgecolor='r',facecolor='none')

# Add the patch to the Axes
ax.add_patch(rect)

angle = np.degrees(get_angle_vec_3(vec1, vec2, vec3))
angle_reset = angle + 90

print("angle = ", angle)
print("angle reset = ", angle_reset)

print("bin = ", angle_reset // 15)

ax.set_xlim(-100, 600)
ax.set_ylim(-100, 600)

print("distance to original : ", np.linalg.norm(point_1 - point_2_ori))
print("distance to repositioned : ", np.linalg.norm(point_1 - point_2))

plt.show()