I am having a hard time trying to understand the mathematical derivation of the equations I have included below. This piece of code is part of an example from a SparkFun IMU library that can be found here.
Could someone please help me understand the theory behind the use of arctan functions to estimate roll, pitch, and yaw? And how is the magnetometer data used to get the right yaw estimate? All the resources I could find online didn't answer these questions in a way that I could understand.
float roll = atan2(ay, az);
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
float heading;
if (my == 0)
heading = (mx < 0) ? PI : 0;
else
heading = atan2(mx, my);
heading -= EARTH_DECLINATION * PI / 180;
if (heading > PI) heading -= (2 * PI);
else if (heading < -PI) heading += (2 * PI);
// Convert everything from radians to degrees:
heading *= 180.0 / PI;
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
Assuming ay and az are the offset from origin given by the magnetometer then atan(ay, az) will give you the angle that produced that offset.
The sqrt(ay * ay + az * az) follows by the pythagorean theorem to give you the length of the third side of the "offset triangle" to be able to calculate pitch. The -ax comes from how pitch is defined.
atan2(y,x) is the angle theta as in the following diagram:
In three dimensions you have three planes and the atan2() is applied to a pair from x, y, and z depending on which plane you are calculating theta for (roll, pitch, yaw).
When at a steady velocity, (i.e. not changing speed or direction) the values ax, ay, az each measure the gravitational component of acceleration only. The value will be inaccurate under acceleration. You have to be a bit cleverer at that point by combining information from other sensors such as the gyro or magnetometer and the other accelerators - that is when stationary ax, ay, az sum to 1G - anything else and there is an additional acceleration in effect. An accelerator measures acceleration, but that includes acceleration due to gravity. That is when at a steady velocity, an accelerator is a tilt sensor relative to gravity.
The magnetometer calculation is determining an angle relative to north from its x and y components and compensating for the difference between true north and magnetic north (the magnetic declination). The x an y components of the magnetometer are at a maximum when they are aligned with a magnetic field, because they are mounted orthogonality, their relative values resolve to a single direction using atan2(mx,my). x and y are swapped from the conventional order because compass directions increase clockwise, while mathematically angles increase counter-clockwise.
In practice you need to use sensor function to combine information from the gyro (angular velocity), accelerator, and magnetometer and probably apply some heuristics too to track motion accurately. Each of these sensors have different confounding factors and some element of measurement overlap in the sense that a single external force can have an effect on more than one sensor. This can be used to discriminate different kinds of motion and attitude. It is complicated, which is probably why this does does not attempt to deal with it.
I am currently looking to implement an algorithm that will be able to compute the arc midpoint. From here on out, I will be referring to the diagram below. What is known are the start and end nodes (A and B respectively), the center (point C) and point P which is the intersection point of the line AB and CM (I am able to find this point without knowing point M because line AB is perpendicular to line CM and thus, the slope is -1/m). I also know the arc angle and the radius of the arc. I am looking to find point M.
I have been looking at different sources. Some suggest converting coordinates to polar, computing the mid point from the polar coordinates then reverting back to Cartesian. This involves sin and cos (and arctan) which I am a little reluctant to do since trig functions take computing time.
I have been looking to directly computing point M by treating the arc as a circle and having Line CP as a line that intersects the circle at Point M. I would then get two values and the value closest to point P would be the correct intersection point. However, this method, the algebra becomes long and complex. Then I would need to create special cases for when P = C and for when the line AB is horizontal and vertical. This method is ok but I am wondering if there are any better methods out there that can compute this point that are simpler?
Also, as a side note, I will be creating this algorithm in C++.
A circumference in polar form is expressed by
x = Cx + R cos(alpha)
y = Cy + R sin(alpha)
Where alpha is the angle from center C to point x,y. The goal now is how to get alpha without trigonometry.
The arc-midpoint M, the point S in the middle of the segment AB, and your already-calculated point P, all of them have the same alpha, they are on the same line from C.
Let's get vector vx,vy as C to S. Also calculate its length:
vx = Sx - Cx = (Ax + Bx)/2 - Cx
vy = Sy - Cy = (Ay + By)/2 - Cy
leV = sqrt(vx * vx + vy * vy)
I prefer S to P because we can avoid some issues like infinite CP slope or sign to apply to slope (towards M or its inverse).
By defintions of sin and cos we know that:
sin(alpha) = vy / leV
cos(alpha) = vx / leV
and finally we get
Mx = Cx + R * vx / leV
My = Cy + R * vy / leV
Note: To calculate Ryou need another sqrt function, which is not quick, but it's faster than sin or cos.
For better accuracy use the average of Ra= dist(AC) and Rb= dist(BC)
I would then get two values
This is algebraically unavoidable.
and the value closest to point P would be the correct intersection point.
Only if the arc covers less than 180°.
Then I would need to create special cases for when P = C
This is indeed the most tricky case. If A, B, C lie on a line, you don't know which arc is the arc, and won't be able to answer the question. Unless you have some additional information to start with, e.g. know that the arc goes from A to B in a counter-clockwise direction. In this case, you know the orientation of the triangle ABM and can use that to decide which solition to pick, instead of using the distance.
and for when the line AB is horizontal and vertical
Express a line as ax + by + c = 0 and you can treat all slopes the same. THese are homogeneous coordinates of the line, you can compute them e.g. using the cross product (a, b, c) = (Ax, Ay, 1) × (Bx, By, 1). But more detailed questions on how best to compute these lines or intersect it with the circle should probably go to the Math Stack Exchange.
if there are any better methods out there that can compute this point that are simpler?
Projective geometry and homogeneous coordinates can avoid a lot of nasty corner cases, like circles of infinite radius (also known as lines) or the intersection of parallel lines. But the problem of deciding between two solutions remains, so it probably doesn't make things as simple as you'd like them to be.
I am working on a game project using OpenGl. I am building a game from skeleton code I found online. I have a character that can move around in a 2D plane. (x and z, ie you are viewing the character from above.) I am currently stuck on making him rotate as he moves, and I can't seem to find a solution online that solves my problem.
At the moment when the character is being drawn he faces a certain way (along the arrow in my diagram below.). I can rotate him an arbitrary number of degrees from his default direction using glm::rotate.
I have updated the code to log the characters position from a frame ago when he moves, so I have this information:
character old position (known)-> O
character starting angle (unknown)-> |\
| \
| \
|(X)\
| \
V O <- character new position (known)
How do I compute the angle (X)? Is it possible with the information I have?
I have been doodling on a page trying to figure this out for the last hour but can't seem to figure it out. Thanks very much.
Yes. This answer gives you an example of how to do it: How to calculate the angle between a line and the horizontal axis?
Note however that that will give you the angle between the horizontal axsis and the point. You can however just add 90 degrees.
What you're doing sounds somewhat convoluted. From the description, it seems like you want a rotation matrix that matches the direction. There's really no need to calculate an angle. You can build the rotation matrix directly, which is easier and more efficient.
I'll illustrate the calculations with points/vectors in the xy-plane, since that's much more standard. It sounds like you're operating in the xz-plane. While that doesn't change things much, you might need slight changes because you have a left-handed coordinate system.
If you have the direction vector (difference between new position and old position), all you need to do is normalize it, and you already have what's needed for the rotation matrix. I'll write the calculation explicitly, but your matrix/vector library most likely has a method to normalize a vector.
float vx = nexPosX - oldPosX;
float vy = newPosY - oldPosY;
float s = 1.0f / sqrt(vx * vx + vy * vy);
vx *= s;
vy *= s;
vx is now the cosine of the rotation angle, and vy the sine of the rotation angle. Substituting this into the standard form of a rotation matrix, you get:
R = ( cos(phi) -sin(phi) ) = ( vx -vy )
( sin(phi) cos(phi) ) ( vy vx )
This is the absolute rotation for the new direction. If you need the relative rotation between old direction and new direction, it just takes a few more operations. Let's say you already calculated the normalized vectors for the old and new directions as (v1x, v1y) and (v2x, v2y). The cosine of the rotation angle is the scalar product of the two vectors:
cosPhi = v1x * v2x + v1y * v2y;
and the sine is the length of the cross product. Since both vectors are in the xy-plane, that's simply the z-component of the cross product:
sinPhi = v1x * v2y - v1y * v2x;
With these two values, you can directly build the rotation matrix again:
R = ( cosPhi -sinPhi )
( sinPhi cosPhi )
I want to find out the clockwise angle between 2 vectors(2D, 3D).
The clasic way with the dot product gives me the inner angle(0-180 degrees) and I need to use some if statements to determine if the result is the angle I need or its complement.
Do you know a direct way of computing clockwise angle?
2D case
Just like the dot product is proportional to the cosine of the angle, the determinant is proprortional to its sine. So you can compute the angle like this:
dot = x1*x2 + y1*y2 # dot product between [x1, y1] and [x2, y2]
det = x1*y2 - y1*x2 # determinant
angle = atan2(det, dot) # atan2(y, x) or atan2(sin, cos)
The orientation of this angle matches that of the coordinate system. In a left-handed coordinate system, i.e. x pointing right and y down as is common for computer graphics, this will mean you get a positive sign for clockwise angles. If the orientation of the coordinate system is mathematical with y up, you get counter-clockwise angles as is the convention in mathematics. Changing the order of the inputs will change the sign, so if you are unhappy with the signs just swap the inputs.
3D case
In 3D, two arbitrarily placed vectors define their own axis of rotation, perpendicular to both. That axis of rotation does not come with a fixed orientation, which means that you cannot uniquely fix the direction of the angle of rotation either. One common convention is to let angles be always positive, and to orient the axis in such a way that it fits a positive angle. In this case, the dot product of the normalized vectors is enough to compute angles.
dot = x1*x2 + y1*y2 + z1*z2 #between [x1, y1, z1] and [x2, y2, z2]
lenSq1 = x1*x1 + y1*y1 + z1*z1
lenSq2 = x2*x2 + y2*y2 + z2*z2
angle = acos(dot/sqrt(lenSq1 * lenSq2))
Edit: Note that some comments and alternate answers advise against the use of acos for numeric reasons, in particular if the angles to be measured are small.
Plane embedded in 3D
One special case is the case where your vectors are not placed arbitrarily, but lie within a plane with a known normal vector n. Then the axis of rotation will be in direction n as well, and the orientation of n will fix an orientation for that axis. In this case, you can adapt the 2D computation above, including n into the determinant to make its size 3×3.
dot = x1*x2 + y1*y2 + z1*z2
det = x1*y2*zn + x2*yn*z1 + xn*y1*z2 - z1*y2*xn - z2*yn*x1 - zn*y1*x2
angle = atan2(det, dot)
One condition for this to work is that the normal vector n has unit length. If not, you'll have to normalize it.
As triple product
This determinant could also be expressed as the triple product, as #Excrubulent pointed out in a suggested edit.
det = n · (v1 × v2)
This might be easier to implement in some APIs, and gives a different perspective on what's going on here: The cross product is proportional to the sine of the angle, and will lie perpendicular to the plane, hence be a multiple of n. The dot product will therefore basically measure the length of that vector, but with the correct sign attached to it.
To compute angle you just need to call atan2(v1.s_cross(v2), v1.dot(v2)) for 2D case.
Where s_cross is scalar analogue of cross production (signed area of parallelogram).
For 2D case that would be wedge production.
For 3D case you need to define clockwise rotation because from one side of plane clockwise is one direction, from other side of plane is another direction =)
Edit: this is counter clockwise angle, clockwise angle is just opposite
This answer is the same as MvG's, but explains it differently (it's the result of my efforts in trying to understand why MvG's solution works). I'm posting it on the off chance that others find it helpful.
The anti-clockwise angle theta from x to y, with respect to the viewpoint of their given normal n (||n|| = 1), is given by
atan2( dot(n, cross(x,y)), dot(x,y) )
(1) = atan2( ||x|| ||y|| sin(theta), ||x|| ||y|| cos(theta) )
(2) = atan2( sin(theta), cos(theta) )
(3) = anti-clockwise angle between x axis and the vector (cos(theta), sin(theta))
(4) = theta
where ||x|| denotes the magnitude of x.
Step (1) follows by noting that
cross(x,y) = ||x|| ||y|| sin(theta) n,
and so
dot(n, cross(x,y))
= dot(n, ||x|| ||y|| sin(theta) n)
= ||x|| ||y|| sin(theta) dot(n, n)
which equals
||x|| ||y|| sin(theta)
if ||n|| = 1.
Step (2) follows from the definition of atan2, noting that atan2(cy, cx) = atan2(y,x), where c is a scalar. Step (3) follows from the definition of atan2. Step (4) follows from the geometric definitions of cos and sin.
Since one of the simplest and most elegant solutions is hidden in one the comments, I think it might be useful to post it as a separate answer.
acos can cause inaccuracies for very small angles, so atan2 is usually preferred. For the 3D case:
dot = x1 * x2 + y1 * y2 + z1 * z2
cross_x = (y1 * z2 – z1 * y2)
cross_y = (z1 * x2 – x1 * z2)
cross_z = (x1 * y2 – y1 * x2)
det = sqrt(cross_x * cross_x + cross_y * cross_y + cross_z * cross_z)
angle = atan2(det, dot)
Scalar (dot) product of two vectors lets you get the cosinus of the angle between them.
To get the 'direction' of the angle, you should also calculate the cross product, it will let you check (via z coordinate) is angle is clockwise or not (i.e. should you extract it from 360 degrees or not).
For a 2D method, you could use the law of
cosines and the "direction" method.
To calculate the angle of segment P3:P1
sweeping clockwise to segment P3:P2.
P1 P2
P3
double d = direction(x3, y3, x2, y2, x1, y1);
// c
int d1d3 = distanceSqEucl(x1, y1, x3, y3);
// b
int d2d3 = distanceSqEucl(x2, y2, x3, y3);
// a
int d1d2 = distanceSqEucl(x1, y1, x2, y2);
//cosine A = (b^2 + c^2 - a^2)/2bc
double cosA = (d1d3 + d2d3 - d1d2)
/ (2 * Math.sqrt(d1d3 * d2d3));
double angleA = Math.acos(cosA);
if (d > 0) {
angleA = 2.*Math.PI - angleA;
}
This has the same number of transcendental
operations as suggestions above and only one
more or so floating point operation.
the methods it uses are:
public int distanceSqEucl(int x1, int y1,
int x2, int y2) {
int diffX = x1 - x2;
int diffY = y1 - y2;
return (diffX * diffX + diffY * diffY);
}
public int direction(int x1, int y1, int x2, int y2,
int x3, int y3) {
int d = ((x2 - x1)*(y3 - y1)) - ((y2 - y1)*(x3 - x1));
return d;
}
If by "direct way" you mean avoiding the if statement, then I don't think there is a really general solution.
However, if your specific problem would allow loosing some precision in angle discretization and you are ok with loosing some time in type conversions, you can map the [-pi,pi) allowed range of phi angle onto the allowed range of some signed integer type. Then you would get the complementarity for free. However, I didn't really use this trick in practice. Most likely, the expense of float-to-integer and integer-to-float conversions would outweigh any benefit of the directness. It's better to set your priorities on writing autovectorizable or parallelizable code when this angle computation is done a lot.
Also, if your problem details are such that there is a definite more likely outcome for the angle direction, then you can use compilers' builtin functions to supply this information to the compiler, so it can optimize the branching more efficiently. E.g., in case of gcc, that's __builtin_expect function. It's somewhat more handy to use when you wrap it into such likely and unlikely macros (like in linux kernel):
#define likely(x) __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)
For 2D case atan2 can easily calculate angle between (1, 0) vector (X-axis) and one of your vectors.
Formula is:
Atan2(y, x)
So you can easily calculate difference of two angles relatively X-axis
angle = -(atan2(y2, x2) - atan2(y1, x1))
Why is it not used as default solution? atan2 is not efficient enough. Solution from the top answer is better. Tests on C# showed that this method has 19.6% less performance (100 000 000 iterations). It's not critical but unpleasant.
So, another info that can be useful:
The smallest angle between outer and inner in degrees:
abs(angle * 180 / PI)
Full angle in degrees:
angle = angle * 180 / PI
angle = angle > 0 ? angle : 360 - angle
or
angle = angle * 180 / PI
if (angle < 0) angle = 360 - angle;
A formula for clockwise angle,2D case, between 2 vectors, xa,ya and xb,yb.
Angle(vec.a-vec,b)=
pi()/2*((1+sign(ya))*
(1-sign(xa^2))-(1+sign(yb))*
(1-sign(xb^2))) +pi()/4*
((2+sign(ya))*sign(xa)-(2+sign(yb))*
sign(xb)) +sign(xa*ya)*
atan((abs(ya)-abs(xa))/(abs(ya)+abs(xa)))-sign(xb*yb)*
atan((abs(yb)-abs(xb))/(abs(yb)+abs(xb)))
just copy & paste this.
angle = (acos((v1.x * v2.x + v1.y * v2.y)/((sqrt(v1.x*v1.x + v1.y*v1.y) * sqrt(v2.x*v2.x + v2.y*v2.y))))/pi*180);
you're welcome ;-)
I want to achieve that two points are rotating around each other. I therefore use a rotation matrix. However I now get the problem that the distance between the points is growing (see atached video 1). The distance however should stay constant over my whole simulation.
Here is my code I use for calculating the speed:
Where p1 and p2 are the two points.
double xPos = p0.x+p1.x;
double yPos = p0.y+p1.y;
//The center between p1 and p2
xPos /=2;
yPos /=2;
//the rotating angle
double omega = 0.1;
//calculate the new positions
double x0new = xPos + (p0.x-xPos)*std::cos(omega) - (p0.y-yPos)*std::sin(omega);
double y0new = yPos + (p0.x-xPos)*std::sin(omega) + (p0.y-yPos)*std::cos(omega);
double x1new = xPos + (p1.x-xPos)*std::cos(omega) - (p1.y-yPos)*std::sin(omega);
double y1new = yPos + (p1.x-xPos)*std::sin(omega) + (p1.y-yPos)*std::cos(omega);
//the speed is exatly the difference as I integrate one timestep
p0.setSpeed(p0.x-x0new, p0.y-y0new);
p1.setSpeed(p1.x-x1new, p1.y-y1new);
I then integrate the speed exactly one timestep. What is wrong in my calculation?
Update
It seems that my integration is wrong. If I set the positions direct it works perfect. However I do not now what is wrong with this integration:
setSpeed(ux,uy){
ux_=ux;
uy_=uy;
}
// integrate one timestep t = 1
move(){
x = x + ux_;
y = y + uy_;
}
Video of my behaviour
There's nothing clearly wrong in this code, but the "speed" integration that isn't shown, suggests that you might be integrating linearly between old and new position, which would make the orbits expand when speed > nominal speed and to contract when speed < nominal_speed.
As I suspected. The integration is actually extrapolation at the line segment between point p0 and p1 which are supposed to be at a fixed distance from origin (a physical simulation would probably make the trajectory elliptical...)
Thus if the extrapolation factor would be 0, the new position would be on the calculated perimeter. If it was < 0 (and > -1), you'd be interpolating inside the expected trajectory.
O This beautiful ascii art is trying to illustrate the integration
/ x is the original position, o is the new one and O is the
/ ___----- "integrated" value and the arc is a perfect circle :)
o-- Only at the calculated position o, there is no expansion.
--/
/ /
/ /
| /
x
At the first glance, the main reason is that you update p0 and p1 coordinates in the each iteration. That would accumulate inaccuracies, which are possibly coming from setSpeed.
Instead, you should use the constant initial coordinates p0 and p1, but increase omega angle.