Develop Reference

Moving single point from curve knowing distance

I have a set of x and y points (whatever the function behind) here in black. I would like to move the (x0, y0) point to the (x1,y1) knowing that there is 3 cm (whatever the unit) from (x0, y0) to (x1,y1) at 90° angle.
I would like to do it in Python, however obviously this is pretty bad.
fig = plt.figure()
from mpl_toolkits.mplot3d import Axes3D
ax = fig.add_subplot(111)
c = [55, 53, 54]
d = [29, 27, 27]
c = [55, 53 + 3, 54]
dd = [29, 27 + 3, 27 ]
ax.plot(c,d,'-o', c='g')
ax.plot(c,dd,'-o', c='b')
Partial final answer translated into Python (Thanks to picobit), however I would like to make the picobit function "orientation sensitive" :
fig = plt.figure()
from mpl_toolkits.mplot3d import Axes3D
ax = fig.add_subplot(111)
a = [0.22520001, 0.22140153, 0.21732369, 0.21258711, 0.20764232, 0.20515779,
0.20449048, 0.20467589, 0.20534733]
b = [0.21270538 ,0.21026637, 0.20749939, 0.20383899, 0.19925433, 0.19559762,
0.19440357, 0.19375025, 0.19344115]
dev = [0.0009969 , 0.00143304, 0.00174457, 0.00193148, 0.00199379, 0.00186918,
0.00149534, 0.00087228, 0. ]
import math
def rotate_vector(x0, y0, angle, dev):
magnitude = math.sqrt(x0**2 + y0**2)
xhat = x0/magnitude
yhat = y0/magnitude
x_rot = -yhat * math.sin(angle) + xhat * math.cos(angle)
y_rot = yhat * math.cos(angle) + xhat * math.sin(angle)
x_rot = x_rot * dev
y_rot = y_rot * dev
anglee = 90 # Obviously different if 0 or 45, etc...
x_rot = (x_rot * math.cos(np.radians(anglee))) - (y_rot * math.sin(np.radians(anglee)))
y_rot = (x_rot * math.sin(np.radians(anglee))) + (y_rot * math.cos(np.radians(anglee)))
x_final = x_rot + x0
y_final = y_rot + y0
return x_final, y_final

Prerequisites:
https://en.wikipedia.org/wiki/Pythagorean_theorem
https://en.wikipedia.org/wiki/Origin_(mathematics)
https://en.wikipedia.org/wiki/Unit_vector
https://en.wikipedia.org/wiki/Rotation_matrix
Your algorithm is:
Find the unit vector that points from the origin to (x0,y0)
Multiply the unit vector by the rotation matrix
Multiply this new vector by your desired length (3cm in your example)
Move the newly-scaled vector's starting point back to (x0,y0)
Step 1:
Let A be the vector between the origin and (x0,y0). We need to find |A|, magnitude of A, (aka the length of the line segment) using the Pythagorean Theorem.
Find the unit vector by dividing (x0,y0) by |A|, giving us (x0/|A|,y0/|A|). This is the unit vector along A. Prove it to yourself by drawing a little, tiny right triangle with one end of the hypotenuse at the origin and the other end at (x0/|A|,y0/|A|).
Just to make things easier to type, let xhat=x0/|A| and yhat=y0/|A|.
Step 2:
You can rotate the unit vector by any angle θ by multiplying by the rotation matrix. After shaking out the matrix multiplication, you get the new point (xhat',yhat') where
xhat' = xhat*Cosθ - yhat*Sinθ
yhat' = xhat*Sinθ + yhat*Cosθ
90 degrees is a friendly angle, so this simplifies to:
xhat' = -yhat
yhat' = xhat
Step 3:
Scale the new vector by 3 units (centimeters in your example):
3*(-yhat,xhat) = (-3*yhat,3*xhat)
Step 4:
Move this new vector's starting point back to (x0,y0)
(x1,y1) = (-3*yhat,3*xhat) + (x0,y0)
= (-3*yhat+x0,3*xhat+y0)
Those are the coordinates for your new point.
As a quick example, if you have the point (3,4), and you want to perform the same translation as in your example image, then you'd do this:
|A| = 5, so (xhat, yhat) = (3/5, 4/5)
(xhat', yhat') = (-4/5, 3/5)
3*(-4/5, 3/5) = (-12/5, 9/5)
(-12/5+3, 9/5+4) = (0.6, 5.8)
Now prove to yourself that the two points are 3 units apart, again using the Pythagorean Theorem. A right triangle with hypotenuse connecting the two points (3,4) and (0.6,5.8) has sides with lengths (3-0.6) and (5.8-3)

