import matplotlib.pyplot as plt
import matplotlib.patches as patches
from ...general_operations.trigonometry import rotational_matrix,convert_degrees_to_radians
from ...hector.constants import robot_center_x,robot_center_y
from math import pi, cos, sin
[docs]class rectangle:
# initializing a rectangle shape with the respective parameters
def __init__(self,center,length,width,orientation):
self.center = center
self.length = length
self.width = width
self.orientation = orientation
# fixing plotting view for Robot file coordinates
self.view_y = self.center[0]
self.view_x = - self.center[1]
# self.view_orientation = self.orientation# - 90
# calculating four corners of the rectangular shape
[docs] def calculate_4corners(self):
rotation_matrix = rotational_matrix(convert_degrees_to_radians(self.orientation))
self.corner1 = (self.center[0] - rotation_matrix[0][0] * self.width / 2 - rotation_matrix[1][0] * self.length / 2, \
self.center[1] - rotation_matrix[0][1] * self.width / 2 - rotation_matrix[1][1] * self.length / 2)
self.corner2 = (self.center[0] + rotation_matrix[0][0] * self.width / 2 - rotation_matrix[1][0] * self.length / 2, \
self.center[1] + rotation_matrix[0][1] * self.width / 2 - rotation_matrix[1][1] * self.length / 2)
self.corner3 = (self.center[0] + rotation_matrix[0][0] * self.width / 2 + rotation_matrix[1][0] * self.length / 2, \
self.center[1] + rotation_matrix[0][1] * self.width / 2 + rotation_matrix[1][1] * self.length / 2)
self.corner4 = (self.center[0] - rotation_matrix[0][0] * self.width / 2 + rotation_matrix[1][0] * self.length / 2, \
self.center[1] - rotation_matrix[0][1] * self.width / 2 + rotation_matrix[1][1] * self.length / 2)
return [self.corner1,self.corner2,self.corner3,self.corner4]
# plotting the rectangle on respective figures
[docs] def draw_rectangle(self,colour,figurenum, ax):
rectangle.calculate_4corners(self)
if figurenum == 1:
ax.plot([self.corner1[0], self.corner2[0]], [self.corner1[1], self.corner2[1]], colour,alpha = 0.5,zorder=3)
ax.plot([self.corner2[0], self.corner3[0]], [self.corner2[1], self.corner3[1]], colour,alpha = 0.5,zorder=3)
ax.plot([self.corner3[0], self.corner4[0]], [self.corner3[1], self.corner4[1]], colour,alpha = 0.5,zorder=3)
ax.plot([self.corner4[0], self.corner1[0]], [self.corner4[1], self.corner1[1]], colour,alpha = 0.5,zorder=3)
ax.axis('scaled')
################################################################
if figurenum == 2:
ax.plot([self.corner1[0] + robot_center_x, self.corner2[0] + robot_center_x], [self.corner1[1] + robot_center_y, self.corner2[1] + robot_center_y], colour)
ax.plot([self.corner2[0] + robot_center_x, self.corner3[0] + robot_center_x], [self.corner2[1] + robot_center_y, self.corner3[1] + robot_center_y], colour)
ax.plot([self.corner3[0] + robot_center_x, self.corner4[0] + robot_center_x], [self.corner3[1] + robot_center_y, self.corner4[1] + robot_center_y], colour)
ax.plot([self.corner4[0] + robot_center_x, self.corner1[0] + robot_center_x], [self.corner4[1] + robot_center_y, self.corner1[1] + robot_center_y], colour)
ax.axis('scaled')
################################################################
# plotting the four corners of rectangle on respective figures
[docs] def plot_corners(self):
rectangle.calculate_4corners(self)
plt.figure(1)
plt.plot(self.corner1[0], self.corner1[1], 'xb')
plt.plot(self.corner2[0], self.corner2[1], 'xb')
plt.plot(self.corner3[0], self.corner3[1], 'xb')
plt.plot(self.corner4[0], self.corner4[1], 'xb')
#############################################
plt.figure(2)
plt.plot(self.corner1[0] + robot_center_x, self.corner1[1] + robot_center_y, 'xb')
plt.plot(self.corner2[0] + robot_center_x, self.corner2[1] + robot_center_y, 'xb')
plt.plot(self.corner3[0] + robot_center_x, self.corner3[1] + robot_center_y, 'xb')
plt.plot(self.corner4[0] + robot_center_x, self.corner4[1] + robot_center_y, 'xb')
###############################################
# plotting the corner labels on respective figures
[docs] def label_corners_on_plot(self):
rectangle.calculate_4corners(self)
plt.figure(1)
plt.text(self.corner1[0], self.corner1[1], str(1), fontsize=15)
plt.text(self.corner2[0], self.corner2[1], str(2), fontsize=15)
plt.text(self.corner3[0], self.corner3[1], str(3), fontsize=15)
plt.text(self.corner4[0], self.corner4[1], str(4), fontsize=15)
################################################################
plt.figure(2)
plt.text(self.corner1[0], self.corner1[1], str(1), fontsize=15)
plt.text(self.corner2[0], self.corner2[1], str(2), fontsize=15)
plt.text(self.corner3[0], self.corner3[1], str(3), fontsize=15)
plt.text(self.corner4[0], self.corner4[1], str(4), fontsize=15)
################################################################