I have quaternion values q available.
These are correct, because they align the object correctly in the ROS RVIZ tool.
I now want to get a heading value from these values.
My idea was to calculate the Yaw value first and then do a Radian to Degree conversion to get the heading.
However, this works only conditionally, as soon as the object moves in the direction of NE everything is correct and the correct heading value is output. The same applies for the direction SW. However, when the object moves in the NW direction, I get a heading value of 135 degrees, exactly the opposite direction. Exactly the same applies to the direction SE.
My conversions look as follows:
double heading = atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI;
if(heading < 0){
heading +=360;
}
I got the formula from:
https://math.stackexchange.com/questions/3496329/map-quaternion-to-heading-pitch-roll-using-different-axis
My Question is:
What I'm doing wrong.. Why are NW and SE swapped?
Really don't know ROS RVIZ.
But for Bullet3d I use:
siny = +2.0 * (q.w * q.z + q.y * q.x);
cosy = +1.0 - 2.0 * (q.x * q.x + q.z * q.z);
heading = atan2(siny, cosy); // in radians
Since this is inside ROS it would typically be recommended to use the tf library that already exists which includes a getRPY() function for quaternions. Take the following example:
tf2::Quaternion tf_quat;
tf2::fromMsg(quat_msg, tf_quat); //Assume quat_msg is a quaternion ros msg
tf2::Matrix3x3 m(tf_quat);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
My Problem was, that the Yaw value was reflected and rotated by 90 Degrees.
My RVIZ tool does not use N E S W System, so in this case it was displayed correct.
The solution was:
double heading = (atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI) - 90;
if(heading < 0){
heading +=360;
}
heading = -heading + 360
Thanks, your solutions are also correct, if the value is not rotated.
Related
I am trying to make a Wolfenstein3D-like game, using pico-8 (that's a 2d engine with many limitations) and the world just bends very weirdly.
Gif of running around
My code: (Warning LUA! Confusing language. starts counting at 1...! )
function ray_cast()
points = {}
for i=1,64 do
points[i] = -1
end
for o = -31,32 do
local angle = player.lvec - o/256
for i=0,96 do
local x,y
x = i * cos(angle)
y = i * sin(angle)
if mget((x+player.x)/8,(y+player.y)/8) == 1 then
local tx = i-1 * cos(angle)
local ty = i-1 * sin(angle)
local dis = sqrt((tx^2+ty^2))
points[o+32] = dis*cos(angle-player.lvec)
break
end
end
end
end
I asked for help in the PICO-8 discord and someone said they could help me, but after a lot of messaging, it still did not result in the solution I wanted.
Edit: New GIF
Distortion of things
I don't know the language of Lua, however, I do understand the basics of ray-casting. From what I see, the issue could be a too large FOV size or the angles not being fixed (subtracting 360 degrees/2PI radians when above 360 degrees/2PI radians, or adding 360 degrees/2PI radians when below 0). If you don't "fix" the angles, you may get incorrect ray hit positions or even in some cases, crash the GUI. If you have a large FOV, the world will appear to be warped.
FOV OF 64 DEGREES:
FOV OF 128 DEGREES:
I want to get the azimuth from the back of the phone (-Z axis) for an augmented reality app. My application only runs in Landscape Right. Testing this on iPhone 5S.
Currently, I'm using the following approach:
CoreLocation heading base on back camera (Augmented reality)
I have 2 problems with this approach:
If I'm pointing the back of the device towards north such that I'm currently at 0 degrees, then rotate it clockwise (yaw) a full 360 degrees, I'm now at -20 degrees. Counterclockwise rotations add 20 degrees. This pattern repeats itself such that rotating 720 degrees from 0 now yields -40 degrees and so on. Also, even if I don't necessarily do these clear rotations, but instead move the phone chaotically (spinning, shaking, etc), but end up in the same spot where I was initially, I can't even predict what value it will show.
The other problem is what I think is called gyro drift. If I don't move the device at all, I can clearly see how the value slowly changes over time, by let's say 0.1 degrees every few seconds, sometimes in one direction, sometimes the other, until a certain point where it decides to stop.
The problem is, I don't have the mathematical background to know how to account for these changes. It's especially problematic that I can't seem to compute the rotation matrix from yaw/pitch/roll from deviceMotion.attitude. I tried:
float w = motion.attitude.yaw;
float v = motion.attitude.pitch;
float u = motion.attitude.roll;
r.m11 = cos(v) * cos(w);
r.m12 = sin(u) * sin(v) * cos(w) - cos(u) * sin(w);
r.m13 = sin(u) * sin(w) + cos(u) * sin(v) * cos(w);
r.m21 = cos(v) * sin(w);
r.m22 = cos(u) * cos(w) + sin(u) * sin(v) * sin(w);
r.m23 = cos(u) * sin(v) * sin(w) - sin(u) * cos(w);
r.m31 = -sin(v);
r.m32 = sin(u) * cos(v);
r.m33 = cos(u) * cos(v);
I've tried every Tait–Bryan combination (u-v-w, u-w-v, v-u-w, v-w-u, w-v-u, w-u-v), some of them came close, but still not close enough.
From my observations, it seems like the magneticHeading from CLLocationManager is much more accurate than computed heading from CMMotionManager, but again, even if I got the correct angle, I don't know where should I start to get the equivalent angle in a different coordinate system reference frame. Any help would be greatly appreciated.
i'm trying to run Madgwick's sensor fusion algorithm on iOS. Since the code is open source i already included it in my project and call the methods with the provided sensor values.
But it seems, that the algorithm expects the sensor measurements in a different coordinate system. The Apple CoreMotion Sensor System is given on the right side, Madgewick's on the left. Here is the picture of the different coordinate systems. Both systems follow the right hand rule.
For me it seems like there is a 90 degree rotation around the z axis. But this didn't work.
I also tried to flip x and y (and invert z) axis as suggested by other stackoverflow posts for WP but this didn't work also. So do you have a hint?
Would be perfect if Madgwick's alogithm output could be in the same system as the CoreMotion output (CMAttitudeReferenceFrameXMagneticNorthZVertical).
Furthermore I'm looking for a good working value for betaDef on the iPhone. betaDef is kind of the proportional gain and is currently set to 0.1f.
Any help on how to achieve the goal would be appreciated.
I'm not sure how to write this in objective c, but here's how I accomplished the coordinate transformations in vanilla c. I also wanted to rotate the orientation so that +y is north. This translation is also reflected in the below method.
This method expects a 4 element quaternion in the form of wxyz, and returns a translated quaternion in the same format:
void madgeq_to_openglq(float *fMadgQ, float *fRetQ) {
float fTmpQ[4];
// Rotate around Z-axis, 90 degres:
float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) };
// Inverse the rotation vectors to accomodate handedness-issues:
fTmpQ[0] = fMadgQ[0];
fTmpQ[1] = fMadgQ[1] * -1.0f;
fTmpQ[2] = fMadgQ[2];
fTmpQ[3] = fMadgQ[3] * -1.0f;
// And then store the translated Rotation into ret:
quatMult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ);
}
// Quaternion Multiplication operator. Expects its 4-element arrays in wxyz order
void quatMult(float *a, float *b, float *ret) {
ret[0] = (b[0] * a[0]) - (b[1] * a[1]) - (b[2] * a[2]) - (b[3] * a[3]);
ret[1] = (b[0] * a[1]) + (b[1] * a[0]) + (b[2] * a[3]) - (b[3] * a[2]);
ret[2] = (b[0] * a[2]) + (b[2] * a[0]) + (b[3] * a[1]) - (b[1] * a[3]);
ret[3] = (b[0] * a[3]) + (b[3] * a[0]) + (b[1] * a[2]) - (b[2] * a[1]);
return;
}
Hope that helps!
I am attempting to simply make objects orbit around a center point, e.g.
The green and blue objects represent objects which should keep their distance to the center point, while rotating, based on an angle which I pass into method.
I have attempted to create a function, in objective-c, but it doesn't work right without a static number. e.g. (It rotates around the center, but not from the true starting point or distance from the object.)
-(void) rotateGear: (UIImageView*) view heading:(int)heading
{
// int distanceX = 160 - view.frame.origin.x;
// int distanceY = 240 - view.frame.origin.y;
float x = 160 - view.image.size.width / 2 + (50 * cos(heading * (M_PI / 180)));
float y = 240 - view.image.size.height / 2 + (50 * sin(heading * (M_PI / 180)));
view.frame = CGRectMake(x, y, view.image.size.width, view.image.size.height);
}
My magic numbers 160, and 240 are the center of the canvas in which I'm drawing the images onto. 50 is a static number (and the problem), which allows the function to work partially correctly -- without maintaining the starting poisition of the object or correct distance. I don't know what to put here unfortunately.
heading is a parameter that passes in a degree, from 0 to 359. It is calculated by a timer and increments outside of this class.
Essentially what I would like to be able to drop any image onto my canvas, and based on the starting point of the image, it would rotate around the center of my circle. This means, if I were to drop an image at Point (10,10), the distance to the center of the circle would persist, using (10,10) as a starting point. The object would rotate 360 degrees around the center, and reach it's original starting point.
The expected result would be to pass for instance (10,10) into the method, based off of zero degrees, and get back out, (15,25) (not real) at 5 degrees.
I know this is very simple (and this problem description is entirely overkill), but I'm going cross eyed trying to figure out where I'm hosing things up. I don't care about what language examples you use, if any. I'll be able to decipher your meanings.
Failure Update
I've gotten farther, but I still cannot get the right calculation. My new code looks like the following:
heading is set to 1 degree.
-(void) rotateGear: (UIImageView*) view heading:(int)heading
{
float y1 = view.frame.origin.y + (view.frame.size.height/2); // 152
float x1 = view.frame.origin.x + (view.frame.size.width/2); // 140.5
float radius = sqrtf(powf(160 - x1 ,2.0f) + powf(240 - y1, 2.0f)); // 90.13
// I know that I need to calculate 90.13 pixels from my center, at 1 degree.
float x = 160 + radius * (cos(heading * (M_PI / 180.0f))); // 250.12
float y = 240 + radius * (sin(heading * (M_PI / 180.0f))); // 241.57
// The numbers are very skewed.
view.frame = CGRectMake(x, y, view.image.size.width, view.image.size.height);
}
I'm getting results that are no where close to where the point should be. The problem is with the assignment of x and y. Where am I going wrong?
You can find the distance of the point from the centre pretty easily:
radius = sqrt((160 - x)^2 + (240 - y)^2)
where (x, y) is the initial position of the centre of your object. Then just replace 50 by the radius.
http://en.wikipedia.org/wiki/Pythagorean_theorem
You can then figure out the initial angle using trigonometry (tan = opposite / adjacent, so draw a right-angled triangle using the centre mass and the centre of your orbiting object to visualize this):
angle = arctan((y - 240) / (x - 160))
if x > 160, or:
angle = arctan((y - 240) / (x - 160)) + 180
if x < 160
http://en.wikipedia.org/wiki/Inverse_trigonometric_functions
Edit: bear in mind I don't actually know any Objective-C but this is basically what I think you should do (you should be able to translate this to correct Obj-C pretty easily, this is just for demonstration):
// Your object gets created here somewhere
float x1 = view.frame.origin.x + (view.frame.size.width/2); // 140.5
float y1 = view.frame.origin.y + (view.frame.size.height/2); // 152
float radius = sqrtf(powf(160 - x1 ,2.0f) + powf(240 - y1, 2.0f)); // 90.13
// Calculate the initial angle here, as per the first part of my answer
float initialAngle = atan((y1 - 240) / (x1 - 160)) * 180.0f / M_PI;
if(x1 < 160)
initialAngle += 180;
// Calculate the adjustment we need to add to heading
int adjustment = (int)(initialAngle - heading);
So we only execute the code above once (when the object gets created). We need to remember radius and adjustment for later. Then we alter rotateGear to take an angle and a radius as inputs instead of heading (this is much more flexible anyway):
-(void) rotateGear: (UIImageView*) view radius:(float)radius angle:(int)angle
{
float x = 160 + radius * (cos(angle * (M_PI / 180.0f)));
float y = 240 + radius * (sin(angle * (M_PI / 180.0f)));
// The numbers are very skewed.
view.frame = CGRectMake(x, y, view.image.size.width, view.image.size.height);
}
And each time we want to update the position we make a call like this:
[objectName rotateGear radius:radius angle:(adjustment + heading)];
Btw, once you manage to get this working, I'd strongly recommend converting all your angles so you're using radians all the way through, it makes it much neater/easier to follow!
The formula for x and y coordinates of a point on a circle, based on radians, radius, and center point:
x = cos(angle) * radius + center_x
y = sin(angle) * radius + center_y
You can find the radius with HappyPixel's formula.
Once you figure out the radius and the center point, you can simply vary the angle to get all the points on the circle that you'd want.
If I understand correctly, you want to do InitObject(x,y). followed by UpdateObject(angle) where angle sweeps from 0 to 360. (But use radians instead of degrees for the math)
So you need to track the angle and radius for each object.:
InitObject(x,y)
relative_x = x-center.x
relative_y = y-center.y
object.radius = sqrt((relative_x)^2, (relative_y)^2)
object.initial_angle = atan(relative_y,relative_x);
And
UpdateObject(angle)
newangle = (object.initial_angle + angle) % (2*PI )
object.x = cos(newangle) * object.radius + center.x
object.y = sin(newangle) * object.radius + center.y
dx=dropx-centerx; //target-source
dy=-(dropy-centery); //minus = invert screen coords to cartesian coords
radius=sqrt(dy*dy+dx*dx); //faster if your compiler optimizer is bad
if dx=0 then dx=0.000001; //hackpatchfudgenudge*
angle=atan(dy/dx); //set this as start angle for the angle-incrementer
Then go with the code you have and you'll be fine. You seem to be calculating radius from current position each time though? This, like the angle, should only be done once, when the object is dropped, or else the radius might not be constant.
*instead of handling 3 special cases for dx=0, if you need < 1/100 degree precision for the start angle go with those instead, google Polar Arctan.
I want to calculate the angle between two lines formed by three points(one of the points is the point of intersection of the two lines) using inverse cosine function as follows:
CGFloat a = initialPosition.x - origin.x;
CGFloat b = initialPosition.y - origin.y;
CGFloat c = currentPosition.x - origin.x;
CGFloat d = currentPosition.y - origin.y;
CGFloat angle = (180/M_PI) * acosf(((a*c) + (b*d)) / ((sqrt(a*a + b*b)) * (sqrt(c*c + d*d))));
Unfortunately, acosf returns a value between 0 and pi only. How do I find a value between 0 and 2*pi (going, say, in the anti-clockwise manner)?
i don't know what language you are using, but typically there is an atan2 function that gives you a value from the full 360 degrees. in this case you need to use it twice and then add a little additional logic.
some pseudocode will help clear things up:
initialAngle = atan2(initialPosition.y - origin.y, initialPosition.x - origin.x)
currentAngle = atan2(currentPosition.y - origin.y, currentPosition.x - origin.x)
# angle is measured from x axis anti-clock, so lets find the value starting from
# initial and rotating anti-clock to current, as a positive number
# so we want current to be larger than initial
if (currentAngle < initialAngle) {currentAngle += 2 pi}
# and then we can subtract
return currentAngle - initialAngle
i know this isn't using acos, but that is multi-valued so to do so ends up using lots of logic about signs of differences that is bug-prone. atan2 is what you want.
found a simple solution...This comes from high school maths! First make an equation of a line made from the origin and the initialPosition of the form y = mx+c. A point lying on either side of this line will satisfy y < mx+c or y > mx+c, depending on where it is. If finding angles in the clockwise or anti-clockwise sense, make the following check:
currentPosition.y < (currentPosition.x *(initialPosition.y - origin.y) + (initialPosition .x * origin.y - initialPosition.y * origin.x)) / (initialPosition.x - origin.x)
If the above condition is true, then the line formed from origin and currentPosition makes an angle less than 180 deg (in the clockwise sense) with the line formed from origin and initialPosition. Otherwise it makes an angle more than 180 deg in the clockwise sense and less than 180 deg in the anti-clockwise sense...and so on. Depending on the requirement, the final angle is either the (angle returned by acos) or (360 - (angle returned by acos)).