Euler Angles and Rotation Matrix from two 3D points

I am trying to find the Euler angles that allow the transformation from point A to point B in 3D space.
Consider the normalized vectors A = [1, 0, 0] and B = [0.32 0.88 -0.34].
I understand that by computing the cross product A × B I get the rotation axis. The angle between A and B is given by tan⁻¹(||cross||, A·B), where A·B is the dot product between A and B.
This gives me the rotation vector rotvec = [0 0.36 0.93 1.24359531111], which is rotvec = [A × B; angle] (the cross product is normalized).
Now my question is: How do I move from here to get the Euler angles that correspond to the transformation from A to B?
In MATLAB the function vrrotvec2mat receives as input a rotation vector and outputs a rotation matrix. Then the function rotm2eul should return the corresponding Euler angles. I get the following result (in radians): [0.2456 0.3490 1.2216], according to the XYZ convention. Yet, this is not the expected result.
The correct answer is [0 0.3490 1.2216] that corresponds to a rotation of 20° and 70° in Y and Z, respectively.
When I use eul2rot([0 0.3490 1.2216]) (with eul2rot taken from here) to verify the resulting rotation matrix, this one is different from the one I obtain when using vrrotvec2mat(rotvec).
I also have a Python spinet that yields the exactly same results as described above.
--- Python (2.7) using transform3d ---
import numpy as np
import transforms3d
cross = np.cross(A, B)
dot = np.dot(A, B.transpose())
angle = math.atan2(np.linalg.norm(cross), dot)
rotation_axes = sklearn.preprocessing.normalize(cross)
rotation_m = transforms3d.axangles.axangle2mat(rotation_axes[0], angle, True)
rotation_angles = transforms3d.euler.mat2euler(rotation_m, 'sxyz')
What I am missing here? What should I be doing instead?
Thank you

