We Keep Coding

How can I find the angle between two vectors that can range from 0 rad to 2 rad, instead of the inside angle with the arccos function

I'm making a python script right now that is trying to find the length of an arc, where it given this information:
center of arc: x1, y1
start point of arc: x2, y2
end point of arc: x3, y3
direction, cw, ccw
so far I have been able to successfully calculate the radius, and I tried calculating the angle using the equation:
But for any arcs that have an angle greater than 1*pi or 180 degrees, it returns the incorrect (but correct) inside angle.
What is the correct equation knowing the radius and these three points that I can use to find the value of the angle of the arc from 0 rad/degrees to 360 degrees/2pi radians, going in either the clockwise or counterclockwise direction (it can be either or and I need to be able to calculate for both scenarios)
Code:
# code to find theta
aVector = np.array([x1 - x2, y1 - y2])
bVector = np.array([x1 - x3, y1 - y3])
aMag = np.linalg.norm(aVector)
bMag = np.linalg.norm(aVector)
theta = np.arcos(np.dot(aVector, bVector) / (aMag * bMag))
as you can see here, I'm using arccos which to my dismay only outputs 0-180 degrees
Solution/Working code:
# equation for angle using atan2
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
if gcodeAnalysis[tempLineNum][4] == "G3": # going CW
start, end = end, start
tau = 2.0 * math.pi
theta = math.fmod(math.fmod(end - start, tau) + tau, tau)
Working Values:
X1 = 0.00048399999999998444
Y1 = 0.0002720000000007161
X2 = 0.378484
Y2 = -14.694728
X3 = 3.376
Y3 = -14.307
Proper result/value
Theta = 6.077209477545957
Assume this arc was done CCW

As you noticed, the range of math.acos is [0, pi], making it rather useless for telling you the relative directions of the vectors. To get full circular information about a pair of angles, you can use math.atan2. While regular math.atan has a range of [-pi/2, pi/2], atan2 splits the inputs into two parts and returns an angle in the range (-pi, pi]. You can compute the angles relative to any reference, not necessarily relative to each other:
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
Now you can use some common formulae to find the difference between the angles in whatever direction you want. I've implemented some of these in a small utility library I made called haggis. The specific function you want is haggis.math.ang_diff_pos.
First, the "manual" computation:
if direction == 'cw':
start, end = end, start
tau = 2.0 * math.pi
angle = math.fmod(math.fmod(end - start, tau) + tau, tau)
If you want to use my function, you can do
if direction == 'cw':
start, end = end, start
angle = ang_diff_pos(start, end)
All of these operations can be easily vectorized using numpy if you find yourself dealing with many points all at once.

You can use the cross product of the two vector to determine if the two vector need to rotate clock or counter-clock wise.
See code below:
import numpy as np
from numpy import linalg as LA
x1 = 0
y1 = 0
x2 = 2
y2 = 0
x3 = 2
y3 = -2
dir = 'ccw' # or ccw
v1 = np.array([x2-x1,y2-y1])
v2 = np.array( [x3-x1,y3-y1])
# if the cross product is positive, then the two vector need to rotate counter clockwise
rot = np.cross(v1,v2)
vdir = 'ccw' if rot >0 else 'cw'
r = (v1[0]*v2[0]+v1[1]*v2[1])/(LA.norm(v1)*LA.norm(v2))
deg = np.arccos(r)/np.pi*180
if vdir != dir:
deg = 360 -deg
print(deg)

How do I construct a point from x,y, line and angle?

I am making a geometry interface in python (currently using tkinter) but I have stumbled upon a major problem: I need a function that is able to return a point, that is at a certain angle with a certain line segment, is a certain length apart from the vertex of the angle. We know the coordinates of the points of the line segment, and also the angle at which we want the point to be. I have attached an image below for a more graphical view of my question.
The problem: I can calculate it using trigonometry, where
x, y = vertex.getCoords()
endx = x + length * cos(radians(angle))
endy = y + length * sin(radians(angle))
p = Point(endx, endy)
The angle I input is in degrees. That calculation is true only when the line segment is parallel to the abscissa. But the sizes of the angles I get back are very strange, to say the least. I want the function to work wherever the first two points are on the tkinter canvas, whatever the angle is. I am very lost as to what I should do to fix it. What I found out: I get as output a point that when connected to the vertex, makes a line that is at the desired angle to the abscissa. So it works when the first arm(leg, shoulder) of the angle is parallel to the abscissa, then the function runs flawlessly (because of cross angles) - the Z formation. As soon as I make it not parallel, it becomes weird. This is because we are taking the y of the vertex, not where the foot of the perpendicular lands(C1 on the attached image). I am pretty good at math, so feel free to post some more technical solutions, I will understand them
EDIT: I just wanted to make a quick recap of my question: how should I construct a point that is at a certain angle from a line segment. I have already made functions that create the angle in respect to the X and Y axes, but I have no idea how i can make it in respect to the line inputted. Some code for the two functions:
def inRespectToXAxis(vertex, angle, length):
x, y = vertex.getCoords()
newx = x + length * cos(radians(angle))
newy = y + length * sin(radians(angle))
p = Point(abs(newx), abs(newy))
return p
def inRespectToYAxis(vertex, length, angle):
x, y = vertex.getCoords()
theta_rad = pi / 2 - radians(angle)
newx = x + length * cos(radians(angle))
newy = y + length * sin(radians(angle))
p = Point(newx, newy)
return p

