Incorrect lookat matrix - c++

I've started working on shadow maps for directional lights and for that I need a lookAt matrix, but when I tried constructing from example of an online tutorial it looked something like this:
It currently looks like this:
https://media.giphy.com/media/QrMnqBBJZuATu/giphy.gif
I've tried multiple ways of constructing it but with no success, I checked if the normalization, cross and translation functions weren't correct but that wasn't the case. I've also tried changing from column-major matrices to row-major matrices but with no luck. Would someone be able to point out what I did wrong?
Lookat matrix constructing:
Center vector = (0, 0, 0),
Up vector = (0, 1, 0)
Matrix4f Matrix4f::lookAt(const Vector3f& position, const Vector3f& center, const Vector3f& up) {
Matrix4f out(1.0f);
Vector3f z = position.substract(center).normalize();
Vector3f y = up;
Vector3f x = y.cross(z).normalize();
y = z.cross(x);
out.mElements[0 * 4 + 0] = x.x;
out.mElements[0 * 4 + 1] = x.y;
out.mElements[0 * 4 + 2] = x.z;
out.mElements[1 * 4 + 0] = y.x;
out.mElements[1 * 4 + 1] = y.y;
out.mElements[1 * 4 + 2] = y.z;
out.mElements[2 * 4 + 0] = z.x;
out.mElements[2 * 4 + 1] = z.y;
out.mElements[2 * 4 + 2] = z.z;
return (out * Matrix4f::translation(Vector3f(-position.x, -position.y, -position.z)));
}
}
Credit for code: https://stackoverflow.com/users/5577765/rabbid76
This is how I pass a matrix to the shader:
void Shader::setMat4(const char* name, const math::Matrix4f& matrix){
glUniformMatrix4fv(getUniformLocation(name), 1, GL_TRUE, matrix.mElements);
}
After I've calculated the lookAt matrix I directly pass it to the vertex shader to the uniform: view and calculate a point like this:
gl_Position = projection * view * model * vec4(vertexPosition, 1.0);
And this is how my matrix multiplication works:
Matrix4f Matrix4f::multiply(const Matrix4f& other) const {
Matrix4f out;
for (int y = 0; y < 4; y++) {
for (int x = 0; x < 4; x++) {
fl32 o = 0;
for (int c = 0; c < 4; c++) {
o += this->mElements[c + y * 4] * other.mElements[x + c * 4]; }
out.mElements[x + y * 4] = o;
}
}
return out;
}
Edit: updated picture
Edit: Added a more detailed description

You need to normalize s before u is calculated. I'm not sure if it is the only problem

If your positions (position, center) and the up vector are in viewport space, then the Z-Axis of the view matrix is the inverse line of sight and the Y-Axis is the up vector. See the following code:
Matrix4f Matrix4f::lookAt(const Vector3f& position, const Vector3f& center, const Vector3f& up)
{
Matrix4f out(1.0f); // I suppose this initilizes a 4*4 identity matrix
// Z-Axis is the line of sight
Vector3f z = position.substract(center).normalize(); // inverse line of sight
// Y-Axis is the up vector
Vector3f y = up;
// X-Axis is the cross product of Y-Axis and Z-Axis
Vector3f x = y.cross(z).normalize();
// orthonormalize the Y-Axis
y = z.cross( x );
out.mElements[0*4 + 0] = x.x;
out.mElements[0*4 + 1] = x.y;
out.mElements[0*4 + 2] = x.z;
out.mElements[1*4 + 0] = y.x;
out.mElements[1*4 + 1] = y.y;
out.mElements[1*4 + 2] = y.z;
out.mElements[2*4 + 0] = z.x;
out.mElements[2*4 + 1] = z.y;
out.mElements[2*4 + 2] = z.z;
return (out * Matrix4f::translation(Vector3f(-position.x, -position.y, -position.z)));
}

