I'm trying to understand quaternions better, but I don't know how the math behind FromToRotation works. I tried looking this up but couldn't find any results.
You may know that a rotation can be represented by a quaternion of the following form:
cos (phi / 2)
sin (phi / 2) * axis.x
sin (phi / 2) * axis.y
sin (phi / 2) * axis.z
axis is the rotation axis and phi is the rotation angle. These are the two measures you need to define your quaternion.
There are multiple rotations that map a vector from to another vector to. The shortest rotation is the one where the axis is perpendicular to both of the vectors. Hence, the axis is:
axis = normalize(from x to)
x denotes the cross product.
And the angle is the angle between the two vectors:
phi = acos(dot(from, to) / (norm(from) * norm(to))
norm is the vector norm or vector length.
With these values, you can then calculate the quaternion.
Related
Problem: I have two vectors. I know the starting point of one vector, its direction, its magnitude. I know the starting point of the other vector and its magnitude. I need to find the direction of second vector as well as the position of intersection.
Vector A: Vector B:
Position = Known Position = Known
Direction= Known Direction= UNKNOWN
Magnitude= Known Magnitude= Known
To Find: Point of intersection.
Is it possible to find the point of intersection, with the given parameters? If so then how?
Application: I want to find the position where a player would be found based on the velocity he is moving, and shoot a bullet at him at the moment he would be found, taking into account the time taken for the bullet to reach that virtual target position.
Following on from the comments I'm going to take a leap here and answer your ultimate question directly.
Say the player is, at the initial time, at a point p and travelling with velocity v; your gun is at position q and shoots a bullet in any direction at speed s:
The length of OP is vΔt and that of Q sΔt. The angle a is given by the dot product:
We can then use the cosine rule to solve for Δt:
Written in this form, we can easily see that it is a quadratic equation, and thus directly solve for Δt using the Quadratic formula:
There are a few cases we need to consider here:
v < s: need to take the positive root only as otherwise we would get negative time.
v > s and dot(PQ, s) < 0: the bullet will never catch the player.
v > s and dot(PQ, s) > 0: take the negative root this time, as the positive root is for a backwards travelling player (longer time; this is also the case presented in the diagram above).
Having the correct value for Δt from above will then enable us to find the intersection point o, and therefore the intended direction d:
Note that d is not normalized. Also, this solution works for 3D too, unlike an approach with angles.
Let subscript 1 mark the player, and subscript 2 mark the AI:
initial: position (x_i, y_i)
angle: alpha_i
speed: u_i
The positions as a function of time t are :
player: (x_1 + u_1 * t * cos(alpha_1), y_1 + u_1 * t * sin(alpha_1))
AI's bullet: (x_2 + u_2 * t * cos(alpha_2), y_2 + u_2 * t * sin(alpha_2))
You have 2 uknowns:
t - the time of collision
alpha_2 - the angle the AI should shoot
The collision happens when Xs and Ys match. i.e.:
x_1 + u_1 * t * cos(alpha_1) = x_2 + u_2 * t * cos(alpha_2)
y_1 + u_1 * t * sin(alpha_1) = y_2 + u_2 * t * sin(alpha_2)
So,
alpha_2 = arcos( (x_1 + u_1 * t * cos(alpha_1) - x_2) / u_2 * t )
and also
alpha_2 = arcsin( (y_1 + u_1 * t * sin(alpha_1) - y_2) / u_1 * t )
substitue your values and equate these to expressions of alpha_2 to obtain t, then you can substitue t in either expression to obtain the angle alpha_2.
We have these logarithmic spirals which are circling around the centre of the coordinate system:
x = ebθ cos(θ)
y = ebθ sin(θ)
where the ebθ is the distance between the point (which is on the spiral) and the centre; and the θ is the angle between the line connecting the point and the origin and the axis x.
Consider a spiral where the angle is θ ϵ <0,10π> and the parameter is b=0.1. By thickening points on the spirals (and the angle θ) calculate the circumference with the relative precision better than 1%. Draw the spiral!
I'm preparing for a (MATLAB) test and I'm stuck with this exercise. Please help, any hint is appreciated.
Start by computing a list of x,y for your range of theta and value of b. For more accurate results, have your theta increment in smaller steps (I chose 5000 arbitrarily). Then, its simply computing the distance for each pair of consecutive points and summing them up.
t = linspace(0,10*pi,5000);
b = 0.1;
x = exp(b*t).*cos(t);
y = exp(b*t).*sin(t);
result = sum(sqrt((x(2:end) - x(1:end-1)).^2 + (y(2:end)-y(1:end-1)).^2))
I found a bug in my code which affects the subsequent calculations. I have two state vectors, one for the actual robot's movement and the second one for the estimated state vector. For the robot
robot = [101.3203; % x
170.6334; % y
2.1103]; % theta in radian
From this position, the robot makes an observation to a beacon located at <200,0> (i.e. <x,y>). Now, the range and angle between the robot and beacon is computed as following
sigma_phi = (degtorad(1))/2; % noise of the angle
sigma_r = (0.001)/2; % noise of the range
% range with some noise from robot to beacon
qr = sqrt((200 - robot(1))^2 + (0 - robot(2))^2) + sigma_r*randn();
% angle with some noise in radian from the robot to beacon
phi = wrapToPi(atan2(0 - robot(2), 200 - robot(1)) - robot(3) + sigma_phi*randn());
the angle is now phi = 3.1285 in radian. For the estimated state vector, I have
Mu = [101.2143; % x
171.0308; % y
2.094]; % theta in radian
From this estimated position, the estimated state vector makes an observation to a beacon at <200,0> (i.e. <x,y>). Now, the range and angle between the estimated state vector and beacon is computed with no noise as following
q = sqrt((200 - Mu(1))^2 + (0 - Mu(2))^2);
theta = wrapToPi(atan2(0 - Mu(2), 200 - Mu(1)) - Mu(3));
the angle is now theta = -3.1410 in radian. My question is why phi is in the opposite direction of theta even though the robot and the estimated state vector have close values??
You say theta = -3.1410, measured in radians. That's very close to -pi radians. Add a little noise to your estimation, and you can easily obtain an angle that is slightly less
than (more negative than) -pi radians.
If you give wrapToPi an angle slightly less than -pi radians, it will return an angle
that is slightly less than pi radians instead.
That is, as far as wrapToPi is concerned, there is not much difference between
your angles phi and theta. If you compute wrapToPi(phi - theta), which is what
you should do if you really want to know how different two directional angles are,
the result will be a relatively small number (something less than 0.02, I think).
Imagine a dome with its centre in the +z direction. What I want to do is to move that dome's centre to a different axis (e.g. 20 degrees x axis, 20 degrees y axis, 20 degrees z axis). How can I do that ? Any hint/tip helps.
Add more info:
I've been dabbling with rotation matrices in wiki for a while. The problem is, it is not a commutative operation. RxRyRz is not same as RzRyRx. So based on the way I multiple it I get a different final results. For example, I want my final projection to have 20 degrees from the original X axis, 20 degrees from original Y axis and 20 degrees from original Z axis. Based on the matrix, giving alpha, beta, gamma values 20 (or its corresponding radian) does NOT result the intended rotation. Am I missing something? Is there a matrix that I can just put the intended angles and get it at the end ?
Using a rotation matrix is an easy way to rotate a collection of (x,y,z) points. You can calculate a rotation matrix for your case using the equations in the general rotation section. Note that figuring out the angle values to plug into those equations can be tricky. Think of it as rotating about one axis at a time and remember that the order of your rotations (order of multiplications) does matter.
An alternative to the general rotation equations is to calculate a rotation matrix from axis and angle. It may be easier for you to define correct parameters with this method.
Update: After perusing Wikipedia, I found a simple way to calculate rotation axis and angle between two vectors. Just fill in your starting and ending vectors for a and b here:
a = [0.0 0.0 1.0];
b = [0.5 0.5 0.0];
vectorMag = #(x) sqrt(sum(x.^2));
rotAngle = acos(dot(a,b) / (vectorMag(a) * vectorMag(b)))
rotAxis = cross(a,b)
rotAxis =
-0.5 0.5 0
rotAngle =
1.5708
I have a set of acceleration values along the x, y, z directions (sensor body frame). The sensor tilted and changed orientation many times during operation. Using the gyroscope, magnetometer and accelerometer, I was able to fuse and extract the Roll, Pitch and Yaw angles all in quaternion format ( cal it Q).
Now, to derotate the acceleration values into global X, Y, Z positions. Do I just simply
1. convert the acceleration values into quaternion by setting the scalar part to 0 (call it Acc)
2. Multiply the acceleration quaternion as follows : Qtranspose * Acc * Q??
Or is my conclusion wrong? how do we derotate the values?
thanks
If you already have an accurate quaternion orientation q, then the 'unrotated' vector of the accelerations is as you said:
Ag = q* A q
or
Ag = q A q*
depending on which convention you are using for quaternion multiplication. (just try both and see which one give you the right gravity vector).
NOTE that A and Ag are 'pure quaternions' - this is a quaternion with the scalar component as 0, and the axis component as your acceleration vector.
In other words if your quaternion is q = [w x y z], then A = [0 ax ay az]. You can then discard the scalar component of Ag to get the unrotated vector.
Make sure the order of the acceleration vector is consistent with your quaternion vector order.