I am currently having trouble with an assignment, and I've been trying to pinpoint my error for hours.
I have a circle and a rectangle (both drawn by user clicks). As shown here:
i.stack.imgur (dot) com/4aXIw.png
Using the center point of the rectangle, I want to calculate a new center point (for the rectangle) that lies on the circle. I achieve this by calculating the angle (theta) and using trigonometry to get the new centerpoint.
i.stack.imgur (dot) com/NoCBS.png
However, when I run my program I am not getting the correct results. Shown here:
i.stack.imgur (dot) com/ZG9Ld.png
I've checked a hundred times, and I can't seem to pinpoint where I messed up. I've checked theta, and the resulting degrees is correct. Here is the relevant code, where I suspect I made an error:
theta = math.atan((initialrec_y-click1.y)/(initialrec_x-click1.x))
theta = int(math.degrees(theta))
rec_x = click1.x + radius*math.cos(theta)
rec_y = click1.y + radius*math.sin(theta)
Here is the full code if you would like to run it:
from graphics import *
import math
def main():
#setup window
win = GraphWin("Traveling Rectangle",600,450)
win.setCoords(0,0,600,450)
click1 = win.getMouse()
click2 = win.getMouse()
radius = click2.x - click1.x
circle = Circle(click1, radius)
circle.draw(win)
click3 = win.getMouse()
click4 = win.getMouse()
rect = Rectangle(click3, click4)
rect.draw(win)
rect.setFill("red")
rect.setOutline("red")
#the centerpoint of the initial rectangle
initialrec_x = (click3.x + click4.x) / 2
initialrec_y = (click3.y + click4.y) / 2
#the trig to calculate the point on the circle
theta = math.atan((initialrec_y-click1.y)/(initialrec_x-click1.x))
theta = int(math.degrees(theta))
#the new centerpoint values of x and y
rec_x = click1.x + radius*math.cos(theta)
rec_y = click1.y + radius*math.sin(theta)
main()
Help would be greatly appreciated!
I apologize, the site did not let me post images. The graphics library can be found here: mcsp.wartburg (dot) edu/zelle/python/graphics.py
I am using a different graphics package, but the following works for me:
...
initialrec_y = (click3.y + click4.y) / 2
theta = math.atan2(initialrec_y - click1.y, initialrec_x - click1.x)
rec_x = click1.x + radius * math.cos(theta)
rec_y = click1.y + radius * math.sin(theta)
This uses the atan2 function, which takes note of the signs on the y and x inputs to correctly calculate the quadrant the point is in and return an angle accordingly. Other than that, the only difference from your code is that there is no conversion from radians (which atan2 returns) to degrees. sin and cos want radians.
Related
I have an algorithm question. I am currently working on a script that generates images of an object from various angles inside of Unreal engine and pairs these images with the coordinates of the object. The way it works is that I have the object at the origin, and I generate random spherical coordinates to place my camera at. I then rotate my camera to face the object and do an extra rotation so that the object can lie anywhere in my camera's FOV. I now want to consider my camera as the origin and find the spherical coordinates of the object relative to the graph.
Currently, I am trying to derive the coordinates as in the code below. I start by noting that the radial distance between the object and the camera is the same regardless of which one is the origin. Then, I use the fact that the angles between my camera and my object are determined entirely by the extra rotation at the end of my camera placement. Finally, I try to find a rotation that will orient the object the same way as in the image based on the angular coordinates of the camera (This is done because I want to encode information about points on the object besides the center. For example, I am currently using a 1 meter cube as a placeholder object, and I want to keep track of the coordinates of the corners. I chose to use rotations because I can use them to make a rotation matrix and use it to convert my coordinates). Below is the code I use to do this (the AirSim library is used here, but all you need to know is airsim.Pose() takes in a Euclidean position coordinate and a Quaternion rotation as arguments to position my camera).
PRECISION_ANGLE = 4 # Fractions of a degree used in generating random pitch, roll, and yaw values
PRECISION_METER = 100 # Fractions of a meter used in generating random distance values
RADIUS_MAX = 20 # Maximum distance from the obstacle to be expected
#TODO: Replace minimum distace with a test for detecting if the camera is inside the obstacle
RADIUS_MIN = 3 # Minimum distance from the obstacle to be expected. Set this value large enough so that the camera will not spawn inside the object
# Camera details should match settings.json
IMAGE_HEIGHT = 144
IMAGE_WIDTH = 256
FOV = 90
# TODO: Vertical FOV rounds down for generating random integers. Some pictures will not be created
VERT_FOV = FOV * IMAGE_HEIGHT // IMAGE_WIDTH
def polarToCartesian(r, theta, phi):
return [
r * math.sin(theta) * math.cos(phi),
r * math.sin(theta) * math.sin(phi),
r * math.cos(theta)]
while 1:
# generate a random position for our camera
r = random.randint(RADIUS_MIN * PRECISION_METER, RADIUS_MAX * PRECISION_METER) / PRECISION_METER
phi = random.randint(0, 360 * PRECISION_ANGLE) / PRECISION_ANGLE
theta = random.randint(0, 180 * PRECISION_ANGLE) / PRECISION_ANGLE
# Convert polar coordinates to cartesian for AirSim
pos = polarToCartesian(r, math.radians(theta), math.radians(phi))
# Generate a random offset for the camera angle
pitch = random.randint(0, VERT_FOV * PRECISION_ANGLE) / PRECISION_ANGLE - VERT_FOV / 2
# TODO: Rotating the drone causes the obstacle to be removed from the image because the camera is not square
#roll = random.randint(0, 360 * PRECISION_ANGLE) / PRECISION_ANGLE
roll = 0
yaw = random.randint(0, FOV * PRECISION_ANGLE) / PRECISION_ANGLE - FOV/2
# Calculate coordinates of the center of the obstacle relative to the drone's new position and orientation
obs_r = r
obs_phi = yaw
obs_theta = 90 - pitch
# Convert polar coordinates to cartesian for AirSim
obs_pos = polarToCartesian(obs_r, math.radians(obs_theta), math.radians(obs_phi))
# Record rotational transformation on obstacle for calculating coordinates of key locations relative to the center
obs_phi_offset = -phi
obs_theta_offset = 270 - theta
# Move the camera to our calculated position
camera_pose = airsim.Pose(airsim.Vector3r(pos[0], pos[1], pos[2]), airsim.to_quaternion(math.radians(90 - theta + pitch), math.radians(roll), math.radians(phi + 180 + yaw))) #radians
Is this algorithm implemented correctly? What other ways could I find the coordinates of my object? Should I be doing something in Unreal Engine to get my coordinates instead of doing this algorithmically (though it needs to be fast)?
A translation of the origin by Vector3(i,j,k) is simply the translation of the original output.
camera_pose = airsim.Pose(airsim.Vector3r(pos[0] + i, pos[1] + j, pos[2] + k), airsim.to_quaternion(math.radians(90 - theta + pitch), math.radians(roll), math.radians(phi + 180 + yaw))) #radians
I am trying to plot a curved path for a robot to follow using the following as a guide: http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
The code i have does not create a path that ends at the destination. I am expecting the path to curve left or right depending on the quadrant the destination is in (+x+y,+x-y,-x+y,-x-y)
import math
start = [400,500]
dest = [200,300]
speed = 10
startangle = 0
rc =0
rotv =0
rads =0
def getPos(t):
ang = (rotv*t)+rads
x = start[0] - rc * math.sin(rads) + rc * math.sin(rotv*(t)+rads)
y = start[1] + rc * math.cos(rads) - rc * math.cos(rotv*(t)+rads)
return (int(x),int(y), ang)
dx = dest[0] - start[0]
dy = dest[1] - start[1]
rads = math.atan2(-dy,dx)
rads %= 2*math.pi
distance = (dx**2 + dy**2)**.5 #rg
bangle = 2*rads
rc = distance /(2 * math.sin(rads))
if rads > (math.pi/2):
bangle = 2*(rads-math.pi)
rc= -rc
if rads < -(math.pi/2):
bangle = 2*(rads+math.pi)
rc= -rc
pathlength = rc * bangle
xc = start[0] - rc * math.sin(rads)
yc = start[1] + rc * math.cos(rads)
rotcenter = [xc,yc]
traveltime = pathlength/speed
rotv = bangle/traveltime
for p in range(int(traveltime)):
pos = getPos(p)
Start: Blue, End: Red, Rotation Point: Purple
UPDATE:
I have added code to allow positive and negative x/y values. I have updated the image.
To answer your question I first read through the article you linked. I think it is very interesting and explains the ideas behind the formulas pretty well, althought it lacks the formulas for when the starting position is not at the origin and when the starting angle is not 0.
It took a little while to come up with these formulas, but now it works for every case I could think of. To be able to use the formulas given in the linked article, I used the names of the variables given there. Notice that I also used the notation with t_0 as the starting time, which you just ignored. You can easily remove any instance of t_0 or set t_0 = 0.
The last part of the following code is used for testing and creates a little red turtle that traces the path of the computed arc in the specified direction. The black turtle indicates the goal position. Both turtles are close to each other at the end of the animation, but they are not directly above each other, because I am only iterating over integers and it is possible that t_1 is not an integer.
from math import pi, hypot, sin, cos, atan2, degrees
def norm_angle(a):
# Normalize the angle to be between -pi and pi
return (a+pi)%(2*pi) - pi
# Given values
# named just like in http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
x_0, y_0 = [400,500] # initial position of robot
theta_0 = -pi/2 # initial orientation of robot
s = 10 # speed of robot
x_1, y_1 = [200,300] # goal position of robot
t_0 = 0 # starting time
# To be computed:
r_G = hypot(x_1 - x_0, y_1 - y_0) # relative polar coordinates of the goal
phi_G = atan2(y_1 - y_0, x_1 - x_0)
phi = 2*norm_angle(phi_G - theta_0) # angle and
r_C = r_G/(2*sin(phi_G - theta_0)) # radius (sometimes negative) of the arc
L = r_C*phi # length of the arc
if phi > pi:
phi -= 2*pi
L = -r_C*phi
elif phi < -pi:
phi += 2*pi
L = -r_C*phi
t_1 = L/s + t_0 # time at which the robot finishes the arc
omega = phi/(t_1 - t_0) # angular velocity
x_C = x_0 - r_C*sin(theta_0) # center of rotation
y_C = y_0 + r_C*cos(theta_0)
def position(t):
x = x_C + r_C*sin(omega*(t - t_0) + theta_0)
y = y_C - r_C*cos(omega*(t - t_0) + theta_0)
return x, y
def orientation(t):
return omega*(t - t_0) + theta_0
#--------------------------------------------
# Just used for debugging
#--------------------------------------------
import turtle
screen = turtle.Screen()
screen.setup(600, 600)
screen.setworldcoordinates(0, 0, 600, 600)
turtle.hideturtle()
turtle.shape("turtle")
turtle.penup()
turtle.goto(x_1, y_1)
turtle.setheading(degrees(orientation(t_1)))
turtle.stamp()
turtle.goto(x_0, y_0)
turtle.color("red")
turtle.showturtle()
turtle.pendown()
for t in range(t_0, int(t_1)+1):
turtle.goto(*position(t))
turtle.setheading(degrees(orientation(t)))
I am not sure at which point your code failed, but I hope this works for you. If you intend to use this snippet multiple times in you code consider encapsulating it in a function that takes in the given values as parameters and returns the position function (and if you like rotation function as well).
I am running Folium 0.2.1' with Python 2.7.11 on Jupyter Notebook Server 4.2.1
I am trying to plot lines on a map, which have a arrowhead to convey direction
import folium
#DFW, LGA coordinates
coordinates=[(32.900908, -97.040335),(40.768571, -73.861603)]
m = folium.Map(location=[32.900908, -97.040335], zoom_start=4)
#line going from dfw to lga
aline=folium.PolyLine(locations=coordinates,weight=2,color = 'blue')
m.add_children(aline)
Is there a way to add an arrowhead to the line?
You could use a regular polygon marker to draw a triangle at the end point...
folium.RegularPolygonMarker(location=(32.900908, -97.040335), fill_color='blue', number_of_sides=3, radius=10, rotation=???).add_to(m)
You'll have to use some trigonometry to calculate the angle of rotation for the triangle to point in the correct direction. The initial point of any such marker points due east.
I may be a little late to the party, but I have another suggestions for other people bothered by this problem. I would suggest to use the pyproj package's Geod class, which can do geodetic and great circle calculations. We can use it to get forward and backward azimuth of a piece of a LineString. Then for each piece we add a small polygon marker(or something similar) on one end.
from pyproj import Geod
# loop your lines
for line in lines.itertuples():
# format coordinates and draw line
loc = [[j for j in reversed(i)] for i in line.geometry.coords]
folium.PolyLine(loc, color="red").add_to(m)
# get pieces of the line
pairs = [(loc[idx], loc[idx-1]) for idx, val in enumerate(loc) if idx != 0]
# get rotations from forward azimuth of the line pieces and add an offset of 90°
geodesic = Geod(ellps='WGS84')
rotations = [geodesic.inv(pair[0][1], pair[0][0], pair[1][1], pair[1][0])[0]+90 for pair in pairs]
# create your arrow
for pair, rot in zip(pairs, rotations):
folium.RegularPolygonMarker(location=pair[0], color='red', fill=True, fill_color='red', fill_opacity=1,
number_of_sides=3, rotation=rot).add_to(m)
I hope someone will find this snippet helpful.
Have a great day! =)
I found a solution for this issue:
I calculate my own arrow points with trigonometry and then I call polyline function using that points.
def arrow_points_calculate(self, ini_lat, ini_long, heading):
lenght_scale = 0.00012
sides_scale = 0.000025
sides_angle = 25
latA= ini_lat
longA = ini_long
latB = lenght_scale * math.cos(math.radians(heading)) + latA
longB = lenght_scale * math.sin(math.radians(heading)) + longA
latC = sides_scale * math.cos(math.radians(heading + 180 - sides_angle)) + latB
longC = sides_scale * math.sin(math.radians(heading + 180 - sides_angle)) + longB
latD = sides_scale * math.cos(math.radians(heading + 180 + sides_angle)) + latB
longD = sides_scale * math.sin(math.radians(heading + 180 + sides_angle)) + longB
pointA = (latA, longA)
pointB = (latB, longB)
pointC = (latC, longC)
pointD = (latD, longD)
point = [pointA, pointB, pointC, pointD, pointB]
return point
folium.PolyLine(locations=points, color="purple").add_to(
position_plot)
Lenght_scale and Side_scale variables must be modified depends of the arrow size you want.
If you have the coords of start and finish of the arrow, just use the final coords as point B and calculate Side Scale relative to the length between that points (20% of the length between points is a correct scale on my opinion).
Example of the result:
Position plot + heading arrows
Hope it could help
I have a boat moving along a transect, looking for animals. Someone is stood on the top of the boat, facing forward, and is logging the distance from the boat and the bearing from the front of the boat when an animal is seen. I have this information as well as the xy coordinates of the boat at the point at which the animal was seen. I need to get the xy coordinates of the animal itself based on this information.
I don't have the original compass bearing of the boat, which makes this tricky; but what I do have is the next GPS (xy) coordinate of the boat, from which I can calculate a starting angle. From this, it should be possible to add or subtract the bearing at which the animal was seen to give a normalised angle which can be used to find the xy coordinates of the animal using trigonometry. Unfortunately my maths skills aren't quite up to the job.
I have several hundred points so I need to put this into a Python script to go through all the points.
In summary, the dataset has:
Original X, Original Y, End(next) X, End(next) Y, Bearing, Distance
EDIT: Sorry, I was in a rush and didn't explain this very well.
I see there being 3 stages to this problem.
Finding the original bearing of the transect
Finding the bearing of the point relative to the transect
Finding the new coordinates of the point based on this normalised angle and distance from the boat at the start xy
The Python code I had originally is below, although it's not much use - the figures given are examples.
distFromBoat = 100
bearing = 45
lengthOpposite = origX-nextX
lengthAdjacent = origY - nextY
virtX = origX #virtual X
virtY = origY-lengthOpposite #virtual Y
angle = math.degrees(math.asin(math.radians((lengthOpposite/transectLen))))
newangle = angle + bearing
newLenAdj = math.cos(newangle)*distFromBoat
newLenOpp = math.sqrt(math.pow(distFromBoat,2) + math.pow(newLenAdj,2) - 2*(distFromBoat*newLenAdj)*(math.cos(newangle)))
newX = virtX-newLenOpp
newY = origY-newLenAdj
print str(newX) +"---"+str(newY)
Thanks for any help in advance!
There is a little problem with Matt's function, so I used atan2 to get you the angle that the boat is going.
Edit: That was more complicated than I expected. In the end you need to subtract 90 and take the inverse to go from the geo-referenced angles to the trig angles.
(There is also an angles library (and probably other geography ones) that have this built in.
Now this takes the origX and origY, finds the trig angle and converts it to a heading, adds the bearing to the angle determined for the transect. Then it does trig on the distance, but using the angle converted back to trig degrees -(X-90). It is kind of warped, because we are used to thinking of 0 degrees as north/up, but in trig it is "to the right", and trig goes counter clockwise vs clockwise for navigation.
import math
origX = 0.0
origY = 0.0
nextX = 0.0
nextY = -1.0
distance = 100.0
bearing = 45
def angle(origX,origY,nextX,nextY):
opp = float(nextY - origY)
adj = float(nextX - origX)
return(math.degrees(math.atan2(adj,opp)))
# atan2 seems to even work correctly (return zero) when origin equals next
transectAngle = angle(origX,origY,nextX,nextY) # assuming the function has been defined
print "bearing plus trans", transectAngle + bearing
trigAngle = -(transectAngle + bearing -90)
print "trig equiv angle", trigAngle
newX = origX + distance * math.cos(math.radians(trigAngle))
newY = origY + distance * math.sin(math.radians(trigAngle))
print "position",newX,newY
Output:
-70.7106781187 -70.7106781187
Here is a function to print out a bunch of test cases (uses global vars so should be folded into the code above)
def testcase():
bearinglist = [-45,45,135,-135]
dist = 10
for bearing in bearinglist:
print "----transect assuming relative bearing of {}------".format(bearing)
print "{:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}".format("x","y","tran","head","trigT","trigH","newX","newY")
for x in [0,.5,-.5]:
for y in [0,.5,1,-.5]:
# print "A:", x,y,angle(origX,origY,x,y)
tA = newangle(origX,origY,x,y)
trigA = -(tA-90)
heading = tA + bearing
trigHead = -(heading-90)
Xnew = distance * math.cos(math.radians(trigHead))
Ynew = distance * math.sin(math.radians(trigHead))
print "{:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f}".format(x,y,tA,heading,trigA,trigHead,Xnew,Ynew)
From what I understand, this is your problem:
You have 2 points, start and next that you are walking between
You want to find the coordinates of a third point, New that should be some distance and bearing from start, given that you are already facing from start to next.
My solution is this:
Create a normalized vector from start to next
Rotate your normalized vector by given bearing
Multiply the normalized, rotated vector by your distance, and then add it to start
treating start as a vector, the result of the addition is your new point
Because a counter-clockwise rotation ("left" from the current point) is considered positive, you need to invert bearing so that port is associated with negative, and starboard with positive.
Code
import math
origX = 95485
origY = 729380
nextX = 95241
nextY = 729215
distance = 2000.0
bearing = 45
origVec = origX, origY
nextVec = nextX, nextY
#Euclidean distance between vectors (L2 norm)
dist = math.sqrt((nextVec[0] - origVec[0])**2 + (nextVec[1] - origVec[1])**2)
#Get a normalized difference vector
diffVec = (nextVec[0] - origVec[0])/dist, (nextVec[1] - origVec[1])/dist
#rotate our vector by bearing to get a vector from orig towards new point
#also, multiply by distance to get new value
#invert bearing, because +45 in math is counter-clockwise (left), not starboard
angle = math.radians(-bearing)
newVec = origVec[0]+(diffVec[0]*math.cos(angle) - diffVec[1]*math.sin(angle))*distance, \
origVec[1]+(diffVec[0]*math.sin(angle) + diffVec[1]*math.cos(angle))*distance
print newVec
Output:
(93521.29597031244, 729759.2973553676)
It's (probably) possible there's a more elegant solution than this...assuming I understood your issue correctly. But, this will give you the bearing from your original location:
(inputs hard-coded as an example)
import math
origX = 0.0
origY = 0.0
nextX = 1.0
nextY = 0.0
Dist = ((nextX - origX)**2 + (nextY - origY)**2)**0.5
if origX == nextX and origY == nextY:
angle = 0
if origX == nextX and nextY < origY:
angle = 180
if nextY < origY and origX > nextX:
angle = math.degrees(math.asin((nextX -origX)/Dist)) - 90
if nextX > origX and nextY < origY:
angle = math.degrees(math.asin((nextX -origX)/Dist)) + 90
else:
angle = math.degrees(math.asin((nextX -origX)/Dist))
print angle
My problem is the following:
I have two points in an image, I get the angle between these two points and rotate the image by this angle. I need to get the new position of this points in the image, but when I try to rotate those points using a rotation matrix with the same angle the points do not concur, what's wrong in the following code?
def rotMat(angle):
return asarray([[cos(angle), -sin(angle)],[sin(angle),cos(angle)]])
for i in batch2:
figure(1)
filename = "../imagens/" + i[-1]
outputFile = "./output/" + i[-1]
x1 = float(i[22]) # x coordinate of first point
y1 = float(i[23]) # y coordinate of first point
x2 = float(i[34]) # x coordinate of second point
y2 = float(i[35]) # y coordinate of second point
# angle of rotation
angle = arctan((y1-y2)/(x1-x2))
im = imread(filename)
im = ndimage.rotate(im, angle*180/pi, reshape=False)
imshow(im)
p1 = asarray([x1,y1])
p2 = asarray([x2,y2])
# Rotating the points
# [512,680] is the center of the image
p1n = (p1-[512,680]).dot(rotMat(angle)) + [512,680]
p2n = (p2-[512,680]).dot(rotMat(angle)) + [512,680]
print p1n, p2n
plot(p1n[0],p1n[1],'d')
plot(p2n[0],p2n[1],'d')
savefig(outputFile)
clf()
I don't understand 100 % what you are doing. But, did you consider that the y-axis in an image runs from 0 at the top to positive values for lower points. Therefore, the direction is opposite compared to the usual mathmetical definition. You defined rotMat in the usual way, but you have to adopt it to the changed y-axis in the image definition that runs in the oposite direction.