A rotation matrix has 3 degrees of freedom but the constraints of your problem only constrain 2 of those degrees.
This can be made more concrete by considering the case where we have a rotation matrix R which rotates from A to B so R*A == B. If we then construct another rotation matrix RB which rotates about vector B then applying this rotation to R*A won't have any effect, i.e. B == R*A == RB*R*A. It will, however, produce a different rotation matrix RB*R with different Euler angles.
Here's an example in MATLAB:
A = [1; 0; 0];
B = [0.32; 0.88; -0.34];
A = A / norm(A);
B = B / norm(B);
ax = cross(A, B);
ang = atan2(norm(ax), dot(A, B)); % ang = acos(dot(A, B)) works too
R = axang2rotm([ax; ang].');
ang_arbitrary = rand()*2*pi;
RB = axang2rotm([B; ang_arbitrary].');
R*A - B
RB*R*A - B
rotm2eul(R)
rotm2eul(RB*R)
Result
ans =
1.0e-15 *
-0.0555
0.1110
0
ans =
1.0e-15 *
0.2220
0.7772
-0.2776
ans =
1.2220 0.3483 0.2452
ans =
1.2220 0.3483 0.7549

I will give you a solution based on Euler's rotation theorem.
This solution gives you only the one angle, but the other angles can be derived.
import numpy as np
a_vec = np.array([1, 0, 0])/np.linalg.norm(np.array([1, 0, 0]))
b_vec = np.array([0.32, 0.88, -0.34])/np.linalg.norm(np.array([0.32, 0.88, -0.34]))
cross = np.cross(a_vec, b_vec)
ab_angle = np.arccos(np.dot(a_vec,b_vec))
vx = np.array([[0,-cross[2],cross[1]],[cross[2],0,-cross[0]],[-cross[1],cross[0],0]])
R = np.identity(3)*np.cos(ab_angle) + (1-np.cos(ab_angle))*np.outer(cross,cross) + np.sin(ab_angle)*vx
validation=np.matmul(R,a_vec)
This uses the common axis of rotation (eigenvector in this case), as the cross product.
The matrix R is then the rotation matrix.
This is a general way of doing it, and very simple.

Recover angles after transformation

This seems like an easy enough task but I've failed to find a solution and I've run out of ideas.
I have two angles which I employ to define some transformation coefficients. Now, I don't actually have the values for those angles in my real data, I have the coefficients and I need to recover the angles.
I thought the arctan2 function would take care of this, but there are cases where it fails to recover the proper a1 angle and instead returns its 180 complement, which later affects the recovery of the a2 angle.
What am I doing wrong and how can I recover the a1, a2 angles properly?
import numpy as np
# Repeat 100 times
for _ in range(100):
# Define two random angles in the range [-pi, pi]. I do not have these
# angles in my actual data, I have the A,B,C coefficients shown below.
a1, a2 = np.random.uniform(-180., 180., (2,))
# Transformation coefficients using the above angles.
# This is the data I actually have.
a1_rad, a2_rad = np.deg2rad(a1), np.deg2rad(a2) # to radians
A = - np.sin(a1_rad) * np.sin(a2_rad)
B = np.cos(a1_rad) * np.sin(a2_rad)
C = np.cos(a2_rad)
# Recover a1 using 'arctan2' (returns angle in the range [-pi, pi])
a1_recover = np.arctan2(-A / B, 1.)
# Now obtain sin(a2), used below to obtain 'a2'
sin_a2 = -A / np.sin(a1_recover)
# Recover a2 using 'arctan2', where: C = cos(a2)
a2_recover = np.arctan2(sin_a2, C)
# Print differences.
a1_recover = np.rad2deg(a1_recover)
print("a1: {:.2f} = {} - {}".format(a1 - a1_recover, a1, a1_recover))
a2_recover = np.rad2deg(a2_recover)
print("a2: {:.2f} = {} - {}\n".format(a2 - a2_recover, a2, a2_recover))

When a2_rad equals 0, (A, B, C) equals (0, 0, 1) no matter what a1_rad equals. So the transformation is not 1-to-1. Therefore there is no well-defined inverse.
def ABC(a1, a2):
a1_rad, a2_rad = np.deg2rad(a1), np.deg2rad(a2) # to radians
A = - np.sin(a1_rad) * np.sin(a2_rad)
B = np.cos(a1_rad) * np.sin(a2_rad)
C = np.cos(a2_rad)
return A, B, C
print(ABC(0, 0))
# (-0.0, 0.0, 1.0)
print(90, 0)
# (-0.0, 0.0, 1.0)
print(-90, 0)
# (-0.0, 0.0, 1.0)
A similar problem happens at the opposite (South) pole. Within the limits of floating point accuracy, all these values (of the form ABC(a1, 180)) are essentially equal too:
ABC(1, 180)
# (-2.1373033680837913e-18, 1.2244602795081332e-16, -1.0)
ABC(0, 180)
# (-0.0, 1.2246467991473532e-16, -1.0)
ABC(90, 180)
# (-1.2246467991473532e-16, 7.498798913309288e-33, -1.0)
You can think of a1, a2 as coordinates on a unit sphere where a1
represents the angle away from the x-axis (more often called theta) and a2
represents the angle away from the z-axis (often called phi).
A,B,C represents the same point on the unit sphere in Cartesian coordinates.
Usually spherical coordinates restrict a1 to the range [0, 2*pi) and a2 to the range [0, pi].
Even with this restriction, the North and South poles have more than one (actually infinite number of) valid representation.

You cannot restore angle sign information because it was loosed in A,B calculation (formation).
8 possible combinations of sin/cos signs give only 4 results of A/B signs (and sign of cos(a2) cannot help here).
Note that for spherical coordinates inclination range is only 0..Pi

You should use np.arctan2(-A , B) instead of np.arctan2(-A / B, 1.). With the latter you are losing information: A = -1 and B = 1 will give the same result as A - 1 and B = -1, hence the 180 mismatch sometimes.
If you restrict a2 to be in (0,180) then you can recover the angles. Note that with this restriction a2 can be recovered as acos(C). (I've tried this but since my program is in C it might not be helpful)

how to rotate a triangle pygame

I have this triangle in pygame
triangle = pygame.draw.polygon(window, (210,180,140), [[x, y], [x -10, y -10], [x + 10, y - 10]], 5)
that i need to rotate towards the mouse, very much like the center arrow in this gif: http://i.stack.imgur.com/yxsV1.gif. Pygame doesn't have a built in function for rotating polygons, so I'll need to manually move the three points in a circle, with the lowermost point [x,y] pointing towards the coords of the mouse. The variables I have are:
the distance between the center of the triangle and the circle i want it to rotate along (i.e. the radius)
the distance from the center to the mouse coordinates
the coordinates of the lowermost point of the triangle [x,y] and the other two sides
with this information, how can I use trigonometry to rotate all three sides of the triangle so that the bottom point allways faces the mouse position?
EDIT: this is what I've got so far, but it only manages to move the triangle back and forth along a diagonal instead of rotating.
def draw(self):
curx,cury = cur
#cur is a global var that is mouse coords
angle = math.atan2(self.x - curx, self.y - cury)
distance = math.sqrt(200 - (200 * math.cos(angle)))
x = self.x + distance
y = self.y + distance
triangle = pygame.draw.polygon(window, (210,180,140), [[x, y], [x - 10,y - 10], [x + 10,y - 10]], 5)

Edit: Thinking about this again this morning there's another way to do this since the polygon is a triangle. Also the math is potentially easier to understand, and it requires less calculation for each point.
Let Cx and Cy be the center of the circle inscribing the triangle. We can describe the equation of a circle using the parametric equation:
F(t) = { x = Cx + r * cos(t)
{ y = Cy + r * sin(t)
Where r is the radius of the circle, and t represents the angle along the circle.
Using this equation we can describe the triangle using the points that touch the circle, in this case we'll use t = { 0, 3 * pi / 4, 5 * pi / 4 } as our points.
We also need to calculate the angle that we need to rotate the triangle so that the point that was at t = (0) is on a line from (Cx, Cy) to the mouse location. The angle between two (normalized) vectors can be calculated by:
t = acos(v1 . v2) = acos(<x1, y1> . <x2, y2>) = acos(x1 * x2 + y1 * y2)
where . represents the dot product, and acos is the inverse cosine (arccos or cos^-1).
From these two equations we can easily create a python function which, given the center of the triangle/circle, the radius of the circle, and the location of the mouse, returns a list of tuples representing the x-y coordinates of the triangle. (For the example the center and mouse position are tuples of the form (x, y))
def get_points(center, radius, mouse_position):
# calculate the normalized vector pointing from center to mouse_position
length = math.hypot(mouse_position[0] - center[0], mouse_position[1] - center[1])
# (note we only need the x component since y falls
# out of the dot product, so we won't bother to calculate y)
angle_vector_x = (mouse_position[0] - center[0]) / length
# calculate the angle between that vector and the x axis vector (aka <1,0> or i)
angle = math.acos(angle_vector_x)
# list of un-rotated point locations
triangle = [0, (3 * math.pi / 4), (5 * math.pi / 4)]
result = list()
for t in triangle:
# apply the circle formula
x = center[0] + radius * math.cos(t + angle)
y = center[1] + radius * math.sin(t + angle)
result.append((x, y))
return result
Calling this function like this:
from pprint import pprint
center = (0,0)
radius = 10
mouse_position = (50, 50)
points = get_points(center, radius, mouse_position)
pprint(points)
produces:
[(7.071067811865475, 7.0710678118654755),
(-10.0, 1.2246467991473533e-15),
(-1.8369701987210296e-15, -10.0)]
which is the three points (x, y) of the triangle.
I'm going to leave the original method below, since it's the way that modern computer graphics systems (OpenGL, DirectX, etc.) do it.
Rotation about the centroid of a arbitrary polygon is a sequence of three distinct matrix operations, Translating the object so that the centroid is at the origin (0,0), applying a rotation, and translating back to the original position.
Calculating the centroid for an arbitrary n-gon is probably outside the scope of an answer here, (Google will reveal many options), but it could be done completely by hand using graph paper. Call that point C.
To simplify operations, and to enable all transformations to be applied using simple matrix multiplications, we use so called Homogeneous coordinates, which are of the form:
[ x ]
p = | y |
[ 1 ]
for 2d coordinates.
Let
[ Cx ]
C = | Cy |
[ 1 ]
The general form of the translation matrix is:
[ 1 0 Vx ]
T = | 0 1 Vy |
[ 0 0 1 ]
Where <Vx, Vy> represents the translation vector. Since the goal of the translation is to move the centroid C to the origin, Vx = -Cx and Vy = -Cy. The inverse translation T' is simply Vx = Cx, Vy = Cy
Next the rotation matrix is needed. Let r be the desired clockwise rotation angle, and R be the general form of the rotation matrix. Then,
[ cos(r) sin(r) 0 ]
R = | -sin(r) cos(r) 0 |
[ 0 0 1 ]
The final transformation matrix is therefore:
[ 1 0 -Cx ] [ cos(r) sin(r) 0 ] [ 1 0 Cx ]
TRT' = | 0 1 -Cy | * | -sin(r) cos(r) 0 | * | 0 1 Cy |
[ 0 0 1 ] [ 0 0 1 ] [ 0 0 1 ]
Which simplifies to:
[ cos(r) sin(r) cos(r)*Cx-Cx+Cy*sin(r) ]
|-sin(r) cos(r) cos(r)*Cy-Cy-Cx*sin(r) |
[ 0 0 1 ]
Applying this to a point p = (x,y) we obtain the following equation:
p' = { x' = Cx*cos(r)-Cx+Cy*sin(r)+x*cos(r)+y*sin(r)
{ y' = -Cx*sin(r)+Cy*cos(r)-Cy-x*sin(r)+y*cos(r)
In Python:
def RotatePoint(c, p, r):
x = c[0]*math.cos(r)-c[0]+c[1]*math.sin(r)+p[0]*math.cos(r)+p[1]*math.sin(r)
y = -c[0]*math.sin(r)+c[1]*math.cos(r)-c[1]-p[0]*math.sin(r)+p[1]*math.cos(r)
return (x, y)
After typing all that I realize that your object may already be centered on the origin, in which case the function above simplifies to x=p[0]*math.cos(r)+p[1]*math.sin(r) and y=p[0]*math.sin(r)+p[1]*math.cos(r)
I put some faith in Wolfram Alpha here, rather than multiplying everything out by hand. If anyone notices any issues, feel free to make the edit.

Angle between two pairs of azimuth and altitude?

I have a solar panel pointing (it's normal vector) in some direction. I want to calculate the angle between that and the current position of the sun. I am using pyephem and I have this information in two pairs of azimuth and altitude.
panel_az = ephem.degrees('180')
panel_alt = ephem.degrees('45')
sun_az = ephem.degrees('245')
sun_alt = ephem.degrees('22')
What is the easiest way to find the angle between the panel's normal vector and the vector pointing towards the sun?

The library offers a separation() function that gives the angle between two spherical coordinates; look near the bottom of this section of the Quick Reference:
http://rhodesmill.org/pyephem/quick.html#other-functions
I think you will get the angle you are seeking if you run:
a = ephem.separation((panel_az, panel_alt), (sun_az, sun_alt))
print a
Good luck!

Convert both to vectors first:
z = sin(altitude)
hyp = cos(altitude)
y = hyp*cos(azimuth)
x = hyp*sin(azimuth)
vector = (x,y,z)
Then calculate the angle between the vectors (say a and b) using cross and dot products.
angle = atan2(norm(cross(a,b)), dot(a,b))
For cross use:
def cross(a, b):
c = [a[1]*b[2] - a[2]*b[1],
a[2]*b[0] - a[0]*b[2],
a[0]*b[1] - a[1]*b[0]]
return c
For dot use:
def dot(a, b):
c = [ a[i] * b[i] for i in range(len(a)) ]
return c
For norm use:
def norm(a):
mag = sqrt(sum(a[i]*a[i] for i in range(len(a))))
c = [ a[i]/mag for i in range(len(a)) ]
return c

clear,clc,clf;
for n=[15 46 74 105 135 166 196 227 258 288 319 349];
delta=23.45*(sind(360*(284+n)/365));
phi=31.2;
omega=acosd(-tand(phi).*tand(delta));
omega1=-omega:0.1:omega;
alt=(sind(delta).*sind(phi))+(cosd(delta).*cosd(omega1).*cosd(phi));
alt1=asind(abs(alt))
azm=(cosd(delta).*sind(omega1))./cosd(alt);
azm1=asind(azm)
plot(azm1,alt1,'b');hold on;
end
grid on
xlabel('Solar Azimuth');ylabel('Solar altitude');
text(0,85,'noon')
hold off