How do I convert quaternion values to specific angles - angle

I'm getting quaternion values, and I want to convert these values to 2 angles.
One of the angles is between the "ground plane" (please correct me if this is an incorrect name for it) and the fictional vector between the origin and the quaternion point ( the complementary angle of gamma ).
The second angle is the rotational angle, this is the rotation that the vector has made relatively to a plane perpendicular to the vector ( R in the picture ).
I looked lot up on the internet, but everything I try has a range of -90° to 90°, while I want it to go from -180° to 180°.
This is the last version of code I ended up with:
float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan(gx / sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan(gy/gz); // about X axis
Serial.print(" yaw ");
Serial.print(yaw * 180/M_PI,0);
Serial.print(" pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print(" sideways ");
// Don't mind this section as I'm applying a mathematical function I created
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);

You should always use atan2(y,x) instead of atan(y/x). It is a common mistake. – Somos
He wrote this on a math form were I asked this too, and that was my stupid mistake -_-
My new version is:
float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan2(gx, sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan2(gy, gz); // about X axis
/*Serial.print(" yaw ");
Serial.print(yaw * 180/M_PI,0);*/
Serial.print(" pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print(" sideways ");
// Please don't pay attention to the extra function I made for the project but it doesn't have to do with the problem
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);

Related

How to get enemies to turn continuously using atan2()

I am trying to build a game where you control a pirate ship and other computer pirate ships try to attack you. I'm currently working on code that gets the enemies to turn to face the player, and I am running into an issue when atan2() returns a value that jumps from 0 to 2PI (I am using the fmod function to change the range from -pi to pi).
The ships will be following and turning with the player until the player crosses over from the 1st quadrant to the 4th quadrant, and then the ship will turn in the exact opposite direction to face the player. Any ideas on how to fix it and make it a continuous smooth turn? Here is my code: rotation is the current angle that the enemy ship is facing
float angleReq = atan2(distToPlayer.y, distToPlayer.x);
angleReq = fmod(angleReq + (2 * M_PI), 2 * M_PI);
if(rotation - angleReq > 0.01) {
rotation -= 0.01;
rotation = fmod(rotation, 2 * M_PI);
} else if (rotation - angleReq < -0.01) {
rotation += 0.01;
rotation = fmod(rotation, 2 * M_PI);
}
I realized how to fix it. As someone stated I was thinking about the problem wrong, I needed to tell the enemy ship to go the "shorter route" in order to get to the required angle based on its current position. Here is the solution code that solved that problem:
float angleReq = atan2(distToPlayer.y, distToPlayer.x);
angleReq = fmod(angleReq + (2 * M_PI), 2 * M_PI);
/* Moves ship counter clockwise from 0 over to 2pi radians */
if ((2 * M_PI - angleReq) + rotation < angleReq - rotation) {
rotation -= 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship clockwise from 2pi over to 0 radians */
else if ((2 * M_PI - rotation) + angleReq < rotation - angleReq) {
rotation += 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship clockwise normally */
else if (angleReq - rotation > 0.01) {
rotation += 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship counterclockwise normally */
else if (angleReq - rotation < -0.01) {
rotation -= 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
}

Quaternion rotation works fine with y/z rotation but gets messed up when I add x rotation

So I've been learning about quaternions recently and decided to make my own implementation. I tried to make it simple but I still can't pinpoint my error. x/y/z axis rotation works fine on it's own and y/z rotation work as well, but the second I add x axis to any of the others I get a strange stretching output. I'll attach the important code for the rotations below:(Be warned I'm quite new to cpp).
Here is how I describe a quaternion (as I understand since they are unit quaternions imaginary numbers aren't required):
struct Quaternion {
float w, x, y, z;
};
The multiplication rules of quaternions:
Quaternion operator* (Quaternion n, Quaternion p) {
Quaternion o;
// implements quaternion multiplication rules:
o.w = n.w * p.w - n.x * p.x - n.y * p.y - n.z * p.z;
o.x = n.w * p.x + n.x * p.w + n.y * p.z - n.z * p.y;
o.y = n.w * p.y - n.x * p.z + n.y * p.w + n.z * p.x;
o.z = n.w * p.z + n.x * p.y - n.y * p.x + n.z * p.w;
return o;
}
Generating the rotation quaternion to multiply the total rotation by:
Quaternion rotate(float w, float x, float y, float z) {
Quaternion n;
n.w = cosf(w/2);
n.x = x * sinf(w/2);
n.y = y * sinf(w/2);
n.z = z * sinf(w/2);
return n;
}
And finally, the matrix calculations which turn the quaternion into an x/y/z position:
inline vector<float> quaternion_matrix(Quaternion total, vector<float> vec) {
float x = vec[0], y = vec[1], z = vec[2];
// implementation of 3x3 quaternion rotation matrix:
vec[0] = (1 - 2 * pow(total.y, 2) - 2 * pow(total.z, 2))*x + (2 * total.x * total.y - 2 * total.w * total.z)*y + (2 * total.x * total.z + 2 * total.w * total.y)*z;
vec[1] = (2 * total.x * total.y + 2 * total.w * total.z)*x + (1 - 2 * pow(total.x, 2) - 2 * pow(total.z, 2))*y + (2 * total.y * total.z + 2 * total.w * total.x)*z;
vec[2] = (2 * total.x * total.z - 2 * total.w * total.y)*x + (2 * total.y * total.z - 2 * total.w * total.x)*y + (1 - 2 * pow(total.x, 2) - 2 * pow(total.y, 2))*z;
return vec;
}
That's pretty much it (I also have a normalize function to deal with floating point errors), I initialize all objects quaternion to: w = 1, x = 0, y = 0, z = 0. I rotate a quaternion using an expression like this:
obj.rotation = rotate(angle, x-axis, y-axis, z-axis) * obj.rotation
where obj.rotation is the objects total quaternion rotation value.
I appreciate any help I can get on this issue, if anyone knows what's wrong or has also experienced this issue before. Thanks
EDIT: multiplying total by these quaternions output the expected rotation:
rotate(angle,1,0,0)
rotate(angle,0,1,0)
rotate(angle,0,0,1)
rotate(angle,0,1,1)
However, any rotations such as these make the model stretch oddly:
rotate(angle,1,1,0)
rotate(angle,1,0,1)
EDIT2: here is the normalize function I use to normalize the quaternions:
Quaternion normalize(Quaternion n, double tolerance) {
// adds all squares of quaternion values, if normalized, total will be 1:
double total = pow(n.w, 2) + pow(n.x, 2) + pow(n.y, 2) + pow(n.z, 2);
if (total > 1 + tolerance || total < 1 - tolerance) {
// normalizes value of quaternion if it exceeds a certain tolerance value:
n.w /= (float) sqrt(total);
n.x /= (float) sqrt(total);
n.y /= (float) sqrt(total);
n.z /= (float) sqrt(total);
}
return n;
}
To implement two rotations in sequence you need the quaternion product of the two elementary rotations. Each elementary rotation is specified by an axis and an angle. But in your code you did not make sure you have a unit vector (direction vector) for the axis.
Do the following modification
Quaternion rotate(float w, float x, float y, float z) {
Quaternion n;
float f = 1/sqrtf(x*x+y*y+z*z)
n.w = cosf(w/2);
n.x = f * x * sinf(w/2);
n.y = f * y * sinf(w/2);
n.z = f * z * sinf(w/2);
return n;
}
and then use it as follows
Quaternion n = rotate(angle1,1,0,0) * rotate(angle2,0,1,0)
for the combined rotation of angle1 about the x-axis, and angle2 about the y-axis.
As pointed out in comments, you are not initializing your quaternions correctly.
The following quaternions are not rotations:
rotate(angle,0,1,1)
rotate(angle,1,1,0)
rotate(angle,1,0,1)
The reason is the axis is not normalized e.g., the vector (0,1,1) is not normalized. Also make sure your angles are in radians.

Animating along an elliptical path in OpenGL

I am trying to make a red circle follow the path of a semi-circle using the DDA algorithm in OpenGL. I almost have it, though the circle is slightly offset on its X-axis, which increases as the angle of the semi-circle increases.
Any assistance would be greatly appreciated! Here's my code:
scrPt movecircle (scrPt p1, scrPt p2)
{
scrPt circlePos;
float angle, x = p1.x, y = p1.y, vectorX, vectorY;
// Get tahe x distance between the two points
int dx = p2.x - p1.x, steps;
// Get the y distance between the two points
int dy = p2.y - p1.y;
// Get the length between the points
float length = sqrt(dx*dx + dy*dy);
if (fabs (dx) > fabs (dy))
steps = fabs (dx);
else
steps = fabs (dy);
// calculate the direction
float xIncrement = float (dx) / float (steps);
float yIncrement = float (dy) / float (steps);
if (nextPos == 0)
{
for(int i = 0; i < steps; i++)
{
glClear(GL_COLOR_BUFFER_BIT);
angle = PI * i / steps;
vectorX = x + (length / 2) * cos(angle + theta);
vectorY = y + dy / 2 + (length / 2) * sin(angle + theta);
circlePos.x = round(vectorX - length / 2);
circlePos.y = round(vectorY);
drawCircle (circlePos.x, circlePos.y);
drawArch();
glFlush();
usleep(3000);
}
}
else
{
for (int i = 0; i < steps; i++)
{
glClear(GL_COLOR_BUFFER_BIT);
drawCircle (round(x),round(y));
glFlush();
usleep(3000);
x += xIncrement;
y += yIncrement;
}
}
return circlePos;
}
There were a couple of errors in the for-loop that were causing the issue. I needed to change
this:
vectorX = x + (length / 2) * cos(angle + theta);
to this:
vectorX = x + (dx / 2) + (length / 2) * cos(angle + theta);
and this:
circlePos.x = round(vectorX - (length / 2));
to this:
circlePos.x = round(vectorX);

C++ Angles between a vector and a point

I got 2 points own=(x, y, z) and en=(x, y, z) which represents my own position in the world and some other player position. the other player also got pitch (from 90 degrees to -90) and yaw (0 to 360). I want to calculate the angles between the other player look and my own position.
In 2D, alpha is what I'm trying to calculate:
int main()
{
float own_x = 1, own_y = 1, own_z = 1;
float en_x = 10, en_y = 1, en_z = 10;
float pi = 3.14159265;
float pitch = 0.f * (pi / 180), yaw = 45.f * (pi / 180);
float x = sin(yaw) * cos(pitch);
float y = sin(pitch);
float z = cos(pitch) * cos(yaw);
float vec_length = sqrt(pow(en_x - own_x, 2) + pow(en_y - own_y, 2) + pow(en_y - own_y, 2));
x /= vec_length;
y /= vec_length;
z /= vec_length;
float cos_t = ((en_x - own_x)*x + (en_y - own_y)*y + (en_z - own_z)*z) / sqrt(pow(en_x - own_x, 2) + pow(en_y - own_y, 2) + pow(en_y - own_y, 2));
float arc = acos(cos_t) * (180 / pi);
return 0;
}
you divide twice with the length of en-own: You should remove
vec_length, and xyz /= vec_length.
your division at cos_t is buggy, you use _y twice in the
expression instead of _y and _z
Note: instead of pow(x, 2), use x*x, it is faster usually (compilers may not optimize pow(x, 2) to x*x).

C++ rotation of point in float coordinates accuracy

i study OpenGL ES 2.0. But i think it's more C++ question rather then OpenGL. I'am stuck with rotation question. It is known, that rotation transformation can be applied using the following equations:
p'x = cos(theta) * (px-ox) - sin(theta) * (py-oy) + ox
p'y = sin(theta) * (px-ox) + cos(theta) * (py-oy) + oy
But it seems that when i perform this rotation operation several times the accuracy problem is occured. I guess, that the core of this problem is in uncertain results of cos function and floating point limitations. As a result i see that my rotating object is getting smaller and smaller and smaller. So:
1.) How do you think, does this issue really connected with floating point accuracy problem?
2.) If so, how can i handle this.
Suppose that float _points[] is array containing coordinates x1,y1,x2,y2...xn,yn. Then i recompute my coordinates after rotation in the following way:
/* For x */
float angle = .... ;
pair<float, float> orig_coordinates(0, 0);
for (; coors_ctr < _n_points * 2; coors_ctr += 2)
_points[coors_ctr] = cos(angle) * (_points[coors_ctr] - _orig_coordinates.first) -
sin(angle) * (_points[coors_ctr + 1] - _orig_coordinates.second) +
_orig_coordinates.first;
/* For y */
coors_ctr = 1;
for (; coors_ctr < _n_points * 2; coors_ctr += 2)
_points[coors_ctr] = sin(angle) * (_points[coors_ctr - 1] - _orig_coordinates.first) +
cos(angle) * (_points[coors_ctr] - _orig_coordinates.second) + _orig_coordinates.second;
I think the problem is that you're writing the rotated result back to the input array.
p'x = cos(theta) * (px-ox) - sin(theta) * (py-oy) + ox
p'y = sin(theta) * (p'x-ox) + cos(theta) * (py-oy) + oy
Try doing the rotation out of place, or use temporary variables and do one point (x,y) at a time.