def fillTriangle(t, color):
#It divides the triangle in 2 rectangle triangles, rasters the 1st triangle then goes to the second
tYSorted = t[np.argsort(t[:, 1])]#Sort by y value(Bottom first)
a = tYSorted[0]#Now they are sorted lowest Y first
b = tYSorted[1]
c = tYSorted[2]
ab,n_ab = geom.line(a,b)
bc,n_bc = geom.line(b,c)
ca,n_ca = geom.line(a,c)
ab_step = 0
bc_step = 0
ca_step = 0
if((int)(ca[1])!=0): ca_step = ca[0] / (float)(abs(ca[1]))
if((int)(ab[1]) != 0):
ab_step = ab[0] / (float)(abs(ab[1]))
for i in range((int)(b[1]-a[1])+1):#Loops across Y between a and b and draws half of the img
a_ab_step = a[0] + (float)(i) * ab_step
a_ca_step = a[0] + (float)(i) * ca_step
#Swap if true so a_ab_step is always smaller
if (a_ab_step > a_ca_step):
buf = a_ab_step
a_ab_step = a_ca_step
a_ca_step = buf
for j in range((int)(a_ca_step - (a_ab_step - 1)) + 2):
v_int = (j + a_ab_step, i + a[1])
#print("v_int: "+str(v_int)+" ab_step: " + str(ab_step)+" a_ac_step: " + str(a_ac_step) + " a_ab_step: " + str(a_ab_step))
pixels[v_int] = color
#Draws the other half of the triangle
if((int)(bc[1]) != 0):
bc_step = bc[0] / (float)(abs(bc[1]))
for i in range((int)(c[1]-b[1])+1):
b_bc_step = b[0] + ((float)(i) * bc_step)
a_ca_step = a[0] + (float)(i + (b[1]-a[1])) * ca_step
#Swap if true
if (b_bc_step > a_ca_step):
buf = b_bc_step
b_bc_step = a_ca_step
a_ca_step = buf
for j in range((int)((a_ca_step - (b_bc_step - 1)) + 2)):
v_int = (j + b_bc_step, i + b[1])
#print("v_int: "+str(v_int)+" ab_step: " + str(ab_step)+" a_ac_step: " + str(a_ac_step) + " b_ab_step: " + str(b_ab_step))
pixels[v_int] = color
def half_line(corners):
"""Divides a quadrilateral in half.
The argument `corners` is a list of four points (tuples of (x, y)),
representing the corners of a quadrilateral. The list may start
on any of the four corners, but must go around the quadrilateral in
clockwise or counter-clockwise order; skipping around is not allowed.
The function returns a line (a list of two points) that joins the
midpoints of two oposite sides of the quadrilateral defined by the
four passed-in corners. Arbitrarily, the bisected sides are the
one joining corner 0 and corner 1, and the one joining corner 2 and
corner 3."""
c = center(corners)
# `d` is the perspective vanishing point for the two sides that
# we're *not* bisecting.
d = intersection(line(corners[0], corners[3]),
line(corners[1], corners[2]))
if not d:
# No vanishing point found: the two sides must be parallel.
# So just use one of the sides as the direction. This adds
# the vector from corner[3] to corner[0] to the center point we
# computed above.
d = (c[0] + corners[0][0] - corners[3][0],
c[1] + corners[0][1] - corners[3][1])
l = line(c, d)
p1 = intersection(l, line(corners[0], corners[1]))
p2 = intersection(l, line(corners[2], corners[3]))
return (p1, p2)
def cast_ray(self, column, angle):
maxlen = length = self.map.size * self.map.SCALE
x, y = cos(angle), sin(angle)
for l in range(length):
_x = self.pose.x + self.SIZE / 2 + x * l / self.map.SCALE
_y = self.pose.y + self.SIZE / 2 + y * l / self.map.SCALE
if self.map.inspect(_x, _y) == Map.WALL:
length = l
break
geometry.line(
column * self.column_thickness,
self.map.size * self.map.SCALE / 2 - 6000 / length / 2,
6000 / length,
pi / 2,
darken((0.75, 0.22, 0.16, 1), length * 8 / maxlen),
self.column_thickness
)
def find_possible_intersections(self, points, detector):
possible_intersections_straight = []
all_points = []
# for every 2 points in row
for i in range(len(points) - 1):
# solving for stright sensor
A = points[i]
B = points[i + 1]
xA = np.around(A[0], 2)
xB = np.around(B[0], 2)
yA = np.around(A[1], 2)
yB = np.around(B[1], 2)
my_x = np.around(self.position.x, 2)
my_y = np.around(self.position.y, 2)
detector_x = np.around(detector.x, 2)
detector_y = np.around(detector.y, 2)
x_straight, y_straight = line_intersection(
line(A, B), line((my_x, my_y), [detector_x, detector_y]))
if x_straight == "lala":
continue
x_straight = np.around(x_straight, 2)
y_straight = np.around(y_straight, 2)
all_points.append((x_straight, y_straight))
if (xA < x_straight < xB or xA > x_straight > xB or (xA == x_straight == xB and (yA <= y_straight <= yB or yA > y_straight > yB)))\
and (my_x < x_straight < detector_x or my_x > x_straight > detector_x or (my_x == x_straight == detector_x and (my_y < y_straight < detector_y or my_y >= y_straight >= detector_y))):
possible_intersections_straight.append(
(x_straight, y_straight))
if not possible_intersections_straight:
pass #(f"WARN all_points = {all_points}")
return possible_intersections_straight
def transform(
wheel1: Point,
wheel2: Point,
tail: Point,
scene_center: Point,
):
"""
:return: (
robot_position, # position against scene center;
angle, # angle against scene center;
distance, # distance to scene center;
)
"""
robot_center = midpoint(wheel1, wheel2)
# finds the tail point's prime and its projection line - the main one
tail_prime = two_points_90(wheel1, robot_center)
intersection_line = line(wheel1, wheel2)
if not pts_same_side(tail, tail_prime, intersection_line):
tail_prime = two_points_90(wheel2, robot_center)
main_projection_line = line(tail, tail_prime)
# finds center line's prime
center_line = line(scene_center, robot_center)
side_line = line(tail, wheel1)
side_intersection = intersection(center_line, side_line)
if side_intersection:
side_line_prime = line(tail_prime, wheel1)
else:
side_line = line(tail, wheel2)
side_intersection = intersection(center_line, side_line)
side_line_prime = line(tail_prime, wheel2)
# noinspection PyTypeChecker
side_intersection_projection_line = parallel_line(main_projection_line, side_intersection)
side_intersection_prime = intersection(side_line_prime, side_intersection_projection_line)
center_line_prime = line(robot_center, side_intersection_prime)
# computes position, angle and distance
center_line_projection = parallel_line(main_projection_line, scene_center)
center_prime = intersection(center_line_projection, center_line_prime)
dist = distance(center_prime, robot_center) / distance(robot_center, tail_prime)
robot_position = robot_center - center_prime
angle = math.degrees(normalize_angle(
three_pts_angle(tail_prime, robot_center, center_prime) - math.pi))
return Object(robot_position, angle, (dist * ROBOT_UNIT))
def half_line(corners):
# TODO what is this?
c = center(corners)
d = intersection(line(corners[0], corners[3]), line(corners[1], corners[2]))
if d:
l = line(c, d)
else:
l = line(c, (c[0] + corners[0][0] - corners[3][0],
c[1] + corners[0][1] - corners[3][1]))
p1 = intersection(l, line(corners[0], corners[1]))
p2 = intersection(l, line(corners[2], corners[3]))
return (p1, p2)
def center(corners):
"""Given a list of four corner points, return the center of the square."""
return intersection(line(corners[0], corners[2]),
line(corners[1], corners[3]))
def _lines(corners, n):
# TODO what is this?
if n == 0:
x = half_line(corners)
return (_lines([corners[0], x[0], x[1], corners[3]], 1) + [x] +
_lines([x[0], corners[1], corners[2], x[1]], 1))
else:
x = half_line(corners)
c = intersection(line(x[0], corners[2]), line(corners[1], corners[3]))
d = intersection(line(corners[0], corners[3]), line(corners[1], corners[2]))
if d:
l = (intersection(line(corners[0], corners[1]), line(c, d)),
intersection(line(corners[2], corners[3]), line(c, d)))
else:
lx = line(c, (c[0] + corners[0][0] - corners[3][0],
c[1] + corners[0][1] - corners[3][1]))
l = (intersection(line(corners[0], corners[1]), lx),
intersection(line(corners[2], corners[3]), lx))
l2 = half_line([corners[0], l[0], l[1], corners[3]])
if n == 1:
return ([l, l2] + _lines([l[0], l2[0], l2[1], l[1]], 2)
+ _lines([corners[0], l2[0], l2[1], corners[3]], 2)
+ _lines([l[0], corners[1], corners[2], l[1]], 2))
if n == 2:
return [l, l2]
def _lines(corners, n):
# `mid_line` is a line that joins the midpoints of two oposite sides
# of the quadrilateral defined by the four passed-in corners.
mid_line = half_line(corners)
# TODO what is this?
if n == 0:
# This recurses to look at the part of the quadrilateral on
# *one* side of `mid_line`. Returns all lines on that side,
# not including `mid_line` but including the other edge.
l0 = _lines([corners[0], mid_line[0], mid_line[1], corners[3]], 1)
# This is just the mid-line.
l1 = [mid_line]
# This recurses to look at the part of the quadrilateral on the
# *other* side of `mid_line`. Returns all lines on that side,
# not including `mid_line` but including the other edge.
l2 = _lines([mid_line[0], corners[1], corners[2], mid_line[1]], 1)
return (l0 + l1 + l2)
else:
c = intersection(line(mid_line[0], corners[2]),
line(corners[1], corners[3]))
d = intersection(line(corners[0], corners[3]),
line(corners[1], corners[2]))
if d:
l = (intersection(line(corners[0], corners[1]), line(c, d)),
intersection(line(corners[2], corners[3]), line(c, d)))
else:
lx = line(c, (c[0] + corners[0][0] - corners[3][0],
c[1] + corners[0][1] - corners[3][1]))
l = (intersection(line(corners[0], corners[1]), lx),
intersection(line(corners[2], corners[3]), lx))
l2 = half_line([corners[0], l[0], l[1], corners[3]])
if n == 1:
return ([l, l2] + _lines([l[0], l2[0], l2[1], l[1]], 2) +
_lines([corners[0], l2[0], l2[1], corners[3]], 2) +
_lines([l[0], corners[1], corners[2], l[1]], 2))
if n == 2:
return [l, l2]
import geometry as g
import physics as p
import pygame as pg
import pid
import rospy
import math as m
from geometry_msgs.msg import Pose, Twist
from sbsim.msg import goalmsg
import controller as c
from std_msgs.msg import Int32
from std_msgs.msg import Float64
from sbsim.msg import dribble
import random as rnd
import time
if __name__ == '__main__':
a = g.point(1, 1)
b = g.point(3, 3)
c = g.point(1, 0)
d = g.point(0, 1)
l1 = g.line(type=1, d1=a, d2=b)
l2 = g.line(type=1, d1=c, d2=d)
X = g.linters(l1, l2)
print X.x, X.y
w = g.point(4, 0)
z = g.point(0, 4)
l3 = g.perpbisector(c, d)
l4 = g.line(1, w, z)
Y = g.linters(l3, l4)
print Y.x, Y.y
