I'm having some trouble calculating the clockwise angles between continuous 2D vectors. My computed angles do not seem right when I compare them by eye on a plot. What follows is my process in R.
If necessary, install and enable the "circular" package:
install.packages('circular')
library(circular)
Generate a small data frame of 2D coordinates:
functest <- data.frame(x=c(2,8,4,9,10,7),y=c(6,8,2,5,1,4))
Plot the points for reference:
windows(height=8,width=8)
par(pty="s")
plot(functest, main = "Circular Functions Test")
## draw arrows from point to point :
s <- seq(length(functest$x)-1) # one shorter than data
arrows(functest$x[s], functest$y[s], functest$x[s+1], functest$y[s+1], col = 1:3)
Create a function that computes the angle between two vectors:
angle <- function(m)
{ # m is a matrix
dot.prod <- crossprod(m[, 1], m[, 2])
norm.x <- norm(m[, 1], type="2")
norm.y <- norm(m[, 2], type="2")
theta <- acos(dot.prod / (norm.x * norm.y))
as.numeric(theta) # returns the angle in radians
}
Generate a vector of compass angles in degrees (clockwise rotation):
functest_matrix <- cbind(x = functest$x,y = functest$y)
moves <- apply(functest_matrix, 2, diff)
tst <- lapply(seq(nrow(moves) - 1), function(idx) moves[c(idx, idx + 1), ])
functest_angles <- vapply(tst, angle, numeric(1))
functest_object <- circular(functest_angles, type="angles", units="radians", zero=0, rotation = "counter")
functest_convert <- conversion.circular(functest_object, type = "angles", units = "degrees", rotation = "clock", zero = pi/2)
functest_compass <- lapply(functest_convert, function(x) {if (x < 0) x+360 else x}) # converts any negative rotations to positive
I suspect something wrong may be occuring in my last three lines of code when I try to convert "normal" counterclockwise angles in radians to clockwise compass angles in degrees. Any help would be greatly appreciated!
Don't know R but see that you calculate angle between vectors using scalar product.
Note that resulted angle is not directed - it is neither clockwise, nor counterclockwise (consider that scalar product is insensitive to vector exchange).
If you really need directed angle (the angle needed to rotate the first vector to make it collinear with the second one), you have to apply ArcTan2 (atan2) approach
(result range usually is -Pi..Pi)
Theta = ArcTan2(CrossProduct(v1,v2), DotProduct(v1,v2))
This line makes no sense to me:
dot.prod <- crossprod(m[, 1], m[, 2])
You assign the cross product of two vectors to a variable named dot product.
I didn't read the rest of your code, but those are two very different things.
The dot product produces a scalar value; the cross product produces a vector orthogonal to the other two.
Are you sure your naming doesn't reflect a misunderstanding of those two operations? It might explain why you're having trouble.
You can get the angle between any two vectors using the dot product. Why do you think you need to go to all the trouble in that method?
Related
Here is the code that the issue is in:
angles = -atan(deriv)
angles = angles*(180/pi)
#shift coordinates onto their polynomials
d[1:mtp,3] = d[1:mtp,3] + poly[,2]
#rotated storage matrix
rrr = as.data.frame(matrix(data = NA, ncol = 2, nrow = 9000))
#for each moment, take in old coordinates and export newly rotated
for(i in 1:mtp){
rotm = matrix(data = c(c(cos(angles[i]),sin(angles[i])),
c(-sin(angles[i]),cos(angles[i]))), ncol=2, nrow = 2)
rotate.1 = d[i,2:3] - poly[i,]
rotate.2 = rotm %*% t(rotate.1)
rotate.3 = rotate.2 + poly[i,]
rrr[i,] = rotate.3
}
#overwrite coordinates with rotations
d[1:mtp,2:3] = rrr
"deriv" is a numeric vector containing the derivative at each point along the polynomial spline "poly" with columns 1:2 the x and y. "angles" therefore contains the calculated angle to rotate by at each point. "d" is the initial data matrix, with columns 2:3 the x and y.
Data to be translated and rotated
"Spline" polynomial along which data will be translated and rotated
Angles along polynomial used during rotation (derived from derivative)
Data + Spline (translated)
Translated and (incorrectly) rotated data
Solved: R trig functions use radians, so the script was converting to degrees needlessly and causing over-correction at a scale of 180/pi.
I have multiple matrices filled with the x and y coordinates of multiple points in 2D space that make up a graph. The matrices look something like this
x1 x2 x3 x4 ...
y1 y2 y3 y4 ...
A possible graph looks something like this
What I want to do is rotate the graph around point A so that the line between the points A and B are parallel to the X-Axis.
My idea was to treat the line AB as the hypothenuse of a right-triangle, calculate α (the angle at point A) and rotate the matrix for this graph by it using a rotation matrix.
What I did so far is the following
#df is the subset of my data that describes the graph we're handling right now,
#df has 2 or more rows
beginx=df[1,]$xcord #get the x coordinate of point A
beginy=df[1,]$ycord #get the y coordinate of point A
endx=df[nrow(df)-1,]$xcord #get the x coordinate of point B
endy=df[nrow(df)-1,]$ycord #get the y coordinate of point B
xnow=df$xcord
ynow=df$ycord
xdif=abs(beginx-endx)
ydif=abs(beginy-endy)
if((xdif != 0) & (ydif!=0)){
direct=sqrt(abs((xdif^2)-(ydif^2))) #calculate the length of the hypothenuse
sinang=abs(beginy-endy)/direct
angle=1/sin(sinang)
if(beginy>endy){
angle=angle
}else{
angle=360-angle
}
rotmat=rot(angle) # use the function rot(angle) to get the rotation matrix for
# the calculated angle
A = matrix(c(xnow,ynow),nrow=2,byrow = TRUE) # matrix containing the graph coords
admat=rotmat%*%A #multiply the matrix with the rotation matrix
}
This approach fails because it isn't flexible enough to always calculate the needed angle with the result being that the graph is rotated by the wrong angle and / or in the wrong direction.
Thanks in advance for reading and hopefully some of you can bring some fresh ideas to this
Edit: Data to reproduce this can be found here
X-Coordinates
Y-Coordinates
Not sure how to provide the data you've asked for, I'll gladly provide it in another way if you specify how you'd like it
Like this?
#read in X and Y as vectors
M <- cbind(X,Y)
#plot data
plot(M[,1],M[,2],xlim=c(0,1200),ylim=c(0,1200))
#calculate rotation angle
alpha <- -atan((M[1,2]-tail(M,1)[,2])/(M[1,1]-tail(M,1)[,1]))
#rotation matrix
rotm <- matrix(c(cos(alpha),sin(alpha),-sin(alpha),cos(alpha)),ncol=2)
#shift, rotate, shift back
M2 <- t(rotm %*% (
t(M)-c(M[1,1],M[1,2])
)+c(M[1,1],M[1,2]))
#plot
plot(M2[,1],M2[,2],xlim=c(0,1200),ylim=c(0,1200))
Edit:
I'll break down the transformation to make it easier to understand. However, it's just basic linear algebra.
plot(M,xlim=c(-300,1200),ylim=c(-300,1200))
#shift points, so that turning point is (0,0)
M2.1 <- t(t(M)-c(M[1,1],M[1,2]))
points(M2.1,col="blue")
#rotate
M2.2 <- t(rotm %*% (t(M2.1)))
points(M2.2,col="green")
#shift back
M2.3 <- t(t(M2.2)+c(M[1,1],M[1,2]))
points(M2.3,col="red")
Instead of a data frame, it looks like your data is better served as a matrix (via as.matrix).
This answer very similar to Roland's, but breaks things down into more steps and has some special-case handling when the angle is a multiple of pi/2.
#sample data
set.seed(1) #for consistency of random-generated data
d <- matrix(c(sort(runif(50)),sort(runif(50))),ncol=2)
#rotation about point A
rotA <- function(d) {
d.offset <- apply(d,2,function(z) z - z[1]) #offset data
endpoint <- d.offset[nrow(d.offset),] #gets difference
rot <- function(angle) matrix(
c(cos(angle),-sin(angle),sin(angle),cos(angle)),nrow=2) #CCW rotation matrix
if(endpoint[2] == 0) {
return(d) #if y-diff is 0, then no action required
} else if (endpoint[1] == 0) {
rad <- pi/2 #if x-diff is 0, then rotate by a right angle
} else {rad <- atan(endpoint[2]/endpoint[1])}
d.offset.rotate <- d.offset %*% rot(-rad) #rotation
d.rotate <- sapply(1:2,function(z) d.offset.rotate[,z] + d[1,z]) #undo offset
d.rotate
}
#results and plotting to check visually
d.rotate <- rotA(d)
plot(d.rotate)
abline(h=d[1,2])
I have a, not necessarily convex, polygon without intersections and a point outside this polygon. I'm wondering how calculate the Euclidian distance most efficiently in a 2-dimensional space. Is there a standard method in R?
My first idea was to calculate the minimum distance of all the lines of the polygon (extended infinitely so they are line, not line pieces) and then calculate the distance from the point to each individual line using the start of the line piece and Pythagoras.
Do you know about a package that implements an efficient algorithm?
You could use the rgeos package and the gDistance method. This will require you to prepare your geometries, creating spgeom objects from the data you have (I assume it is a data.frame or something similar). The rgeos documentation is very detailed (see the PDF manual of the package from the CRAN page), this is one relevant example from the gDistance documentation:
pt1 = readWKT("POINT(0.5 0.5)")
pt2 = readWKT("POINT(2 2)")
p1 = readWKT("POLYGON((0 0,1 0,1 1,0 1,0 0))")
p2 = readWKT("POLYGON((2 0,3 1,4 0,2 0))")
gDistance(pt1,pt2)
gDistance(p1,pt1)
gDistance(p1,pt2)
gDistance(p1,p2)
readWKT is included in rgeos as well.
Rgeos is based on the GEOS library, one of the de facto standards in geometric computing. If you don't feel like reinventing the wheel, this is a good way to go.
I decided to return and write up a theoretical solution, just for posterity. This isn't the most concise example, but it is fully transparent for those who want to know how to go about solving a problem like this by hand.
The theoretical algorithm
First, our assumptions.
We assume the polygon's vertices specify the points of a polygon in a rotational order going clockwise or counter-clockwise and the lines of the polygon cannot intersect. This means we have a normal geometric polygon, and not some strangely-defined vector graphic shape.
We assume this is a set of Cartesian coordinates, using 'x' and 'y' values that represent location on a 2-dimensional plane.
We assume the point must be outside the internal area of the polygon.
Finally, we assume that the distance desired is the minimum distance between the point and all of the infinite number of points on the perimeter of the polygon.
Now before coding, we should write out in basic terms what we want to do. We can assume that the shortest distance between the polygon and the point outside the polygon will always be one of two things: a vertex of the polygon or a point on a line between two vertices. With this in mind, we do the following steps:
Calculate the distances between all vertices and the target point.
Find the two vertices closest to the target point.
If either:
(a) the two closest vertices are not adjacent or
(b) the inside angles of either of the two vertices is greater or equal to 90 degrees,
then the closest vertex is the closest point. Calculate the distance between the closest point and the target point.
Otherwise, calculate the height of the triangle formed between the two points.
We're basically just looking to see if a vertex is closest to the point or if a point on a line is closest to the point. We have to use a few trig functions to make this work.
The code
To make this work properly, we want to avoid any 'for' loops and want to only use vectorized functions when looking at the entire list of polygon vertices. Luckily, this is pretty easy in R. We accept a data frame with 'x' and 'y' columns for our polygon's vertices, and we accept a vector with one 'x' and 'y' value for the point's location.
get_Point_Dist_from_Polygon <- function(.polygon, .point){
# Calculate all vertex distances from the target point.
vertex_Distance <- sqrt((.point[1] - .polygon$x)^2 + (.point[2] - .polygon$y)^2)
# Select two closest vertices.
min_1_Index <- which.min(vertex_Distance)
min_2_Index <- which.min(vertex_Distance[-min_1_Index])
# Calculate lengths of triangle sides made of
# the target point and two closest points.
a <- vertex_Distance[min_1_Index]
b <- vertex_Distance[min_2_Index]
c <- sqrt(diff(.polygon$x[c(min_1_Index, min_2_Index)])^2 + diff(.polygon$y[c(min_1_Index, min_2_Index)])^2)
if(abs(min_1_Index - min_2_Index) != 1 |
acos((b^2 + c^2 - a^2)/(2*b*c)) >= pi/2 |
acos((a^2 + c^2 - b^2)/(2*a*c)) >= pi/2
){
# Step 3 of algorithm.
return(vertex_Distance[min_1_Index])
} else {
# Step 4 of algorithm.
# Here we are using the law of cosines.
return(sqrt((a+b-c) * (a-b+c) * (-a+b+c) * (a+b+c)) / (2 * c))
}
}
Demo
polygon <- read.table(text="
x, y
0, 1
1, 0.8
2, 1.3
3, 1.4
2.5,0.3
1.5,0.5
0.5,0.1", header=TRUE, sep=",")
point <- c(3.2, 4.1)
get_Point_Dist_from_Polygon(polygon, point)
# 2.707397
Otherwise:
p2poly <- function(pt, poly){
# Closing the polygon
if(!identical(poly[1,],poly[nrow(poly),])){poly<-rbind(poly,poly[1,])}
# A simple distance function
dis <- function(x0,x1,y0,y1){sqrt((x0-x1)^2 +(y0-y1)^2)}
d <- c() # Your distance vector
for(i in 1:(nrow(poly)-1)){
ba <- c((pt[1]-poly[i,1]),(pt[2]-poly[i,2])) #Vector BA
bc <- c((poly[i+1,1]-poly[i,1]),(poly[i+1,2]-poly[i,2])) #Vector BC
dbc <- dis(poly[i+1,1],poly[i,1],poly[i+1,2],poly[i,2]) #Distance BC
dp <- (ba[1]*bc[1]+ba[2]*bc[2])/dbc #Projection of A on BC
if(dp<=0){ #If projection is outside of BC on B side
d[i] <- dis(pt[1],poly[i,1],pt[2],poly[i,2])
}else if(dp>=dbc){ #If projection is outside of BC on C side
d[i] <- dis(poly[i+1,1],pt[1],poly[i+1,2],pt[2])
}else{ #If projection is inside of BC
d[i] <- sqrt(abs((ba[1]^2 +ba[2]^2)-dp^2))
}
}
min(d)
}
Example:
pt <- c(3,2)
triangle <- matrix(c(1,3,2,3,4,2),byrow=T, nrow=3)
p2poly(pt,triangle)
[1] 0.3162278
I used distm() function in geosphere package to calculate the distence when points and apexes are presented in coordinate system. Also, you can easily make some alternation by substitude dis <- function(x0,x1,y0,y1){sqrt((x0-x1)^2 +(y0-y1)^2)}
for distm() .
algo.p2poly <- function(pt, poly){
if(!identical(poly[1,],poly[nrow(poly),])){poly<-rbind(poly,poly[1,])}
library(geosphere)
n <- nrow(poly) - 1
pa <- distm(pt, poly[1:n, ])
pb <- distm(pt, poly[2:(n+1), ])
ab <- diag(distm(poly[1:n, ], poly[2:(n+1), ]))
p <- (pa + pb + ab) / 2
d <- 2 * sqrt(p * (p - pa) * (p - pb) * (p - ab)) / ab
cosa <- (pa^2 + ab^2 - pb^2) / (2 * pa * ab)
cosb <- (pb^2 + ab^2 - pa^2) / (2 * pb * ab)
d[which(cosa <= 0)] <- pa[which(cosa <= 0)]
d[which(cosb <= 0)] <- pb[which(cosb <= 0)]
return(min(d))
}
Example:
poly <- matrix(c(114.33508, 114.33616,
114.33551, 114.33824,
114.34629, 114.35053,
114.35592, 114.35951,
114.36275, 114.35340,
114.35391, 114.34715,
114.34385, 114.34349,
114.33896, 114.33917,
30.48271, 30.47791,
30.47567, 30.47356,
30.46876, 30.46851,
30.46882, 30.46770,
30.47219, 30.47356,
30.47499, 30.47673,
30.47405, 30.47723,
30.47872, 30.48320),
byrow = F, nrow = 16)
pt1 <- c(114.33508, 30.48271)
pt2 <- c(114.6351, 30.98271)
algo.p2poly(pt1, poly)
algo.p2poly(pt2, poly)
Outcome:
> algo.p2poly(pt1, poly)
[1] 0
> algo.p2poly(pt2, poly)
[1] 62399.81
How do you find the 3 euler angles between 2 3D vectors?
When I have one Vector and I want to get its rotation, this link can be usually used: Calculate rotations to look at a 3D point?
But how do I do it when calculating them according to one another?
As others have already pointed out, your question should be revised. Let's call your vectors a and b. I assume that length(a)==length(b) > 0 otherwise I cannot answer the question.
Calculate the cross product of your vectors v = a x b; v gives the axis of rotation. By computing the dot product, you can get the cosine of the angle you should rotate with cos(angle)=dot(a,b)/(length(a)length(b)), and with acos you can uniquely determine the angle (#Archie thanks for pointing out my earlier mistake). At this point you have the axis angle representation of your rotation.
The remaining work is to convert this representation to the representation you are looking for: Euler angles. Conversion Axis-Angle to Euler is a way to do it, as you have found it. You have to handle the degenerate case when v = [ 0, 0, 0], that is, when the angle is either 0 or 180 degrees.
I personally don't like Euler angles, they screw up the stability of your app and they are not appropriate for interpolation, see also
Strange behavior with android orientation sensor
Interpolating between rotation matrices
At first you would have to subtract vector one from vector two in order to get vector two relative to vector one. With these values you can calculate Euler angles.
To understand the calculation from vector to Euler intuitively, lets imagine a sphere with the radius of 1 and the origin at its center. A vector represents a point on its surface in 3D coordinates. This point can also be defined by spherical 2D coordinates: latitude and longitude, pitch and yaw respectively.
In order "roll <- pitch <- yaw" calculation can be done as follows:
To calculate the yaw you calculate the tangent of the two planar axes (x and z) considering the quadrant.
yaw = atan2(x, z) *180.0/PI;
Pitch is quite the same but as its plane is rotated along with yaw the 'adjacent' is on two axis. In order to find its length we will have to use the Pythagorean theorem.
float padj = sqrt(pow(x, 2) + pow(z, 2));
pitch = atan2(padj, y) *180.0/PI;
Notes:
Roll can not be calculated as a vector has no rotation around its own axis. I usually set it to 0.
The length of your vector is lost and can not be converted back.
In Euler the order of your axes matters, mix them up and you will get different results.
It took me a lot of time to find this answer so I would like to share it with you now.
first, you need to find the rotation matrix, and then with scipy you can easily find the angles you want.
There is no short way to do this.
so let's first declare some functions...
import numpy as np
from scipy.spatial.transform import Rotation
def normalize(v):
return v / np.linalg.norm(v)
def find_additional_vertical_vector(vector):
ez = np.array([0, 0, 1])
look_at_vector = normalize(vector)
up_vector = normalize(ez - np.dot(look_at_vector, ez) * look_at_vector)
return up_vector
def calc_rotation_matrix(v1_start, v2_start, v1_target, v2_target):
"""
calculating M the rotation matrix from base U to base V
M # U = V
M = V # U^-1
"""
def get_base_matrices():
u1_start = normalize(v1_start)
u2_start = normalize(v2_start)
u3_start = normalize(np.cross(u1_start, u2_start))
u1_target = normalize(v1_target)
u2_target = normalize(v2_target)
u3_target = normalize(np.cross(u1_target, u2_target))
U = np.hstack([u1_start.reshape(3, 1), u2_start.reshape(3, 1), u3_start.reshape(3, 1)])
V = np.hstack([u1_target.reshape(3, 1), u2_target.reshape(3, 1), u3_target.reshape(3, 1)])
return U, V
def calc_base_transition_matrix():
return np.dot(V, np.linalg.inv(U))
if not np.isclose(np.dot(v1_target, v2_target), 0, atol=1e-03):
raise ValueError("v1_target and v2_target must be vertical")
U, V = get_base_matrices()
return calc_base_transition_matrix()
def get_euler_rotation_angles(start_look_at_vector, target_look_at_vector, start_up_vector=None, target_up_vector=None):
if start_up_vector is None:
start_up_vector = find_additional_vertical_vector(start_look_at_vector)
if target_up_vector is None:
target_up_vector = find_additional_vertical_vector(target_look_at_vector)
rot_mat = calc_rotation_matrix(start_look_at_vector, start_up_vector, target_look_at_vector, target_up_vector)
is_equal = np.allclose(rot_mat # start_look_at_vector, target_look_at_vector, atol=1e-03)
print(f"rot_mat # start_look_at_vector1 == target_look_at_vector1 is {is_equal}")
rotation = Rotation.from_matrix(rot_mat)
return rotation.as_euler(seq="xyz", degrees=True)
Finding the XYZ Euler rotation angles from 1 vector to another might give you more than one answer.
Assuming what you are rotation is the look_at_vector of some kind of shape and you want this shape to stay not upside down and still look at the target_look_at_vector
if __name__ == "__main__":
# Example 1
start_look_at_vector = normalize(np.random.random(3))
target_look_at_vector = normalize(np.array([-0.70710688829422, 0.4156269133090973, -0.5720613598823547]))
phi, theta, psi = get_euler_rotation_angles(start_look_at_vector, target_look_at_vector)
print(f"phi_x_rotation={phi}, theta_y_rotation={theta}, psi_z_rotation={psi}")
Now if you want to have a specific role rotation to your shape, my code also supports that!
you just need to give the target_up_vector as a parameter as well.
just make sure it is vertical to the target_look_at_vector that you are giving.
if __name__ == "__main__":
# Example 2
# look and up must be vertical
start_look_at_vector = normalize(np.array([1, 2, 3]))
start_up_vector = normalize(np.array([1, -3, 2]))
target_look_at_vector = np.array([0.19283590755300162, 0.6597510192626469, -0.7263217228739983])
target_up_vector = np.array([-0.13225754322703182, 0.7509361508721898, 0.6469955018014842])
phi, theta, psi = get_euler_rotation_angles(
start_look_at_vector, target_look_at_vector, start_up_vector, target_up_vector
)
print(f"phi_x_rotation={phi}, theta_y_rotation={theta}, psi_z_rotation={psi}")
Getting Rotation Matrix in MATLAB is very easy
e.g.
A = [1.353553385, 0.200000003, 0.35]
B = [1 2 3]
[q] = vrrotvec(A,B)
Rot_mat = vrrotvec2mat(q)
I have a bunch of points that lie around y=x (see the examples below), and I hope to calculate the orthogonal distance of each point to this y=x. Suppose that a point has coordinates (a,b), then it's easy to see the projected point on the y=x has coordinates ((a+b)/2, (a+b)/2). I use the following native codes for the calculation, but I think I need a faster one without the for loops. Thank you very much!
set.seed(999)
n=50
typ.ord = seq(-2,3, length=n) # x-axis
#
good.ord = sort(c(rnorm(n/2, typ.ord[1:n/2]+1,0.1),rnorm(n/2,typ.ord[(n/2+1):n]-0.5,0.1)))
y.min = min(good.ord)
y.max = max(good.ord)
#
plot(typ.ord, good.ord, col="green", ylim=c(y.min, y.max))
abline(0,1, col="blue")
#
# a = typ.ord
# b = good.ord
cal.orth.dist = function(n, typ.ord, good.ord){
good.mid.pts = (typ.ord + good.ord)/2
orth.dist = numeric(n)
for (i in 1:n){
num.mat = rbind(rep(good.mid.pts[i],2), c(typ.ord[i], good.ord[i]))
orth.dist[i] = dist(num.mat)
}
return(orth.dist)
}
good.dist = cal.orth.dist(50, typ.ord, good.ord)
sum(good.dist)
As easy as
good.dist <- sqrt((good.ord - typ.ord)^2 / 2)
It all boils down to compute the distance between a point and a line. In the 2D case of y = x, this becomes particularly easy (try it yourself).
In the more general case (extending to other lines in possibly more than 2-D space), you can use the following. It works by constructing a projection matrix P from the subspace (here the vector A) onto which you want to project the points x. Subtracting the projected component from the points leaves the orthogonal component, for which it's easy to calculate the distances.
x <- cbind(typ.ord, good.ord) # Points to be projected
A <- c(1,1) # Subspace to project onto
P <- A %*% solve(t(A) %*% A) %*% t(A) # Projection matrix P_A = A (A^T A)^-1 A^T
dists <- sqrt(rowSums(x - x %*% P)^2) # Lengths of orthogonal residuals