After hours of trying to make the lookat matrix working I had given up on way of constructing the lookat matrix, instead I constructed a lookat matrix based on the position of the camera and position the camera should look at, using trigonometry functions I was able to create the result which I was seeking.
My current way of constructing a lookat matrix:
Matrix4f Matrix4f::lookAt(const Vector3f& position, const Vector3f& center) {
Vector3f deltaVector = (position - center).normalize();
fl32 yaw = (fl32)radToDeg(atan(deltaVector.x / deltaVector.z));
fl32 pitch = (fl32)radToDeg(acos(Vector2f(deltaVector.x, deltaVector.z).magnitude()));
if (deltaVector.z > 0)
yaw = yaw - 180.0f;
Matrix4f yRotation = Matrix4f::rotation(Vector3f(0.0f, 1.0f, 0.0f), -yaw);
Matrix4f xRotation = Matrix4f::rotation(Vector3f(1.0f, 0.0f, 0.0f), pitch);
Matrix4f translation = Matrix4f::translation(position);
return (translation * (yRotation * xRotation));
}

Related

How to do frustum culling in OpenGL with the view and projection matrix?

I'm trying to implement frustum culling to my voxel engine, basically I'm rendering chunks and I want to cull every chunk that is outside of the frustum of the camera. I tried a lot of different approaches and code that I found on the web, but yet I can't get it to work. The algorithm is in two parts:
• First I'm extracting the frustum planes from the projectionview matrix.
• Then I'm checking for each chunk if it is inside or colliding with the frustum.
The behavior is generally the same: when looking from the origin to positive direction it seems to work, but when looking to the negative direction it doesn't, and when I'm going away from the origin it starts breaking and doing non-sense. Also when looking up and down the culling is weird.
Here is my frustum plane extraction:
public static Plane[] frustumPlanes(Matrix4f mat, boolean normalize)
{
Plane[] p = new Plane[6];
p[0] = normalizePlane(mat.m30 + mat.m00, mat.m31 + mat.m01, mat.m32 + mat.m02, mat.m33 + mat.m03); // left
p[1] = normalizePlane(mat.m30 - mat.m00, mat.m31 - mat.m01, mat.m32 - mat.m02, mat.m33 - mat.m03); // right
p[2] = normalizePlane(mat.m30 - mat.m10, mat.m31 - mat.m11, mat.m32 - mat.m12, mat.m33 - mat.m13); // top
p[3] = normalizePlane(mat.m30 + mat.m10, mat.m31 + mat.m11, mat.m32 + mat.m12, mat.m33 + mat.m13); // bottom
p[4] = normalizePlane(mat.m30 + mat.m20, mat.m31 + mat.m21, mat.m32 + mat.m22, mat.m33 + mat.m23); // near
p[5] = normalizePlane(mat.m30 - mat.m20, mat.m31 - mat.m21, mat.m32 - mat.m22, mat.m33 - mat.m23); // far
return p;
}
public static Plane normalizePlane(float A, float B, float C, float D) {
float nf = 1.0f / (float)Math.sqrt(A * A + B * B + C * C);
return new Plane(new Vector3f(nf * A, nf * B, nf * C), nf * D);
}
mat is the projectionview matrix, here is the projection matrix:
private void createProjectionMatrix() {
float aspectRatio = (float) DisplayManager.WIDTH / (float) DisplayManager.HEIGHT;
float y_scale = (float) ((1f / Math.tan(Math.toRadians(FOV / 2f))));
float x_scale = y_scale / aspectRatio;
float frustum_length = FAR_PLANE - NEAR_PLANE;
projectionMatrix = new Matrix4f();
projectionMatrix.m00 = x_scale;
projectionMatrix.m11 = y_scale;
projectionMatrix.m22 = -((FAR_PLANE + NEAR_PLANE) / frustum_length);
projectionMatrix.m23 = -1;
projectionMatrix.m32 = -((2 * NEAR_PLANE * FAR_PLANE) / frustum_length);
projectionMatrix.m33 = 0;
}
Here is the view matrix:
public static Matrix4f createViewMatrix(Camera camera) {
Matrix4f viewMatrix = new Matrix4f();
viewMatrix.setIdentity();
Matrix4f.rotate((float) Math.toRadians(camera.getRotation().x), new Vector3f(1, 0, 0), viewMatrix, viewMatrix);
Matrix4f.rotate((float) Math.toRadians(camera.getRotation().y), new Vector3f(0, 1, 0), viewMatrix, viewMatrix);
Matrix4f.rotate((float) Math.toRadians(camera.getRotation().z), new Vector3f(0, 0, 1), viewMatrix, viewMatrix);
Vector3f cameraPos = camera.getPosition();
Vector3f negativeCameraPos = new Vector3f(-cameraPos.x,-cameraPos.y,-cameraPos.z);
Matrix4f.translate(negativeCameraPos, viewMatrix, viewMatrix);
return viewMatrix;
}
Here is the collision detection code aabb vs plane:
public static int boxToPlaneCollision(Plane plane, Vector3f[] minMax)
{
int result = 2; //Inside
// planes have unit-length normal, offset = -dot(normal, point on plane)
int nx = plane.normal.x > 0?1:0;
int ny = plane.normal.y > 0?1:0;
int nz = plane.normal.z > 0?1:0;
// getMinMax(): 0 = return min coordinate. 1 = return max.
float dot = (plane.normal.x*minMax[nx].x) + (plane.normal.y*minMax[nx].y) + (plane.normal.z*minMax[nx].z);
if ( dot < -plane.offset )
return 0; //Outside
float dot2 = (plane.normal.x*minMax[1-nx].x) + (plane.normal.y*minMax[1-nx].y) + (plane.normal.z*minMax[1-nx].z);
if ( dot2 <= -plane.offset )
result = 1; //Intersect
return result;
}
And finally here is where everything is called:
public boolean chunkInsideFrustum(Vector3f chunkPos) {
Vector3f chunkPosMax = new Vector3f(chunkPos.x + Terrain.CHUNK_SIZE, Terrain.CHUNK_HEIGHT, chunkPos.z + Terrain.CHUNK_SIZE);
for (int i = 0; i < 6; i++) {
if(Collider.boxToPlaneCollision(frustumPlanes[i], new Vector3f[] {chunkPos,chunkPosMax}) == 0)
return false;
}
return true;
}
I'm using openGL with LWJGL 2 (Java).
My questions are:
Where is the problem? In the frustum plane extraction code? In the collision detection?
and
I saw people calculating the frustum with projection and modelview matrix, what about this technique? is it better?
Thank you very much for your help!
EDIT:
for the second question, I saw here Extracting View Frustum Planes (Gribb & Hartmann method) someone posted that:
The missing part:
comboMatrix = projection_matrix * Matrix4_Transpose(modelview_matrix)
And then he did the exact same algorithm that I did to extract the planes, but what is modelview_matrix? What model should I use?

