Start of line (yellow) and axes are at [xc,yc,zc]
End of line is at [xp,yp,zc].
a,b, c are the angles which line makes in space.
What I need are the angles which line's projections (black line) create on xy,yz and xz planes.
A_y_to_z: Projected line's angle from y axis to z axis on xz plane.
A_z_to_x: Angle from z to x axis on zx plane.
A_x_to_y: Angle from x to y axis on xy plane.
Writing code on Matlab
You can calculate the projection angle to any plane by:
Obtaining the direction of the line, d = (xp - xc, yp - yc, zp - zc)
Normalizing d
Calculating the dot-product with the normal of the plane, dot(d, n) = d.x * n.x + d.y * n.y + d.z * n.z
Calculating the angle to the normal by a = acos(dot(d, n))
Finally obtaining the angle to the plane by taking b = 90 - a (assuming units in degrees - NB most math library functions use radians)
Special case: if dot(d, n) < 0, then the angle a will be greater than 90 degrees. In this case if you only want the acute angle, do b = a - 90 instead of 90 - a.
e.g. To calculate the angle to the xy plane, use n = (0, 0, 1), i.e. the z-axis, which is the normal to that plane.
Related
I place an IMU on my wrist and extend my arm as shown in the photo below. I spin in a circle once, while my arm remains in the fixed position. I calculate the euler pitch and quaternion angle. In the photo below, the euler pitch remains approximately constant ( my hand shakes a bit ), while the quaternion angle increases linearly it seems. My data is located here: (sample_data.csv)
Question:
What changes should I make to measure the angle from the quaternion? ( I suspect it is in form of RPR', adjust to world axis, but am unsure what P would be )
Matlab code:
clc;
clear;
table = readtable("sample_data.csv", 'Delimiter', ',');
euler_angles = zeros(length(table.w),1);
quaternion_angles = zeros(length(table.w),1);
for idx = 1:length(table.w)
w = table.w(idx);
x = table.x(idx);
y = table.y(idx);
z = table.z(idx);
euler_angles(idx) = getEulerAngle(w,x,y,z);
quaternion_angles(idx) = getQuaternionAngle(w,x,y,z);
end
figure(1);
clf;
hold on;
plot(euler_angles,'ro');
ylabel("Angle in deg");
xlabel("Sample");
plot(quaternion_angles, 'bo');
hold off;
legend('Euler','Quaternion');
function angle = getQuaternionAngle(w,x,y,z)
q = quaternion(w,x,y,z);
angle = acosd(w);
end
function angle = getEulerAngle(w, x, y, z)
mag = (2*(y * w - z * x));
angle = rad2deg(asin(mag));
end
Assuming you rotate around an axis [x;y;z] , the post you refer to states that the quaternion associated with the rotation is of the form:
q(1) = cos(r/2);
q(2) = sin(r/2)*x;
q(3) = sin(r/2)*y;
q(4) = sin(r/2)*z;
Where r is the angle in radians and [x;y;z] is the 3d vector representing the axis around which you rotate.
To get the instantaneous rotation r you need to calculate 2*acos(q(1)) for an angle in radians, or 2*acosd(q(1)) for an angle in degrees.
Now plotting the data from your csv file gives the following:
Which is coherent with the assumption that w is the first coordinate of your quaternion, and that you indeed rotate mostly around z.
Euler angles (Or more likely Tait Bryan angles) are a different way to represent a rotation. A rotation is represented by a composition of 3 elemental rotations. These 3 rotations are sometimes called yaw, pitch and roll. If you want to fully represent your rotation, you will have to calculate all of these 3 elemental rotations:
table = readtable("sample_data.csv", 'Delimiter', ',');
w = table.w;
x = table.x;
y = table.y;
z = table.z;
[yaw,pitch,roll] = getAngles(w, x, y, z);
quaternion_angles = getQuaternionAngle(w);
figure; plot([quaternion_angles,yaw,pitch,roll]);
legend({'quat','yaw','pitch','roll'})
function angle = getQuaternionAngle(w)
angle = 2*acosd(w);
end
function [yaw,pitch,roll] = getAngles(w, x, y, z)
yaw = atan2d(2*(y.*z + w.*x), w.*w - x.*x - y.*y + z.*z);
pitch = asind(-2*(x.*z-w.*y));
roll = atan2d(2*(x.*y + w.*z), w.*w + x.*x - y.*y - z.*z);
end
See? The angle around Z is represented by the roll (the discontinuity is due to the use of arctan). When you rotate around yourself, the angle increases steadily between 0 and 360º.
You can also see that you have a bit of yaw, i.e your IMU is a bit tilted
I have two points in my coordinate system (x,y) that I want to know the angle of their line and x-axis.
I use swift for solving this but I can't get the angle.
I need this angle in radians to use it in the following equation:
(x0 + r cos theta, y0 + r sin theta)
r : radius of circle
If you have two points, (x0, y0) and (x1, y1), then the angle of the line joining them (relative to the X axis) is given by:
theta = atan2((y1 - y0), (x1 - x0))
The angle between a line, for reference, let's call this A, defined by two points p1=(x1,y1),p2=(x2, y2) and the x-axis is related to finding the slope/ gradient of the line, A.
# To solve a problem you sometimes have to simplify it and then work up to the full solution"
Let's start by obtaining the gradient of the line A.
The gradient of line A:
slope = (y2 - y1)/(x2 - x1)
for a straight line, that makes an angle theta with the x-axis
tan(theta) = slope = (change in y) / (change in x)
Therefore, theta = tan_inverse (slope)
theta = atan(slope)
I am new to MATLAB, but have worked with javascript and other programming languages.
I am writing a MATLAB program that will generate an equilateral triangle given the side length, an x coordinate, a y coordinate and an angle of rotation. It is working as intended except for the rotations.
I am using a rotation matrix to rotate the triangle. This works, except it rotates around the origin instead of rotating on the spot. (see below example).
90 degree Rotations Example
In order to rotate it on the spot I think I need to calculate the centre of the triangle and then rotate around that(somehow). I am not sure how to do this or if there is an easier/better way to do this. I did see there is a rotate function, but from what I have seen, it is for spherical space, not Cartesian planes.
Code is below, sorry for the mess:
function [ side, coord1,coord2 ] = equilateral(side, x,y, rotation)
%EQUILATERAL- given a side length and x,y, coordinates as inputs, the
%function plots an equilateral triangle an angle of rotation can be
%given as an input as well. This will rotate the trianlge around the x
%and y coordinates given.
%rotation argument is not required. If not given, angle is 0
if(exist('rotation','var'))
angle = rotation;
else
angle = 0;
end
%rotation matrix
R = [cos(angle), -sin(angle); sin(angle), cos(angle)];
%Make the axis equal so the triangles look equilateral
axis equal;
%max horizontal x coordinate
x2 = x + side;
%max horiontal y coordinate (equal to original y coordinate)
y2 = y;
%height of the triangle at midpoint (perpendicular height)
h = side*sin(pi/3) + y;
%coordinates of midpoint/top vertice
mid = [x2-(0.5*side), h];
%min coordinates
coord1 = [x,y];
%max coordinates
coord2 = [x2,y2];
if (angle > 0)
coord1 = coord1*R;
coord2 = coord2*R;
mid = mid*R;
end
%plot the base of the triangle
plot(linspace(coord1(1),coord2(1)), linspace(coord1(2),coord2(2)));
hold on
%plot the first side from inital coords to midpoint
plot(linspace(coord1(1),mid(1)), linspace(coord1(2),mid(2)));
%plot second side from mid point to max coords
plot(linspace(mid(1),coord2(1)), linspace(mid(2),coord2(2)));
end
I am open to any suggestions for improvements to the code/help to clean it up as well as help with the rotation issues. Thanks for the help.
The rotation matrix
[cos(theta), -sin(theta)
sin(theta), cos(theta)]
rotates a set of points of an angle theta about the origin of the axes.
In order to rotate a point around a given point, you have to:
-shift your points so that the rotatioin point concides with the origin of the axes
- rotate the point using the rotation matrix
- shift back your points
The following code is a modified version of your function in which the proposed approach has been implemented.
In the code I've set the rotation point as the centre of gravity
XR=x+side/2
YR=y+h*.3
nevertheless, you can compute them in a different way.
function [ side, coord1,coord2 ] = equilateral(side, x,y, rotation)
%EQUILATERAL- given a side length and x,y, coordinates as inputs, the
%function plots an equilateral triangle an angle of rotation can be
%given as an input as well. This will rotate the trianlge around the x
%and y coordinates given.
%rotation argument is not required. If not given, angle is 0
if(exist('rotation','var'))
% angle = rotation;
% Convert the angle from deg to rad
angle = rotation*pi/180;
else
angle = 0;
end
%rotation matrix
R = [cos(angle), -sin(angle); sin(angle), cos(angle)];
%Make the axis equal so the triangles look equilateral
axis equal;
%max horizontal x coordinate
x2 = x + side;
%max horiontal y coordinate (equal to original y coordinate)
y2 = y;
%height of the triangle at midpoint (perpendicular height)
h = side*sin(pi/3) + y;
%coordinates of midpoint/top vertice
mid = [x2-(0.5*side), h];
%min coordinates
coord1 = [x,y];
%max coordinates
coord2 = [x2,y2];
plot([coord1(1) coord2(1) mid(1) coord1(1)],[coord1(2) coord2(2) mid(2) coord1(2)],'r')
hold on
%
% Define the coord of the point aroud with to turn
%
XR=x+side/2
YR=y+h*.3
plot(XR,YR,'o','markerfacecolor','k','markeredgecolor','k')
if (angle > 0)
% coord1 = coord1*R;
% coord2 = coord2*R;
% mid = mid*R;
% Shift the triangle so that the rotation point coincides with the origin
% of the axes
r_coord1 = (coord1-[XR YR])*R+[XR YR];
r_coord2 = (coord2-[XR YR])*R+[XR YR];
r_mid = (mid-[XR YR])*R+[XR YR];
%
% Plot the rotated triangle
plot([r_coord1(1) r_coord2(1) r_mid(1) r_coord1(1)],[r_coord1(2) r_coord2(2) r_mid(2) r_coord1(2)],'r')
end
% % %
% % % %plot the base of the triangle
% % % plot(linspace(coord1(1),coord2(1)), linspace(coord1(2),coord2(2)));
% % % hold on
% % % %plot the first side from inital coords to midpoint
% % % plot(linspace(coord1(1),mid(1)), linspace(coord1(2),mid(2)));
% % % %plot second side from mid point to max coords
% % % plot(linspace(mid(1),coord2(1)), linspace(mid(2),coord2(2)));
end
I've also made a couple of additional modification:
I've added a conversion of the input angle from deg to rad; you can discard it if you assume the input is already in rad
I've updated the way to plot the triangle.
As a suggestion, you can use nargin to thest the number of input to your function and varargin to inspect them instead of testing in they exist.
Hope this helps.
Qapla'
I would like rotate a vector in MATLAB and right after check the angle between the original and the rotated one:
v = [-1 -12 5]; %arbitrarily rotated vector
theta =30; %arbitrary angle to rotate with
R = [cosd(theta) -sind(theta) 0; sind(theta) cosd(theta) 0; 0 0 1]; %rotate around Z axis
vR = R*v'; %calculate the new vector
angle=atan2d(norm(cross(v,vR)),dot(v,vR));
%the angle between the old and rotated vector, also do normalisation before.
%atan2d is better to resolve extremely small angle
angle =
27.6588
%THIS is the problem
As you can see I rotated with 30° but when checking back it's different.
You are not actually calculating the same angle. Consider the situation where your input vector is v = [0, 0, 1] (i.e., a vertical line). If you rotate the vertical line about the z-axis by 30 deg then you just get the same vertical line again, so vR = [0, 0, 1]. The angle between v and vR would be 0 based on your analysis because you are calculating the actual angle between two vectors that intersect somewhere.
So, if you want to calculate the angle between two vectors then I believe your code is correct. But, if you want to calculate the amount of rotation in a specific frame (i.e., the z-axis), then you'll have to project v and vR onto the x-y plane first before using your formula:
v = [-1 -12 5]; %arbitrarily rotated vector
theta =30; %arbitrary angle to rotate with
R = [cosd(theta) -sind(theta) 0; sind(theta) cosd(theta) 0; 0 0 1]; %rotate around Z axis
vR = R*v'; %calculate the new vector
angle=atan2d(norm(cross(v,vR)),dot(v,vR)); %calculate the angle between the vectors
% copy over the vectors and remove the z-component to project onto the x-y
% plane
v_xy = v;
v_xy(3) = 0;
vR_xy = vR;
vR_xy(3) = 0;
angle_xy=atan2d(norm(cross(v_xy,vR_xy)),dot(v_xy,vR_xy)); %calculate the angle between the vectors in the xy-plane
Edit: Note you still won't be able to get the angle for the vertical line case (there is a singularity in the solution there). Also, my suggestion works only for the case of a z-axis rotation. To do this for any general rotation axis just requires a bit more math:
Say you have an axis defined by unit vector a, then rotate vector v about axis a to get the new vector vR. Project v and vR onto the plane for which a is normal and you get vectors p and pR:
p = v - dot(v,a)*a;
pR = vR - dot(vR,a)*a;
Then you find the angle between these projected vectors based on your formula:
angle = atan2d(norm(cross(p,pR)),dot(p,pR));
I have a point in 3d space (x,y,z). I want to move radially outward from that point discretely (say for r=1 and r=2). In the x,y plane I can simply move outward by stepping ((x+r cos(theta)), (y+r sin(theta)), z) with r = 1 or 2 and theta varying every, say 10 degrees.
However, I am unsure how to describe this movement if I want to have lines moving outward on a tilted plane and step my lines within this plane.
I thought it would be just using spherical coordinates. But if I'm drawing lines from a center point using (x=rho sin phi cos theta, y=..., z=...) won't that form a cone rather than a circle tilted on a plane?
P.S. Will be implementing this in MATLAB
You could first make the coordinates going outwards from P0 and then rotate the coordinates using a rotation matrix.
So you take points P for all R's and thetas, as MBo pointed out:
P = [ P0x + R * cos(theta); P0y + R * sin(theta); 0 ]
Then you make a rotation matrix that rotates the XY plane with the angles you want
If you multiply that with your coordinates you get the rotated coordinates. For example a 90 degree rotation about the Z axis for the point [1,0,0]:
However you probably want to rotate about the point P0 and not about the origin, then you have to make an affine matrix with the following translation:
tx = x- r00 * x - r01 * y - r02 * z
ty = y- r10 * x - r11 * y - r12 * z
tz = z- r20 * x - r21 * y - r22 * z
And then make an affine transformation matrix with T and R (designated as M in the figure, sorry):
In this figure Q are the old coordinates and Q' the new coordinates.
I had a similar problem and used this answer and adjusted it to your problem:
%input point and rotated plane
p0 = [10;10;10;1]; % the last entry is your homogeneous dimension
r0 = [45,45,45]; r0 = r0*pi/180;
%rotation to plane
Rx=[1 0 0 0;
0 cos(r0(1)) sin(r0(1)) 0;
0 -sin(r0(1)) cos(r0(1)) 0;
0 0 0 1];
Ry=[cos(r0(2)) 0 -sin(r0(2)) 0;
0 1 0 0;
sin(r0(2)) 0 cos(r0(2)) 0;
0 0 0 1];
Rz=[cos(r0(3)) sin(r0(3)) 0 0;
-sin(r0(3)) cos(r0(3)) 0 0;
0 0 1 0;
0 0 0 1];
R = Rz*Ry*Rx; A = R;
T = ( eye(3)-R(1:3,1:3) ) * p0(1:3); %calculate translation to rotate about the point P0
A(1:3,4) = T; % to rotate about the origin just leave out this line
%make coordinates for the points going outward from p0
nangles = 36; anglestep = 2*pi/nangles;
nradii = 2; radiistep = 1;
thetas = anglestep:anglestep:2*pi;
rs = radiistep:radiistep:nradii*radiistep;
npoints = nradii*nangles;
coordinates = zeros(4,npoints); curpoint = 0;
for itheta = 1:nangles; for iradius = 1:nradii;
curpoint = curpoint+1;
coordinates(:, curpoint) = p0+rs(iradius)*[cos(thetas(itheta));sin(thetas(itheta));0;0];
end; end
coordinates_tilted = A*coordinates; %rotate the coordinates to the new plane
Which results in this figure:
figure;
scatter3(coordinates_tilted(1,:),coordinates_tilted(2,:),coordinates_tilted(3,:), 'MarkerEdgeColor', 'green')
hold on
scatter3(coordinates(1,:),coordinates(2,:),coordinates(3,:), 'MarkerEdgeColor', 'red')
legend('tilted', 'original')
Or plot them as lines:
%or as lines
coorarray = reshape(coordinates, [4 nradii nangles]);
Xline = squeeze(coorarray(1,:,:));
Yline = squeeze(coorarray(2,:,:));
Zline = squeeze(coorarray(3,:,:));
coorarray_tilted = reshape(coordinates_tilted, [4 nradii nangles]);
Xline_tilted = squeeze(coorarray_tilted(1,:,:));
Yline_tilted = squeeze(coorarray_tilted(2,:,:));
Zline_tilted = squeeze(coorarray_tilted(3,:,:));
figure;
plot3(Xline,Yline,Zline, 'r');
hold on
plot3(Xline_tilted,Yline_tilted,Zline_tilted, 'g');
legend( 'original', 'tilted')
Does this answer your question? These are now points at all multiples of 36 degree angles at a distance of one and two from point P0 in the plane that is tilted 45 degrees on all axes around the point P0. If you need individual 'pixels' to designate your line (so integer coordinates) you can round the coordinates and that would be sort of a nearest neighbour approach:
coordinates_tilted_nearest = round(coordinates_tilted);
How is your tilted plane defined?
Define it with base point P0 and two perpendicular unit vectors U and V. It is not hard to get this representation from any other. For example, if normal vector of your plane have angles ax, ay, az with axes OX, OY, OZ respectively, it's normalized form is N = (nx, ny, nz) = (Cos(ax), Cos(ay), Cos(az)). You can choose arbitrary vector U (lying in the plane) as described here, and find V vector as vector product V = U x N
Then needed points are:
P = P0 + U * R * Cos(Theta) + V * R * Sin(Theta)