Screen Coordinates [ x, y ] to Pan and Tilt angles - python

I have a pan-tilt system with an airbrush on top, the pressure is quite strong so that I can place the robot at a distance of at least 1.5 meters. I currently have normalized coordinates XY that I can visualize on my camera like this . Now I want to translate those coordinates to a real canvas and allow the pan-tilt to point towards them and eventually spray.
The two servos have 0 to 180 degrees but the airbrush is positioned on top of the tilt at 90. So if we consider that the pan and tilt it's at 90 the airbrush points perpendicularly to the real canvas.
I followed along with this answer but could not actually produce any valuable outcome.
The code below is something I come up with but is not working.
for x, y in points:
screenX = (x * w) + rect[0] #- differencesqrx
screenY = (y * h) + rect[1] #- differencesqry
cv2.circle(frame,(int(screenXCenter),int(screenYCenter)),2,(255, 255, 0),2)
cv2.circle(frame,(int(screenX),int(screenY)),2,(255, 45, 250),2)
canvasXpoint = (x * canvasW) + canvasW
canvasYpoin = (y * canvasH) + canvasH
servoXcentralp = canvasW / 2
servoYcentralp = canvasH / 2
dx = servoXcentralp - canvasXpoint
dy = servoYcentralp - canvasYpoin
pan = np.arctan((distanceZ/dx)) * 180/np.pi
tilt = np.arctan(distanceZ/dy) * 180/np.pi

Related

Angle Reflexion for bouncing ball in a circle

I am making a game with some bouncing elements IN a circle (I use pygame) ,
My elements have 2 attributes , one for the angle and one for the speed
Here is how elements moves :
mvx = math.sin(self.angle) * self.speed
mvy = -math.cos(self.angle) * self.speed
self.x += mvx
self.y += mvy
My problem is this : I know the angle at the top (99.6°) , I have the collision point (x and y ) , but I'm unable to find the angle at the bottom(42.27°)
Does someones can make a relation between the first angle and the second ?
Picture is better ...
I recommend do calculate the reflection vector to the incident vector on the circular surface.
In the following formula N is the normal vector of the circle, I is the incident vector (the current direction vector of the bouncing ball) and R is the reflection vector (outgoing direction vector of the bouncing ball):
R = I - 2.0 * dot(N, I) * N.
Use the pygame.math.Vector2.
To calculate the normal vector, you' ve to know the "hit" point (dvx, dvy) and the center point of the circle (cptx, cpty):
circN = (pygame.math.Vector2(cptx - px, cpty - py)).normalize()
Calculate the reflection:
vecR = vecI - 2 * circN.dot(vecI) * circN
The new angle can be calculated by math.atan2(y, x):
self.angle = math.atan2(vecR[1], vecR[0])
Code listing:
import math
import pygame
px = [...] # x coordinate of the "hit" point on the circle
py = [...] # y coordinate of the "hit" point on the circle
cptx = [...] # x coordinate of the center point of the circle
cpty = [...] # y coordinate of the center point of the circle
circN = (pygame.math.Vector2(cptx - px, cpty - py)).normalize()
vecI = pygame.math.Vector2(math.cos(self.angle), math.sin(self.angle))
vecR = vecI - 2 * circN.dot(vecI) * circN
self.angle = math.pi + math.atan2(vecR[1], vecR[0])
The inner angles of a triangle need to sum up to 180°. Also, the angle 99.96° is supplementary to the triangle's angle next to it (calling it by A), i.e. 99.96° + A = 180° so A = 180° - 99.96°. Calling of B = 42.27° the bottom angle. And for the last angle C, we can use that it is opposed by the vertex with the other angle that is equal to 2 * 28.85 = 57.7°.
Then:
A + B + C = 180°
180° - 99.96° + 42.27° + 2 * 28.85° = 180°
180° - 99.96° + 42.27° + 2 * 28.85° = 180°
-99.96° + 42.27° + 2 * 28.85° = 0°
42.27° + 2 * 28.85° = 99.96°
B + C = Top angle
P.S.: I know that the values are not exactly equal, but it must be because of the decimal places rounding

Drawing arrowheads which follow the direction of the line in PyGame