Problems rotating opengl camera

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

OpenGL Object rotation using your own Matrix class in C++

I am playing around with OpenGL and one thing I decided to do is create my own Matrix class, instead of using glm's matrices.
The Matrix class has methods for translating, rotating and scaling the object, which are written below:
Matrix4 Matrix4::translate(Matrix4& matrix, Vector3& translation)
{
Vector4 result(translation, 1.0f);
result.multiply(matrix);
matrix.mElements[3 * 4 + 0] = result.x;
matrix.mElements[3 * 4 + 1] = result.y;
matrix.mElements[3 * 4 + 2] = result.z;
return matrix;
}
Matrix4 Matrix4::rotate(Matrix4& matrix, float angle, Vector3& axis)
{
if (axis.x == 0 && axis.y == 0 && axis.z == 0)
return matrix;
float r = angle;
float s = sin(r);
float c = cos(r);
float omc = 1.0f - cos(r);
float x = axis.x;
float y = axis.y;
float z = axis.z;
matrix.mElements[0 + 0 * 4] = c + x * x * omc;
matrix.mElements[1 + 0 * 4] = x * y * omc - z * s;
matrix.mElements[2 + 0 * 4] = z * x * omc + y * s;
matrix.mElements[0 + 1 * 4] = x * y * omc + z * s;
matrix.mElements[1 + 1 * 4] = c + y * y * omc;
matrix.mElements[2 + 1 * 4] = z * y * omc - x * s;
matrix.mElements[0 + 2 * 4] = x * z * omc - y * s;
matrix.mElements[1 + 2 * 4] = y * z * omc + x * s;
matrix.mElements[2 + 2 * 4] = c + z * z * omc;
return matrix;
}
Matrix4 Matrix4::scale(Matrix4& matrix, Vector3& scaler)
{
matrix.mElements[0 + 0 * 4] *= scaler.x;
matrix.mElements[1 + 0 * 4] *= scaler.x;
matrix.mElements[2 + 0 * 4] *= scaler.x;
matrix.mElements[0 + 1 * 4] *= scaler.y;
matrix.mElements[1 + 1 * 4] *= scaler.y;
matrix.mElements[2 + 1 * 4] *= scaler.y;
matrix.mElements[0 + 2 * 4] *= scaler.z;
matrix.mElements[1 + 2 * 4] *= scaler.z;
matrix.mElements[2 + 2 * 4] *= scaler.z;
matrix.mElements[3 + 3 * 4] = 1;
return matrix;
}
When I call the translate, rotate and scale methods in while loop (in this particular order), it does what I want, which is translate the object, then rotate it around its local origin and scale it. However, when I want to switch order so I call rotation first and then translation, I want it to do this:
But my code dosen't do that. Instead, its doing this:
What can I do so that my object only rotates around the center of the screen and not around it's local origin aswell?
My only guess is that I am doing something wrong with adding the rotation calculation on transformed matrix, but I still can't tell what it is.
EDIT: One thing i need to point out is if i left out the rotation method and i only tackle with translation and scaling, they do what i expect them to do in translation first, rotation second and in rotation first, translation second order.
EDIT 2: Here is how i call these functions in while loop.
Matrix4 trans = Matrix4(1.0f);
trans = Matrix4::rotate(trans, (float)glfwGetTime(), Vector3(0.0f, 0.0f, 1.0f));
trans = Matrix4::translate(trans, Vector3(0.5f, -0.5f, 0.0f));
trans = Matrix4::scale(trans, Vector3(0.5f, 0.5f, 1.0f));
shader.setUniformMatrix4f("uTransform", trans);
You have to concatenate the matrices by a matrix multiplication.
A matrix multiplication C = A * B works like this:
Matrix4x4 A, B, C;
// C = A * B
for ( int k = 0; k < 4; ++ k )
for ( int j = 0; j < 4; ++ j )
C[k][j] = A[0][j] * B[k][0] + A[1][j] * B[k][1] + A[2][j] * B[k][2] + A[3][j] * B[k][3];
I recommend to create specify the matrix class somehow like this:
#include <array>
class Matrix4
{
public:
std::array<float, 16> mElements{
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1 };
const float * dataPtr( void ) const { return mElements.data(); }
Matrix4 & multiply( const Matrix4 &mat );
Matrix4 & translate( const Vector3 &translation );
Matrix4 & scale( const Vector3 &scaler );
Matrix4 & rotate( float angle, const Vector3 &axis );
};
Implement the matrix multiplication. Note, you have to store the result in a buffer.
If you would write the result back to the matrix member directly, then you would change elements, which will read again later in the nested loop and the result wouldn't be correct:
Matrix4& Matrix4::multiply( const Matrix4 &mat )
{
// multiply the existing matrix by the new and store the result in a buffer
const float *A = dataPtr();
const float *B = mat.dataPtr();
std::array<float, 16> C;
for ( int k = 0; k < 4; ++ k ) {
for ( int j = 0; j < 4; ++ j ) {
C[k*4+j] =
A[0*4+j] * B[k*4+0] +
A[1*4+j] * B[k*4+1] +
A[2*4+j] * B[k*4+2] +
A[3*4+j] * B[k*4+3];
}
}
// copy the buffer to the attribute
mElements = C;
return *this;
}
Adapt the methods for translation, rotation and scaling like this:
Matrix4 & Matrix4::translate( const Vector3 &translation )
{
float x = translation.x;
float y = translation.y;
float z = translation.z;
Matrix4 transMat;
transMat.mElements = {
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
x, y, z, 1.0f };
return multiply(transMat);
}
Matrix4 & Matrix4::rotate( float angle, const Vector3 &axis )
{
float x = axis.x;
float y = axis.y;
float z = axis.z;
float c = cos(angle);
float s = sin(angle);
Matrix4 rotationMat;
rotationMat.mElements = {
x*x*(1.0f-c)+c, x*y*(1.0f-c)-z*s, x*z*(1.0f-c)+y*s, 0.0f,
y*x*(1.0f-c)+z*s, y*y*(1.0f-c)+c, y*z*(1.0f-c)-x*s, 0.0f,
z*x*(1.0f-c)-y*s, z*y*(1.0f-c)+x*s, z*z*(1.0f-c)+c, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return multiply(rotationMat);
}
Matrix4 & Matrix4::scale( const Vector3 &scaler )
{
float x = scaler.x;
float y = scaler.y;
float z = scaler.z;
Matrix4 scaleMat;
scaleMat.mElements = {
x, 0.0f, 0.0f, 0.0f,
0.0f, y, 0.0f, 0.0f,
0.0f, 0.0f, z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return multiply(scaleMat);
}
If you use the matrix class like this,
float angle_radians = ....;
Vector3 scaleVec{ 0.2f, 0.2f, 0.2f };
Vector3 transVec{ 0.3f, 0.3f, 0.0f };
Vector3 rotateVec{ 0.0f, 0.0f, 1.0f };
Matrix4 model;
model.rotate( angle_rad, rotateVec );
model.translate( transVec );
model.scale( scaleVec );
then the result would look like this:
The function rotate() isn't performing an actual rotation. Only generating a partial rotation matrix, and overwriting it over the original matrix.
You need to construct a complete one and multiply it to the original matrix.
Matrix4 Matrix4::rotate(const Matrix4& matrix, float angle, const Vector3& axis)
{
if (axis.x == 0 && axis.y == 0 && axis.z == 0)
return matrix;
float r = angle;
float s = sin(r);
float c = cos(r);
float omc = 1.0f - cos(r);
float x = axis.x;
float y = axis.y;
float z = axis.z;
Matrix4 r;
r.mElements[0 + 0 * 4] = c + x * x * omc;
r.mElements[1 + 0 * 4] = x * y * omc - z * s;
r.mElements[2 + 0 * 4] = z * x * omc + y * s;
r.mElements[3 + 0 * 4] = 0;
r.mElements[0 + 1 * 4] = x * y * omc + z * s;
r.mElements[1 + 1 * 4] = c + y * y * omc;
r.mElements[2 + 1 * 4] = z * y * omc - x * s;
r.mElements[3 + 1 * 4] = 0;
r.mElements[0 + 2 * 4] = x * z * omc - y * s;
r.mElements[1 + 2 * 4] = y * z * omc + x * s;
r.mElements[2 + 2 * 4] = c + z * z * omc;
r.mElements[3 + 2 * 4] = 0;
r.mElements[0 + 3 * 4] = 0;
r.mElements[1 + 3 * 4] = 0;
r.mElements[2 + 3 * 4] = 0;
r.mElements[3 + 3 * 4] = 1;
return r * matrix;
}

Euler to Quaternion / Quaternion to Euler using Eigen

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.

Projecting a 3D point to 2D screen coordinate OpenTK

Using Monotouch and OpenTK I am trying to get the screen coordinate of one 3D point. I have my world view projection matrix set up, and OpenGL makes sense of it and projects my 3D model perfectly, but how to use the same matrix to project just one point from 2D to 3D?
I thought I could simply use:
Vector3.Transform(ref input3Dpos, ref matWorldViewProjection, out projected2Dpos);
Then have the projected screen coordinate in projected2DPos. But the resulting Vector4 does not seem to represent the proper projected screen coordinate. And I do not know how to calculate it from there on.
I found I need to divide by Vector4.w, however I am still getting the wrong values. Using this method now:
private static bool GluProject(OpenTK.Vector3 objPos, OpenTK.Matrix4 matWorldViewProjection, int[] viewport, out OpenTK.Vector3 screenPos)
{
OpenTK.Vector4 _in;
_in.X = objPos.X;
_in.Y = objPos.Y;
_in.Z = objPos.Z;
_in.W = 1f;
Vector4 _out = OpenTK.Vector4.Transform(_in, matWorldViewProjection);
if (_out.W <= 0.0)
{
screenPos = OpenTK.Vector3.Zero;
return false;
}
_out.X /= _out.W;
_out.Y /= _out.W;
_out.Z /= _out.W;
/* Map x, y and z to range 0-1 */
_out.X = _out.X * 0.5f + 0.5f;
_out.Y = -_out.Y * 0.5f + 0.5f;
_out.Z = _out.Z * 0.5f + 0.5f;
/* Map x,y to viewport */
_out.X = _out.X * viewport[2] + viewport[0];
_out.Y = _out.Y * viewport[3] + viewport[1];
screenPos.X = _out.X;
screenPos.Y = _out.Y;
screenPos.Z = _out.Z;
return true;
}
I cannot see any errors though... :S
In the first question you're missing the last step: Mapping from NDC (Normalized Device Coordinates) to viewport coordinates. That's what the lines
/* Map x,y to viewport */
_out.X = _out.X * viewport[2] + viewport[0];
_out.Y = _out.Y * viewport[3] + viewport[1];
in your GluProject do,
You have two options. You can calculate it yourself, or use the glProject function. I prefer the first.
Number 1:
private Vector2 Convert(
Vector3 pos,
Matrix4 viewMatrix,
Matrix4 projectionMatrix,
int screenWidth,
int screenHeight)
{
pos = Vector3.Transform(pos, viewMatrix);
pos = Vector3.Transform(pos, projectionMatrix);
pos.X /= pos.Z;
pos.Y /= pos.Z;
pos.X = (pos.X + 1) * screenWidth / 2;
pos.Y = (pos.Y + 1) * screenHeight / 2;
return new Vector2(pos.X, pos.Y);
}
Number 2:
public Vector2 form3Dto2D(Vector3 our3DPoint)
{
Vector3 our2DPoint;
float[] modelviewMatrix = new float[16];
float[] projectionMatrix = new float[16];
int[] viewport = new int[4];
GL.GetFloat(GetPName.ModelviewMatrix, modelviewMatrix);
GL.GetFloat(GetPName.ProjectionMatrix, projectionMatrix);
GL.GetInteger(GetPName.Viewport, viewport);
OpenTK.Graphics.Glu.Project(our3DPoint, convertFloatsToDoubles(modelviewMatrix),
convertFloatsToDoubles(projectionMatrix), viewport, out our2DPoint);
return new Vector2(our2DPoint.X, our2DPoint.Y)
}
public static double[] convertFloatsToDoubles(float[] input)
{
if (input == null)
{
return null; // Or throw an exception - your choice
}
double[] output = new double[input.Length];
for (int i = 0; i < input.Length; i++)
{
output[i] = input[i];
}
return output;
}