I am trying to display a 360 panorama using an IMU for head tracking.
Yaw works correctly but the roll and pitch are reverse. I also notice that the pitch contains some roll (and maybe vice-versa).
I am receiving (W, X, Y, Z) coordinate from the IMU that I am storing in an array as X, Y, Z, W.
The next step is converting the quaternion to a rotation matrix. I have looked at many examples, and can't seem to find anything wrong with the following code:
static GLfloat rotation[16];
// Quaternion (x, y, z, w)
static void quaternionToRotation(float* quaternion)
{
// Normalize quaternion
float magnitude = sqrt(quaternion[0] * quaternion[0] +
quaternion[1] * quaternion[1] +
quaternion[2] * quaternion[2] +
quaternion[3] * quaternion[3]);
for (int i = 0; i < 4; ++i)
{
quaternion[i] /= magnitude;
}
double xx = quaternion[0] * quaternion[0], xy = quaternion[0] * quaternion[1],
xz = quaternion[0] * quaternion[2], xw = quaternion[0] * quaternion[3];
double yy = quaternion[1] * quaternion[1], yz = quaternion[1] * quaternion[2],
yw = quaternion[1] * quaternion[3];
double zz = quaternion[2] * quaternion[2], zw = quaternion[2] * quaternion[3];
// Column major order
rotation[0] = 1.0f - 2.0f * (yy + zz);
rotation[1] = 2.0f * (xy - zw);
rotation[2] = 2.0f * (xz + yw);
rotation[3] = 0;
rotation[4] = 2.0f * (xy + zw);
rotation[5] = 1.0f - 2.0f * (xx + zz);
rotation[6] = 2.0f * (yz - xw);
rotation[7] = 0;
rotation[8] = 2.0f * (xz - yw);
rotation[9] = 2.0f * (yz + xw);
rotation[10] = 1.0f - 2.0f * (xx + yy);
rotation[11] = 0;
rotation[12] = 0;
rotation[13] = 0;
rotation[14] = 0;
rotation[15] = 1;
}
The rotation matrix is then used in the draw call as such:
static void draw()
{
// Get IMU quaternion
float* quaternion = tracker.getTrackingData();
if (quaternion != NULL)
{
quaternionToRotation(quaternion);
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
glPushMatrix();
// TODO: Multiply initialRotation quaternion with IMU quaternion
glMultMatrixf(initialRotation); // Initial rotation to point forward
glMultMatrixf(rotation); // Rotation based on IMU
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, texture);
gluSphere(quad, 0.1, 50, 50);
glBindTexture(GL_TEXTURE_2D, 0);
glPopMatrix();
glFlush();
glutSwapBuffers();
}
I tried to set all but one fields in the quaternion to 0, and I notice that they all work individually, except roll and pitch is swapped around. I tried swapping X and Y but this does not seem to help.
Any help would be really appreciated. Please let me know as well if you have any steps that can let me debug my issue. Thanks!
Related
I want to rotate a point in OpenGL around an arbitrary axis. I want to utilize that to rotate a sphere.
This is what I got so far:
float degreeBetweenTwoVec(glm::vec3 &a, glm::vec3 b)
{
float prod = b.x * a.x + b.y * a.y + b.z * a.z;
float mag_axis = sqrt((b.x * b.x) + (b.y * b.y) + (b.z * b.z));
float mag_vec = sqrt((a.x * a.x) + (a.y * a.y) + (a.z * a.z));
float degree = prod / (mag_axis * mag_vec);
return acos(degree) * 180.0 / PI;;
}
void rotAroundZ(glm::vec3 &point, float degree)
{
glm::vec3 n_point;
n_point.x = (point.x * cos(degree * PI / 180.0)) - (point.y * sin(degree * PI / 180.0));
n_point.y = (point.x * sin(degree * PI / 180.0)) + (point.y * cos(degree * PI / 180.0));
n_point.z = point.z;
point.x = n_point.x;
point.y = n_point.y;
point.z = n_point.z;
}
void rotAroundY(glm::vec3& point, float degree)
{
glm::vec3 n_point;
n_point.x = (point.x * cos(degree * PI / 180.0)) + (point.z * sin(degree * PI / 180.0));
n_point.y = point.y;
n_point.z = ((point.x * -1.0f) * sin(degree * PI / 180.0)) + (point.z * cos(degree * PI / 180.0));;
point.x = n_point.x;
point.y = n_point.y;
point.z = n_point.z;
}
void rotAroundA(glm::vec3& point, glm::vec3 &axis, float zdegree)
{
float xdegree = degreeBetweenTwoVec(axis, glm::vec3{ 1.0f, 0.0f, 0.0f });
float ydegree = degreeBetweenTwoVec(axis, glm::vec3{ 0.0f, 1.0f, 0.0f });
rotAroundZ(point, xdegree);
rotAroundY(point, ydegree);
rotAroundZ(point, zdegree);
rotAroundY(point, -ydegree);
rotAroundZ(point, -xdegree);
}
void rotAObject(Object& obj, glm::vec3 &axis, float degree)
{
axis = glm::normalize(axis);
translate(axis, glm::vec3{ axis.x, axis.y, axis.z });
for (int i = 0; i < obj.vertices.size(); i++)
{
rotAroundA(obj.vertices[i], axis, degree);
}
rotAroundA(obj.mp, axis, degree);
translate(axis, glm::vec3{ axis.x * -1.0f, axis.y * -1.0f, axis.z * -1.0f });
}
This works just fine if the given axis happens to be on one of the global axis. However, if it isn't and the given axis is basiclly rotating around something else. There is some kind of axis it is rotating around but as soon as change the given axis, for example rotating it around the z axis it rotates around a completlly different axis than before. It looks like for every position the given axis can take there is some other axis the object is actually rotating around.
Any help is appreciated!
I recommend to use a rotation matrix. Use glm::rotate(), to set a rotation matrix by axis and angle.
Convert the point to glm::vec4 and transform it by the rotation matrix:
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
glm::mat4 rot_mat = glm::rotate(glm::mat4(1.0f), glm::radians(degree), axis);
glm::vec3 n_point = glm::vec3(glm::vec4(point, 1.0f) * rot_mat);
I cannot understand the math behind this problem, I am trying to create an FPS camera where I can look freely with my mouse input.
I am trying to rotate and position my lookat point with 180 degrees of freedom. I understand the easier solution is to glRotate the world to fit my perspective, but I do not want this approach. I am fairly unfamiliar with the trigonometry involved here and cannot figure out how to solve this problem the way I want to...
here is my attempt to do this so far...
code to get mouse coordinates relative to the center of the window, then process it in my camera object
#define DEG2RAD(a) (a * (M_PI / 180.0f))//convert to radians
static void glutPassiveMotionHandler(int x, int y) {
glf centerX = WinWidth / 2; glf centerY = WinHeight / 2;//get windows origin point
f speed = 0.2f;
f oldX = mouseX; f oldY = mouseY;
mouseX = DEG2RAD(-((x - centerX)));//get distance from 0 and convert to radians
mouseY = DEG2RAD(-((y - centerY)));//get distance from 0 and convert to radians
f diffX = mouseX - oldX; f diffY = mouseY - oldY;//get difference from last frame to this frame
if (mouseX != 0 || mouseY != 0) {
mainCamera->Rotate(diffX, diffY);
}
Code to rotate the camera
void Camera::Rotate(f angleX, f angleY) {
Camera::refrence = Vector3D::NormalizeVector(Camera::refrence * cos(angleX)) + (Camera::upVector * sin(angleY));//rot up
Camera::refrence = Vector3D::NormalizeVector((Camera::refrence * cos(angleY)) - (Camera::rightVector * sin(angleX)));//rot side to side
};
Camera::refrence is our lookat point, processing the lookat point is handled as follows
void Camera::LookAt(void) {
gluLookAt(
Camera::position.x, Camera::position.y, Camera::position.z,
Camera::refrence.x, Camera::refrence.y, Camera::refrence.z,
Camera::upVector.x, Camera::upVector.y, Camera::upVector.z
);
};
The camera is defined by a position point (position) a target point (refrence) and a up-vector upVector. If you want to change the orientation of the camera, then you've to rotate the direction vector from the position (position) to the target (refrence) rather then the target point by a Rotation matrix.
Note, since the 2 angles are angles which should change an already rotated view, you've to use a rotation matrix, to rotate the vectors which point in an arbitrary direction.
Write a function which set 3x3 rotation matrix around an arbitrary axis:
void RotateMat(float m[], float angle_radians, float x, float y, float z)
{
float c = cos(angle_radians);
float s = sin(angle_radians);
m[0] = x*x*(1.0f-c)+c; m[1] = x*y*(1.0f-c)-z*s; m[2] = x*z*(1.0f-c)+y*s;
m[3] = y*x*(1.0f-c)+z*s; m[4] = y*y*(1.0f-c)+c; m[5] = y*z*(1.0f-c)-x*s;
m[6] = z*x*(1.0f-c)-y*s; m[7] = z*y*(1.0f-c)+x*s; m[8] = z*z*(1.0f-c)+c };
}
Write a function which rotates a 3 dimensional vector by the matrix:
Vector3D Rotate(float m[], const Vector3D &v)
{
Vector3D rv;
rv.x = m[0] * v.x + m[3] * v.y + m[6] * v.z;
rv.y = m[1] * v.x + m[4] * v.y + m[7] * v.z;
rv.z = m[2] * v.x + m[5] * v.y + m[8] * v.z;
return rv;
}
Calculate the vector form the position to the target:
Vector3D los = Vector3D(refrence.x - position.x, refrence.y - position.y, refrence.z - position.z);
Rotate all the vectors around the z axis of the world by angleX:
float rotX[9];
RotateMat(rotX, angleX, Vector3D(0, 0, 1));
los = Rotate(rotX, los);
upVector = Rotate(rotX, upVector);
Rotate all the vectors around the current y axis of the view by angleY:
float rotY[9];
RotateMat(rotY, angleY, Vector3D(los.x, los.y, 0.0));
los = Rotate(rotY, los);
upVector = Rotate(rotY, upVector);
Calculate the new target point:
refrence = Vector3D(position.x + los.x, position.y + los.y, position.z + los.z);
U_Cam_X_angle is left right rotation.. U_Cam_Y_angle is up down rotation.
view_radius is the view distance (zoom) to U_look_point_x, U_look_point_y and U_look_point_z.
This is ALWAYS a negative number! This is because you are always looking in positive direction. Deeper in the screen is more positive.
This is all in radians.
The last three.. eyeX, eyeY and eyeZ is where the camera is in 3D space.
This code is in VB.net. Find a converter online for VB to C++ or do it manually.
Public Sub set_eyes()
Dim sin_x, sin_y, cos_x, cos_y As Single
sin_x = Sin(U_Cam_X_angle + angle_offset)
cos_x = Cos(U_Cam_X_angle + angle_offset)
cos_y = Cos(U_Cam_Y_angle)
sin_y = Sin(U_Cam_Y_angle)
cam_y = Sin(U_Cam_Y_angle) * view_radius
cam_x = (sin_x - (1 - cos_y) * sin_x) * view_radius
cam_z = (cos_x - (1 - cos_y) * cos_x) * view_radius
Glu.gluLookAt(cam_x + U_look_point_x, cam_y + U_look_point_y, cam_z + U_look_point_z, _
U_look_point_x, U_look_point_y, U_look_point_z, 0.0F, 1.0F, 0.0F)
eyeX = cam_x + U_look_point_x
eyeY = cam_y + U_look_point_y
eyeZ = cam_z + U_look_point_z
End Sub
I'm trying to implement a functionality that can convert an Euler angle into an Quaternion and back "YXZ"-convention using Eigen. Later this should be used to let the user give you Euler angles and rotate around as Quaternion and convert Back for the user. In fact i am realy bad at math but tried my best. I have no Idea if this matrices are correct or anything. The code Works, but my results are way to off, i suppose. Any idea where i take the wrong turn? This is what my Quat.cpp looks like:
#include "Quat.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
using namespace Eigen;
Vector3f Quat::MyRotation(const Vector3f YPR)
{
Matrix3f matYaw(3, 3), matRoll(3, 3), matPitch(3, 3), matRotation(3, 3);
const auto yaw = YPR[2]*M_PI / 180;
const auto pitch = YPR[0]*M_PI / 180;
const auto roll = YPR[1]*M_PI / 180;
matYaw << cos(yaw), sin(yaw), 0.0f,
-sin(yaw), cos(yaw), 0.0f, //z
0.0f, 0.0f, 1.0f;
matPitch << cos(pitch), 0.0f, -sin(pitch),
0.0f, 1.0f, 0.0f, // X
sin(pitch), 0.0f, cos(pitch);
matRoll << 1.0f, 0.0f, 0.0f,
0.0f, cos(roll), sin(roll), // Y
0.0f, -sin(roll), cos(roll);
matRotation = matYaw*matPitch*matRoll;
Quaternionf quatFromRot(matRotation);
quatFromRot.normalize(); //Do i need to do this?
return Quat::toYawPitchRoll(quatFromRot);
}
Vector3f Quat::toYawPitchRoll(const Eigen::Quaternionf& q)
{
Vector3f retVector;
const auto x = q.y();
const auto y = q.z();
const auto z = q.x();
const auto w = q.w();
retVector[2] = atan2(2.0 * (y * z + w * x), w * w - x * x - y * y + z * z);
retVector[1] = asin(-2.0 * (x * z - w * y));
retVector[0] = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
#if 1
retVector[0] = (retVector[0] * (180 / M_PI));
retVector[1] = (retVector[1] * (180 / M_PI))*-1;
retVector[2] = retVector[2] * (180 / M_PI);
#endif
return retVector;
}
Input: x = 55.0, y = 80.0, z = 12.0
Quaternion: w:0.872274, x: -0.140211, y:0.447012, z:-0.140211
Return Value: x:-55.5925, y: -6.84901, z:-21.8771
The X-Value seems about right disregarding the prefix, but Y and z are off.
From Euler to Quaternion:
using namespace Eigen;
//Roll pitch and yaw in Radians
float roll = 1.5707, pitch = 0, yaw = 0.707;
Quaternionf q;
q = AngleAxisf(roll, Vector3f::UnitX())
* AngleAxisf(pitch, Vector3f::UnitY())
* AngleAxisf(yaw, Vector3f::UnitZ());
std::cout << "Quaternion" << std::endl << q.coeffs() << std::endl;
From Quaternion to Euler:
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler << std::endl;
Taken from https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
Here's one approach (not tested):
Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler[0]; pitch = euler[1]; roll = euler[2];
The Quaternation to Euler solution didnt work for me, so i researched and modified the code, now it works for my purpose:
Vector3f ToEulerAngles(const Eigen::Quaternionf& q) {
Vector3f angles; //yaw pitch roll
const auto x = q.x();
const auto y = q.y();
const auto z = q.z();
const auto w = q.w();
// roll (x-axis rotation)
double sinr_cosp = 2 * (w * x + y * z);
double cosr_cosp = 1 - 2 * (x * x + y * y);
angles[2] = std::atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (w * y - z * x);
if (std::abs(sinp) >= 1)
angles[1] = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angles[1] = std::asin(sinp);
// yaw (z-axis rotation)
double siny_cosp = 2 * (w * z + x * y);
double cosy_cosp = 1 - 2 * (y * y + z * z);
angles[0] = std::atan2(siny_cosp, cosy_cosp);
return angles;
}
I was inspired by this wiki entry and did some bench marking with the presented solution here.
Checkout the wiki:
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
When I use
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2)
It can not work perfectly all the time, the euler angle always has a regular beat (the actual value and the calculated value have a deviation of ±π).
For example, read and show yaw angle by rqt
picture.
I have no idea about this, but I find ros tf::getYaw() also can achieve "Quaternion to Euler" (because I just need yaw angle).
Without Eigen (just in case), I did:
tf2::Matrix3x3 ( quat ) . getEulerYPR( &roll, &pitch, &yaw );
// and
tf2::Matrix3x3 ( quat ) . getRPY( &roll, &pitch, &yaw );
Though, these can give only two of the 24 configurations possible.
I have the following code to draw an arbitrary arc:
void CenteredArc::drawPolygonArc(float radius, float thickness, float startAngle, float arcAngle) {
float num_segments = 360.0;
float radiusOuter = radius + thickness / 2;
float radiusInner = radius - thickness / 2;
float theta = arcAngle / num_segments;
float tangetial_factor = tanf(theta);//calculate the tangential factor
float radial_factor = cosf(theta);//calculate the radial factor
float xxOuter = radiusOuter * cosf(startAngle);
float yyOuter = radiusOuter * sinf(startAngle);
float xxInner = radiusInner * cosf(startAngle);
float yyInner = radiusInner * sinf(startAngle);
float prevXXOuter = -1;
float prevYYOuter = -1;
float prevXXInner = -1;
float prevYYInner = -1;
glPolygonMode(GL_FRONT, GL_FILL);
for(int ii = 0; ii < num_segments; ii++)
{
if (prevXXOuter != -1) {
glBegin(GL_POLYGON);
glVertex2f(prevXXOuter, prevYYOuter);
glVertex2f(xxOuter, yyOuter);
glVertex2f(xxInner, yyInner);
glVertex2f(prevXXInner, prevYYInner);
glEnd();
}
//calculate the tangential vector
//remember, the radial vector is (x, y)
//to get the tangential vector we flip those coordinates and negate one of them
float txOuter = -yyOuter;
float tyOuter = xxOuter;
float txInner = -yyInner;
float tyInner = xxInner;
//add the tangential vector
prevXXOuter = xxOuter;
prevYYOuter = yyOuter;
prevXXInner = xxInner;
prevYYInner = yyInner;
xxOuter += txOuter * tangetial_factor;
yyOuter += tyOuter * tangetial_factor;
xxInner += txInner * tangetial_factor;
yyInner += tyInner * tangetial_factor;
//correct using the radial factor
xxOuter *= radial_factor;
yyOuter *= radial_factor;
xxInner *= radial_factor;
yyInner *= radial_factor;
}
}
However, I would like for the arc to start off with the specified thickness on one end and gradually decrease to a thickness of zero on the other end. Any suggestions?
Edit: I am not using GL_LINE_STRIP because I am trying to avoid having overlapping lines and gaps like so:
I would use a line strip with decreasing glLineWidth.
This is my implementation, it doesn't gradially reduce the lineWidth but it could be modified to do so. Sorry for the extra stuff, it's from my game engine.
for(int i=0;i<arcs().size();i++)
{
Entities::Arc temp = arcs().at(i);
glLineWidth(temp.LW.value); // change LWidth
glColor3f( temp.CL.R, temp.CL.G, temp.CL.B );
// theta is now calculated from the arc angle instead, the
// - 1 part comes from the fact that the arc is open
float theta = temp.A.value*DEG2RAD / float(WW_SPLINE_ACCURACY - 1);
float tan = tanf(theta);
float cos = cosf(theta);
// we are now at the starting angle
double x = temp.R.value * cosf(temp.A.value*DEG2RAD);
double y = temp.R.value * sinf(temp.A.value*DEG2RAD);
// since the arc is not a closed curve, this is a strip now
glBegin(GL_LINE_STRIP);
for(int ii = 0; ii < WW_SPLINE_ACCURACY; ii++)
{
glVertex2d(x + temp.C.X, y + temp.C.Y);
double tx = -y;
double ty = x;
x += tx * tan;
y += ty * tan;
x *= cos;
y *= cos; //y = ( y + (ty*tan) )*cos;
}
glEnd();
glLineWidth(WW_DEFAULT_LWIDTH); // reset LWidth
}
I also used these values
#define WW_SPLINE_ACCURACY 72 // 72 for extra smooth arcs/circles, 32 minimum
#define WW_BEZIER_ACCURACY 20
/* Math stuff */
#define DEG2RAD 3.14159/180
#define PI 3.1415926535897932384626433832795;
...
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
glDisable(GL_COLOR_MATERIAL);
glEnable (GL_LINE_SMOOTH);
glEnable (GL_BLEND);
//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glHint (GL_LINE_SMOOTH_HINT, GL_DONT_CARE);
glEnable(GL_POLYGON_SMOOTH);
glClearColor(0.188f, 0.169f, 0.329f, 1.0f); //#302b54
I'm not allowed to release the full source since I wrote it for a company but sharing a part or two wont hurt anybody :D
I am trying to map a texture to a circle using GL_POLYGON using this code:
void drawCircleOutline(Circle c, int textureindex)
{
float angle, radian, x, y; // values needed by drawCircleOutline
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, textureLib[textureindex]);
glBegin(GL_POLYGON);
for (angle=0.0; angle<360.0; angle+=2.0)
{
radian = angle * (pi/180.0f);
x = (float)cos(radian) * c.r + c.pos.x;
y = (float)sin(radian) * c.r + c.pos.y;
glTexCoord2f(x, y);
glVertex2f(x, y);
}
glEnd();
glDisable(GL_TEXTURE_2D);
}
it looks like this when running.
And should look like this:
Try:
radian = angle * (pi/180.0f);
xcos = (float)cos(radian);
ysin = (float)sin(radian);
x = xcos * c.r + c.pos.x;
y = ysin * c.r + c.pos.y;
tx = xcos * 0.5 + 0.5;
ty = ysin * 0.5 + 0.5;
glTexCoord2f(tx, ty);
glVertex2f(x, y);