In Pygame, how can I calculate the coordinates for the three points of the head of an arrow, given a start point and an end point for the arrow, so that the arrowhead points in the same direction as the line?
def __draw_arrow(self, screen, colour, start, end):
start = self.__coordinate_lookup[start]
end = self.__coordinate_lookup[end]
dX = start[0] - end[0]
dY = -(start[1] - end[1])
print m.degrees(m.atan(dX/dY)) + 360
pygame.draw.line(screen,colour,start,end,2)
I've tried playing around with angles and with the gradient of the line, the fact the Y coordinates increase downwards instead of upwards is throwing me off and I would really appreciate a nudge in the right direction.
I denote start and end coordinates as startX, startY, endX, endY
dX = endX - startX
dY = endY - startY
//vector length
Len = Sqrt(dX* dX + dY * dY) //use Hypot if available
//normalized direction vector components
udX = dX / Len
udY = dY / Len
//perpendicular vector
perpX = -udY
perpY = udX
//points forming arrowhead
//with length L and half-width H
arrowend = (end)
leftX = endX - L * udX + H * perpX
leftY = endY - L * udY + H * perpY
rightX = endX - L * udX - H * perpX
rightY = endY - L * udY - H * perpY
This should work:
def draw_arrow(screen, colour, start, end):
pygame.draw.line(screen,colour,start,end,2)
rotation = math.degrees(math.atan2(start[1]-end[1], end[0]-start[0]))+90
pygame.draw.polygon(screen, (255, 0, 0), ((end[0]+20*math.sin(math.radians(rotation)), end[1]+20*math.cos(math.radians(rotation))), (end[0]+20*math.sin(math.radians(rotation-120)), end[1]+20*math.cos(math.radians(rotation-120))), (end[0]+20*math.sin(math.radians(rotation+120)), end[1]+20*math.cos(math.radians(rotation+120)))))
Sorry for the poorly organized code. But as you said, the coordinates starting in the top-left do require some math to be flipped. Also, if you want to change the triangle from being equalatiral to something else you just have to change the rotation +/- 120 in line 4, or the 20* for a different radius.
Hope this helps :)
Vladimir's answer is great! For anyone visiting in the future, here's the function with control over every aspect of the arrow:
def arrow(screen, lcolor, tricolor, start, end, trirad):
pg.draw.line(screen,lcolor,start,end,2)
rotation = math.degrees(math.atan2(start[1]-end[1], end[0]-start[0]))+90
pg.draw.polygon(screen, tricolor, ((end[0]+trirad*math.sin(math.radians(rotation)), end[1]+trirad*math.cos(math.radians(rotation))), (end[0]+trirad*math.sin(math.radians(rotation-120)), end[1]+trirad*math.cos(math.radians(rotation-120))), (end[0]+trirad*math.sin(math.radians(rotation+120)), end[1]+trirad*math.cos(math.radians(rotation+120)))))'

Setting diagonal mirroring via Jython (user to set points)

Trying to get my my head around this program we need to create
What is needed is as per the notes:
create a function named
arbitraryMirror() that allows the user to place a mirror at an arbitrary angle, causing an intersect and therefore mirror the image.
This will need to be done on either a square or rectangle picture.
As per the pics below, this is the Output of what is required.
Output
I know how to mirror a pic (as shown below) with a square image, but i cannot work out if this can also be done with a rectangle image?
Cross
I had a look at a method of using y=mx+b but it seems overcomplicated?
Maybe there is some coordinate geometry i need? Or algebra?
Any help would be greatly appreciated!
The key formulas are (python):
# (x0, y0) and (x1, y1) are two points on the mirroring line
# dx, dy, L is the vector and lenght
dx, dy = x1 - x0, y1 - y0
L = (dx**2 + dy**2) ** 0.5
# Tangent (tx, ty) and normal (nx, ny) basis unit vectors
tx, ty = dx / L, dy / L
nx, ny = -dy / L, dx / L
# For each pixel
for y in range(h):
for x in range(w):
# Map to tangent/normal space
n = (x+0.5 - x0)*nx + (y+0.5 - y0)*ny
t = (x+0.5 - x0)*tx + (y+0.5 - y0)*ty
# If we're in the positive half-space
if n >= 0:
# Compute mirrored point in XY space
# (negate the normal component)
xx = int(x0 + t*tx - n*nx + 0.5)
yy = int(y0 + t*ty - n*ny + 0.5)
# If valid copy to destination
if 0 <= xx < w and 0 <= yy < h:
img[y][x] = img[yy][xx]
Here you can see an example of the results
The top-left red corner are pixels that would be mirroring pixels outside of the original image and they're left untouched by the above code.

