Related
I am breaking my head trying to find an appropriate formula to calculate a what sounds to be an easy task but in practice is a big mathematical headache.
I want to find out the offset it needs to turn my vector's angle (X, Y, Angle) to face a coord ( X, Y )
My vector won't always be facing 360 degrees, so i need that as a variable as well..
Hoping an answer before i'm breaking my pc screen.
Thank you.
input
p1 = (x1,y1) point1 (vector origin)
p2 = (x2,y2) point2
a1 = 360 deg direction of vector
assuming your coodinate system is: X+ is right Y+ is up ang+ is CCW
your image suggest that you have X,Y mixed up (angle usually start from X axis not Y)
da=? change of a1 to match direction of p2-p1
solution 1:
da=a1-a2=a1-atanxy(x2-x1,y1-y1)
atanxy(dx,dy) is also called atan2 on some libs just make sure the order of operands is the right one
you can also use mine atanxy in C++
it is 4 quadrant arctangens
solution 2:
v1=(cos(a1),sin(a1))
v2=(x2-x1,y2-y1)
da=acos(dot(v1,v2)/(|v1|*|v2|))
or the same slightly different
v1=(cos(a1),sin(a1))
v2=(x2-x1,y2-y1)
v2/=|v2| // makes v2 unit vector, v1 is already unit
da=acos(dot(v1,v2))
so:
da=acos((cos(a1)*(x2-x1)+sin(a1)*(y2-y1)/sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)));
[notes]
just change it to match your coordinate system (which you did not specify)
use radians or degrees according to your sin,cos,atan dependencies ...
The difference between the vectors is also a vector.
Then calculate the tangens (y part / x part) and invert it to an angle.
Of course use the sign of y if x = 0.
if the coord to face is (x2 ,y2)
deltaY = y2 - y1
deltaX = x2 - x1
You have the angle in degrees between the two points using this formula...
angleInDegrees = arctan(deltaY / deltaX) * 180 / PI
subtract the original angle of your vector and you will get the correct offset!
I'm trying to program a simulation of people, and one thing I'd like to do is simulate personal-space buffers. To do this, I need to check one point pt1 to see if it needs to be repelled by another point pt2. I want the scaling of the resistance of pt1 to model a hyperbola such as 1 / (distance + 1) where the +1 ensures that at small distances the force does not go to infinity.
I have most of this figured out, but I can not figure out how to get a force vector which relative to pt1 is a normalized vector of the force against it. Can anybody here good with vector math help me? Thank you!
Not sure if I understood your question correctly, but I'm assuming this:
you have a list of points, let's say in an array of pairs of coordinates:
[[x0, y0], [x1, y1], [x2, y2], ... [xn, yn]]
Then, if you need to calculate the resulting force vector on the point #k you need this:
force_vector = [0, 0]
for i from 0 to n:
skip if i = k
x_force = xk - xi
y_force = yk - yi
// Resulting force vector for i-k pair will be aligned as [x_force, y_force]
// we just need to normalize it
vector_modulo = square_root(x_force^2 + y_force^2)
normalized_vector = [x_force/vector_modulo, y_force/vector_modulo]
dist_ik = square_root((xk-xi)^2 + (yk - yi)^2)
force_vector[0] += normalized_vector[0]/(dist_ik + 1)
force_vector[1] += normalized_vector[1]/(dist_ik + 1)
At the end you will have force_vector with x and y values of a "force" for #k point.
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)
Ok, I know this sounds really daft to be asking here, but it is programming related.
I'm working on a game, and I'm thinking of implementing a system that allows users to triangulate their 3D coordinates to locate something (eg for a task).
I also want to be able to let the user make the coordinates of the points they are using for triangulation have user-determined coordinates (so the location's coordinate is relative, probably by setting up a beacon or something).
I have a method in place for calculating the distance between the points, so essentially I can calculate the lengths of the sides of the triangle/pyramid as well as all but the coordinate I am after.
It has been a long time since I have done any trigonometry and I am rusty with the sin, cos and tan functions, I have a feeling they are required but have no clue how to implement them.
Can anyone give me a demonstration as to how I would go about doing this in a mathematical/programatical way?
extra info:
My function returns the exact distance between the two points, so say you set two points to 0,0,0 and 4,4,0 respectively, and those points are set to scale(the game world is divided into a very large 3d grid, with each 'block' area being represented by a 3d coordinate) then it would give back a value at around 5.6.
The key point about it varying is that the user can set the points, so say they set a point to read 0,0,0, the actual location could be something like 52, 85, 93. However, providing they then count the blocks and set their other points correctly (eg, set a point 4,4,0 at the real point 56, 89, 93) then the final result will return the relative position (eg the object they are trying to locate is at real point 152, 185, 93, it will return the relative value 100,100,0). I need to be able to calculate it knowing every point but the one it's trying to locate, as well as the distances between all points.
Also, please don't ask why I can't just calculate it by using the real coordinates, I'm hoping to show the equation up on screen as it calculates the result.7
Example:
Here is a diagram
Imagine these are points in my game on a flat plain.
I want to know the point f.
I know the values of points d and e, and the sides A,B and C.
Using only the data I know, I need to find out how to do this.
Answered Edit:
After many days of working on this, Sean Kenny has provided me with his time, patience and intellect, and thus I have now got a working implementation of a triangulation method.
I hope to place the different language equivalents of the code as I test them so that future coders may use this code and not have the same problem I have had.
I spent a bit of time working on a solution but I think the implementer, i.e you, should know what it's doing, so any errors encountered can be tackled later on. As such, I'll give my answer in the form of strong hints.
First off, we have a vector from d to e which we can work out: if we consider the coordinates as position vectors rather than absolute coordinates, how can we determine what the vector pointing from d to e is? Think about how you would determine the displacement you had moved if you only knew where you started and where you ended up? Displacement is a straight line, point A to B, no deviation, not: I had to walk around that house so I walked further. A straight line. If you started at the point (0,0) it would be easy.
Secondly, the cosine rule. Do you know what it is? If not, read up on it. How can we rearrange the form given in the link to find the angle d between vectors DE and DF? Remember you need the angle, not a function of the angle (cos is a function remember).
Next we can use a vector 'trick' called the scalar product. Notice there is a cos function in there. Now, you may be thinking, we've just found the angle, why are we doing it again?
Define DQ = [1,0]. DQ is a vector of length 1, a unit vector, along the x-axis. Which other vector do we know? Do we know of two position vectors?
Once we have two vectors (I hope you worked out the other one) we can use the scalar product to find the angle; again, just the angle, not a function of it.
Now, hopefully, we have 2 angles. Could we take one from the other to get yet another angle to our desired coordinate DF? The choice of using a unit vector earlier was not arbitrary.
The scalar product, after some cancelling, gives us this : cos(theta) = x / r
Where x is the x ordinate for F and r is the length of side A.
The end result being:
theta = arccos( xe / B ) - arccos( ( (A^2) + (B^2) - (C^2) ) / ( 2*A*B ) )
Where theta is the angle formed between a unit vector along the line y = 0 where the origin is at point d.
With this information we can find the x and y coordinates of point f relative to d. How?
Again, with the scalar product. The rest is fairly easy, so I'll give it to you.
x = r.cos(theta)
y = r.sin(theta)
From basic trigonometry.
I wouldn't advise trying to code this into one value.
Instead, try this:
//pseudo code
dx = 0
dy = 0 //initialise coordinates somehow
ex = ex
ey = ey
A = A
B = B
C = C
cosd = ex / B
cosfi = ((A^2) + (B^2) - (C^2)) / ( 2*A*B)
d = acos(cosd) //acos is a method in java.math
fi = acos(cosfi) //you will have to find an equivalent in your chosen language
//look for a method of inverse cos
theta = fi - d
x = A cos(theta)
y = A sin(theta)
Initialise all variables as those which can take decimals. e.g float or double in Java.
The green along the x-axis represents the x ordinate of f, and the purple the y ordinate.
The blue angle is the one we are trying to find because, hopefully you can see, we can then use simple trig to work out x and y, given that we know the length of the hypotenuse.
This yellow line up to 1 is the unit vector for which scalar products are taken, this runs along the x-axis.
We need to find the black and red angles so we can deduce the blue angle by simple subtraction.
Hope this helps. Extensions can be made to 3D, all the vector functions work basically the same for 3D.
If you have the displacements from an origin, regardless of whether this is another user defined coordinate or not, the coordinate for that 3D point are simply (x, y, z).
If you are defining these lengths from a point, which also has a coordinate to take into account, you can simply write (x, y, z) + (x1, y1, z1) = (x2, y2, z2) where x2, y2 and z2 are the displacements from the (0, 0, 0) origin.
If you wish to find the length of this vector, i.e if you defined the line from A to B to be the x axis, what would the x displacement be, you can use Pythagoras for 3D vectors, it works just the same as with 2D:
Length l = sqrt((x^2) + (y^2) + (z^2))
EDIT:
Say you have a user defined point A (x1, y1, z1) and you want to define this as the origin (0,0,0). You have another user chosen point B (x2, y2, z2) and you know the distance from A to B in the x, y and z plane. If you want to work out what this point is, in relation to the new origin, you can simply do
B relative to A = (x2, y2, z2) - (x1, y1, z1) = (x2-x1, y2-y1, z2-z1) = C
C is the vector A>B, a vector is a quantity which has a magnitude (the length of the lines) and a direction (the angle from A which points to B).
If you want to work out the position of B relative to the origin O, you can do the opposite:
B relative to O = (x2, y2, z2) + (x1, y1, z1) = (x1+x2, y1+y2, z1+z2) = D
D is the vector O>B.
Edit 2:
//pseudo code
userx = x;
usery = y;
userz = z;
//move origin
for (every block i){
xi = xi-x;
yi = yi - y;
zi = zi -z;
}
Given four points in the plane, A,B,X,Y, I wish to determine which of the following two angles is smaller ∢ABX or ∢ABY.
The angle ∢ABX is defined as the angle of BX, when AB is translated to lie on the open segment (-∞,0]. Intuitively when saying ∢ABX I mean the angle you get when you turn left after visiting vertex B.
I'd rather not use cos or sqrt, in order to preserve accuracy, and to minimize performance (the code would run on an embedded system).
In the case where A=(-1,0),B=(0,0), I can compare the two angles ∢ABX and ∢ABY, by calculating the dot product of the vectors X,Y, and watch its sign.
What I can do in this case is:
Determine whether or not ABX turns right or left
If ABX turns left check whether or not Y and A are on the same side of the line on segment BX. If they are - ∢ABX is a smaller than ABY.
If ABX turns right, then Y and A on the same side of BX means that ∢ABX is larger than ∢ABY.
But this seems too complicated to me.
Any simpler approach?
Here's some pseudocode. Doesn't detect the case when both angles are the same. Also doesn't deal with angle orientation, e.g. assumes all angles are <= 180 degrees.
v0 = A-B
v1 = X-B
v2 = Y-B
dot1 = dot(v0, v1)
dot2 = dot(v0, v2)
if(dot1 > 0)
if(dot2 < 0)
// ABX is smaller
if(dot1 * dot1 / dot(v1,v1) > dot2 * dot2 / dot(v2, v2) )
// ABX is smaller
// ABY is smaller
if(dot2 > 0)
// ABY is smaller
if(dot1 * dot1 / dot(v1,v1) > dot2 * dot2 / dot(v2,v2) )
// ABY is smaller
// ABX is smaller
Note that much of this agonizing pain goes away if you allow taking two square roots.
Center the origin on B by doing
X = X - B
Y = Y - B
A = A - B
EDIT: you also need to normalise the 3 vectors
A = A / |A|
X = X / |X|
Y = Y / |Y|
Find the two angles by doing
acos(A dot X)
acos(A dot Y)
===
I don't understand the point of the loss of precision. You are just comparing, not modifying in any way the coordinates of the points...
You might want to check out Rational Trigonometry. The ideas of distance and angle are replaced by quadrance and spread, which don't involve sqrt and cos. See the bottom of that webpage to see how spread between two lines is calculated. The subject has its own website and even a youtube channel.
I'd rather not use cos or sqrt, in order to preserve accuracy.
This makes no sense whatsoever.
But this seems too complicated to me.
This seems utterly wrong headed to me.
Take the difference between two vectors and look at the signs of the components.
The thing you'll have to be careful about is what "smaller" means. That idea isn't very precise as stated. For example, if one point A is in quadrant 4 (x-component > 0 and y-component < 0) and the other point B is in quadrant 1 (x-component > 0 and y-component > 0), what does "smaller" mean? The angle of the vector from the origin to A is between zero and π/2; the angle of the vector from the origin to B is between 3π/4 and 2π. Which one is "smaller"?
I am not sure if you can get away without using sqrt.
Simple:
AB = A-B/|A-B|
XB = X-B/|X-B|
YB = Y-B/|Y-B|
if(dot(XB,AB) > dot (YB,AB)){
//<ABY is grater
}
else
{
...
}
Use the law of cosines: a**2 + b**2 - 2*a*b*cos(phi) = c**2
where a = |ax|, b =|bx| (|by|), c=|ab| (|ay|) and phi is your angle ABX (ABY)