Seems you want to add line segment angle to get proper result. You can calculate it using segment ends coordinates (x1,y1) and (x2,y2)
lineAngle = math.atan2(y2 - y1, x2 - x1)
Result is in radians, so apply it as
endx = x1 + length * cos(radians(angle) + lineAngle) etc

Python - Derive segment coordinates from length and angle

I face a seemingly simple problem that somehow I didn't manage to solve, despite looking into several trigonometry and geometry intros.
I have a 2D space in which x=0; y=0 is the centre. I would like, given some position x1, y1 (being the coordinates of one end of the segment), and a length and an angle (0 denoting vertical lines), to find the coordinates of the other end of the segment.
In other words, being able to move from one set of parameters (x1; y1; angle; length) to (x1; y1; x2; y2) and vice versa.
Thanks a lot,

For this you want to use sine and cosine. Here is some example code:
from math import cos, sin, radians
a = radians(45)
l = 10
x1, y1 = (10, 15)
x2 += sin(a) * l
y2 += cos(a) * l
Here is an article about how and why this works.

Looking for an approach to fit rectangles into contourpoints

This is a more general question since I don't know how to tackle or solve this problem efficiently:
I am using OpenCV to get real-time contour points of moving objects (list of (x,y) coordinate tuples)
I want to draw rectangles inside these contour points depending if there are angles in it
Suppose the following image shows the contour points of my arm. I want it to detect angles of certain degree and depending on that, draw some rectangles inside.
The whole idea behind this is to make it interact with some game objects (e.g. ball) in pygame later on. For example, move a ball with your hands or a stick or any object that moves in front of the camera.
That is why i want to avoid using more advances libraries like openpose to get the skeleton of my arm since the game should be playable with any kind of object but also run smoothly.
I would be very grateful if you know a suitable approach you can name for this problem!
Approaches I have thought of so far:
My initial idea is to calculate the distance as well as angle of each neighbor contour points. If the angle is larger than some certain degree it will be considered as a new cluster. However, this doesn't seem to be reliable since the hand (fingers) have sharp edges and i dont want to get a skeleton of small things but rather simple large shapes like in the picture above
My next idea was to connect all contour points together and form a polygon. However, this would create a complex mask and the contour points are non-constant so it oscillates too much. That is why i thought a simple rectangle should be enough even if it doesn't has a pixel perfect shape

Another Approach is to make a line in between each point and then you can make objects be made out of lines then do line intersection to check for collisions. I recently made a program that does this using math from Wikipedia
#check if 2 lines are intersecting
#lines are 2 pygame Vector2
def LineIntersect(line1, line2):
#the math is from wikipedia
x1 = line1[0].x
y1 = line1[0].y
x2 = line1[1].x
y2 = line1[1].y
x3 = line2[0].x
y3 = line2[0].y
x4 = line2[1].x
y4 = line2[1].y
#denominator
den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
if den == 0:
return
t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den
u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den
if t > 0 and t < 1 and u > 0 and u < 1:
pt = Vector2()
pt.x = x1 + t * (x2 - x1)
pt.y = y1 + t * (y2 - y1)
return pt
return
Another approach that you could do with the first one is to simplify the shape by getting rid of points in a straight line. I did a test and got the following result
where the black dots get removed and red is the simplified shape. Not really sure what to do from there, so i guess with my first approach would work best?
Here is the code for it
def Prepare(Points):
New_points = [Points[0]]
last_point = Vector2(Points[0])
for i in range(1,len(Points)-1,1):
p = Vector2(Points[i])
Dir = p - last_point
if i < len(Points) - 1:
New_dir = Points[i+1] - p
New_dir = New_dir.normalize()
angle = Dir.angle_to(New_dir)
if abs(angle) > 15:
New_points.append(Points[i])
#print(last_point.angle_to(p))
pygame.draw.circle(screen,(255,0,0),(int(p.x),int(p.y)),5)
last_point = p
New_points.append(Points[-1])
return New_points
What i did was got the direction from the previous point to the current point and the next point, if the difference was more than 15 degrees, it was a corner and i added to new points

Calculating 'Diagonal Distance' in 3 dimensions for A* path-finding heuristic

I'm running A* Path finding over a 3D grid of data. Available movements are the 26 surrounding nodes (i.e. you can move diagonally) I've been using euclidean distance as the heuristic and this works well but I'd also like to try 'diagonal distance' to see how that works (and if there are any speed gains)
I've found some logic online to do this in 2 dimensions...
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
..,where D is the north/east/south/west distance (for example 1m) and D2 is the diagonal distance (for example sqrt(2))
I'm not exactly sure how to convert this to 3 dimensions - so any help would be greatly appreciated
As an extra question (as this is how much grid is in reality...) suppose nodes on the x and y axis are 5m apart, but 2m apart on the z axis...how would the formula work then?
Thanks for any help!

It can be extended to 3D relatively easily. It does require finding the "middle" of 3 values, there is a trick for that given that we have the minimum and maximum.
dx = absdiff(node.x, goal.x)
dy = absdiff(node.y, goal.y)
dz = absdiff(node.z, goal.z)
dmin = min(dx, dy, dz)
dmax = max(dx, dy, dz)
dmid = dx + dy + dz - dmin - dmax
This works for Python style integers and even for Java style int, for floats it can cause some rounding though.
Combine them like this:
return (D3 - D2) * dmin + (D2 - D1) * dmid + D1 * dmax

am i right?
Depth camera show us the depth of object.
The depth data is a diagonal distance from the center of camera.
It is not a z-axis distance which is moving control.
Therefore, it has to be calculating using depth, pixel X and Pixel Y.
I know that we don't know the pysical unit of