Pygame draw anti-aliased thick line

I used to draw lines (given some start and end points) at pygame like this: pygame.draw.line(window, color_L1, X0, X1, 2), where 2 was defining the thickness of the line.
As, anti-aliasing is not supported by .draw, so I moved to .gfxdraw and pygame.gfxdraw.line(window, X0[0], X0[1], X1[0], X1[1], color_L1).
However, this does not allow me to define the thickness of the line. How could I have thickness and anti-aliasing together?
After many trials and errors, the optimal way to do it would be the following:
First, we define the center point of the shape given the X0_{x,y} start and X1_{x,y} end points of the line:
center_L1 = (X0+X1) / 2.
Then find the slope (angle) of the line:
length = 10 # Total length of line
thickness = 2
angle = math.atan2(X0[1] - X1[1], X0[0] - X1[0])
Using the slope and the shape parameters you can calculate the following coordinates of the box ends:
UL = (center_L1[0] + (length/2.) * cos(angle) - (thickness/2.) * sin(angle),
center_L1[1] + (thickness/2.) * cos(angle) + (length/2.) * sin(angle))
UR = (center_L1[0] - (length/2.) * cos(angle) - (thickness/2.) * sin(angle),
center_L1[1] + (thickness/2.) * cos(angle) - (length/2.) * sin(angle))
BL = (center_L1[0] + (length/2.) * cos(angle) + (thickness/2.) * sin(angle),
center_L1[1] - (thickness/2.) * cos(angle) + (length/2.) * sin(angle))
BR = (center_L1[0] - (length/2.) * cos(angle) + (thickness/2.) * sin(angle),
center_L1[1] - (thickness/2.) * cos(angle) - (length/2.) * sin(angle))
Using the computed coordinates, we draw an unfilled anti-aliased polygon (thanks to #martineau) and then fill it as suggested in the documentation of pygame's gfxdraw module for drawing shapes.
pygame.gfxdraw.aapolygon(window, (UL, UR, BR, BL), color_L1)
pygame.gfxdraw.filled_polygon(window, (UL, UR, BR, BL), color_L1)
I would suggest a filled rectangle, as shown here: https://www.pygame.org/docs/ref/gfxdraw.html#pygame.gfxdraw.rectangle.
Your code would look something like:
thickLine = pygame.gfxdraw.rectangle(surface, rect, color)
and then remember to fill the surface. This is along the lines of:
thickLine.fill()
You can also do a bit of a hack with the pygame.draw.aalines() function by drawing copies of the line +/- 1-N pixels around the original line (yes, this isn't super efficient, but it works in a pinch). For example, assuming we have a list of line segments (self._segments) to draw and with a width (self._LINE_WIDTH):
for segment in self._segments:
if len(segment) > 2:
for i in xrange(self._LINE_WIDTH):
pygame.draw.aalines(self._display, self._LINE_COLOR, False,
((x,y+i) for x,y in segment))
pygame.draw.aalines(self._display, self._LINE_COLOR, False,
((x,y-i) for x,y in segment))
pygame.draw.aalines(self._display, self._LINE_COLOR, False,
((x+i,y) for x,y in segment))
pygame.draw.aalines(self._display, self._LINE_COLOR, False,
((x-i,y) for x,y in segment))
Your answer gets the job done but I think this would be a better/more readable way to do it. This is piggybacking off of your answer though so credit to you.
from math import atan2, cos, degrees, radians, sin
def Move(rotation, steps, position):
"""Return coordinate position of an amount of steps in a direction."""
xPosition = cos(radians(rotation)) * steps + position[0]
yPosition = sin(radians(rotation)) * steps + position[1]
return (xPosition, yPosition)
def DrawThickLine(surface, point1, point2, thickness, color):
angle = degrees(atan2(point1[1] - point2[1], point1[0] - point2[0]))
vertices = list()
vertices.append(Move(angle-90, thickness, point1))
vertices.append(Move(angle+90, thickness, point1))
vertices.append(Move(angle+90, thickness, point2))
vertices.append(Move(angle-90, thickness, point2))
pygame.gfxdraw.aapolygon(surface, vertices, color)
pygame.gfxdraw.filled_polygon(surface, vertices, color)
Keep in mind that this treats the thickness more as a radius than a diameter. If you want it to act more like a diameter you can divide each instance of the variable by 2.
So anyway, this calculates all the points of the rectangle and fills it in. It does this by going to each point and calculating the two adjacent points by turning 90 degrees and moving forward.
Here is a slightly faster and shorter solution:
def drawLineWidth(surface, color, p1, p2, width):
# delta vector
d = (p2[0] - p1[0], p2[1] - p1[1])
# distance between the points
dis = math.hypot(*d)
# normalized vector
n = (d[0]/dis, d[1]/dis)
# perpendicular vector
p = (-n[1], n[0])
# scaled perpendicular vector (vector from p1 & p2 to the polygon's points)
sp = (p[0]*width/2, p[1]*width/2)
# points
p1_1 = (p1[0] - sp[0], p1[1] - sp[1])
p1_2 = (p1[0] + sp[0], p1[1] + sp[1])
p2_1 = (p2[0] - sp[0], p2[1] - sp[1])
p2_2 = (p2[0] + sp[0], p2[1] + sp[1])
# draw the polygon
pygame.gfxdraw.aapolygon(surface, (p1_1, p1_2, p2_2, p2_1), color)
pygame.gfxdraw.filled_polygon(surface, (p1_1, p1_2, p2_2, p2_1), color)
The polygon's points here are calculated using vector math rather than trigonometry, which is much less costly.
If efficiency is of the essence, it's easy to further optimize this code - for instance the first few lines can be condensed to:
d = (p2[0] - p1[0], p2[1] - p1[1])
dis = math.hypot(*d)
sp = (-d[1]*width/(2*dis), d[0]*width/(2*dis))
Hope this helps someone.
This is a slightly longer code, but maybe will help someone.
It uses vectors and create a stroke on each side of the line connecting two points.
def make_vector(pointA,pointB): #vector between two points
x1,y1,x2,y2 = pointA[0],pointA[1],pointB[0],pointB[1]
x,y = x2-x1,y2-y1
return x,y
def normalize_vector(vector): #sel explanatory
x, y = vector[0], vector[1]
u = math.sqrt(x ** 2 + y ** 2)
try:
return x / u, y / u
except:
return 0,0
def perp_vectorCL(vector): #creates a vector perpendicular to the first clockwise
x, y = vector[0], vector[1]
return y, -x
def perp_vectorCC(vector): #creates a vector perpendicular to the first counterclockwise
x, y = vector[0], vector[1]
return -y, x
def add_thickness(point,vector,thickness): #offsets a point by the vector
return point[0] + vector[0] * thickness, point[1] + vector[1] * thickness
def draw_line(surface,fill,thickness, start,end): #all draw instructions
x,y = make_vector(start,end)
x,y = normalize_vector((x,y))
sx1,sy1 = add_thickness(start,perp_vectorCC((x,y)),thickness//2)
ex1,ey1 = add_thickness(end,perp_vectorCC((x,y)),thickness//2)
pygame.gfxdraw.aapolygon(surface,(start,end,(ex1,ey1),(sx1,sy1)),fill)
pygame.gfxdraw.filled_polygon(surface, (start, end, (ex1, ey1), (sx1, sy1)), fill)
sx2, sy2 = add_thickness(start, perp_vectorCL((x, y)), thickness // 2)
ex2, ey2 = add_thickness(end, perp_vectorCL((x, y)), thickness//2)
pygame.gfxdraw.aapolygon(surface, (start, end, (ex2, ey2), (sx2, sy2)), fill)
pygame.gfxdraw.filled_polygon(surface, (start, end, (ex2, ey2), (sx2, sy2)), fill)

Rotate line around center point given two vertices

I've been trying to rotate a bunch of lines by 90 degrees (that together form a polyline). Each line contains two vertices, say (x1, y1) and (x2, y2). What I'm currently trying to do is rotate around the center point of the line, given center points |x1 - x2| and |y1 - y2|. For some reason (I'm not very mathematically savvy) I can't get the lines to rotate correctly.
Could someone verify that the math here is correct? I'm thinking that it could be correct, however, when I set the line's vertices to the new rotated vertices, the next line may not be grabbing the new (x2, y2) vertex from the previous line, causing the lines to rotate incorrectly.
Here's what I've written:
def rotate_lines(self, deg=-90):
# Convert from degrees to radians
theta = math.radians(deg)
for pl in self.polylines:
self.curr_pl = pl
for line in pl.lines:
# Get the vertices of the line
# (px, py) = first vertex
# (ox, oy) = second vertex
px, ox = line.get_xdata()
py, oy = line.get_ydata()
# Get the center of the line
cx = math.fabs(px-ox)
cy = math.fabs(py-oy)
# Rotate line around center point
p1x = cx - ((px-cx) * math.cos(theta)) - ((py-cy) * math.sin(theta))
p1y = cy - ((px-cx) * math.sin(theta)) + ((py-cy) * math.cos(theta))
p2x = cx - ((ox-cx) * math.cos(theta)) - ((oy-cy) * math.sin(theta))
p2y = cy - ((ox-cx) * math.sin(theta)) + ((oy-cy) * math.cos(theta))
self.curr_pl.set_line(line, [p1x, p2x], [p1y, p2y])
The coordinates of the center point (cx,cy) of a line segment between points (x1,y1) and (x2,y2) are:
cx = (x1 + x2) / 2
cy = (y1 + y2) / 2
In other words it's just the average, or arithmetic mean, of the two pairs of x and y coordinate values.
For a multi-segmented line, or polyline, its logical center point's x and y coordinates are just the corresponding average of x and y values of all the points. An average is just the sum of the values divided by the number of them.
The general formulas to rotate a 2D point (x,y) θ radians around the origin (0,0) are:
x′ = x * cos(θ) - y * sin(θ)
y′ = x * sin(θ) + y * cos(θ)
To perform a rotation about a different center (cx, cy), the x and y values of the point need to be adjusted by first subtracting the coordinate of the desired center of rotation from the point's coordinate, which has the effect of moving (known in geometry as translating) it is expressed mathematically like this:
tx = x - cx
ty = y - cy
then rotating this intermediate point by the angle desired, and finally adding the x and y values of the point of rotation back to the x and y of each coordinate. In geometric terms, it's the following sequence of operations:  Tʀᴀɴsʟᴀᴛᴇ ─► Rᴏᴛᴀᴛᴇ ─► Uɴᴛʀᴀɴsʟᴀᴛᴇ.
This concept can be extended to allow rotating a whole polyline about any arbitrary point—such as its own logical center—by just applying the math described to each point of each line segment within it.
To simplify implementation of this computation, the numerical result of all three sets of calculations can be combined and expressed with a pair of mathematical formulas which perform them all simultaneously. So a new point (x′,y′) can be obtained by rotating an existing point (x,y), θ radians around the point (cx, cy) by using:
x′ = ( (x - cx) * cos(θ) + (y - cy) * sin(θ) ) + cx
y′ = ( -(x - cx) * sin(θ) + (y - cy) * cos(θ) ) + cy
Incorporating this mathematical/geometrical concept into your function produces the following:
from math import sin, cos, radians
def rotate_lines(self, deg=-90):
""" Rotate self.polylines the given angle about their centers. """
theta = radians(deg) # Convert angle from degrees to radians
cosang, sinang = cos(theta), sin(theta)
for pl in self.polylines:
# Find logical center (avg x and avg y) of entire polyline
n = len(pl.lines)*2 # Total number of points in polyline
cx = sum(sum(line.get_xdata()) for line in pl.lines) / n
cy = sum(sum(line.get_ydata()) for line in pl.lines) / n
for line in pl.lines:
# Retrieve vertices of the line
x1, x2 = line.get_xdata()
y1, y2 = line.get_ydata()
# Rotate each around whole polyline's center point
tx1, ty1 = x1-cx, y1-cy
p1x = ( tx1*cosang + ty1*sinang) + cx
p1y = (-tx1*sinang + ty1*cosang) + cy
tx2, ty2 = x2-cx, y2-cy
p2x = ( tx2*cosang + ty2*sinang) + cx
p2y = (-tx2*sinang + ty2*cosang) + cy
# Replace vertices with updated values
pl.set_line(line, [p1x, p2x], [p1y, p2y])
Your center point is going to be:
centerX = (x2 - x1) / 2 + x1
centerY = (y2 - y1) / 2 + y1
because you take half the length (x2 - x1) / 2 and add it to where your line starts to get to the middle.
As an exercise, take two lines:
line1 = (0, 0) -> (5, 5)
then: |x1 - x2| = 5, when the center x value is at 2.5.
line2 = (2, 2) -> (7, 7)
then: |x1 - x2| = 5, which can't be right because that's the center for
the line that's parallel to it but shifted downwards and to the left

Categories