I'm trying to convert an Eigen 3x3 rotation matrix to a quaternion using this code:
//m_labelMatrix : raw data of vtk4x4 matrix4d.
//m_transformationMatrix : Eigen4x4 matrix4d.
m_transformationMatrix = Eigen::Map<Eigen::Matrix4d>(m_labelMatrix);
m_transformationMatrix.transposeInPlace();
//m_affinedMatrix : affine3d matrix.
m_affinedMatrix = m_transformationMatrix;
auto label_pos = m_affinedMatrix.translation();
auto rotationMatrix = m_affinedMatrix.linear();
auto scaleX = rotationMatrix.col(0).norm();
auto scaleY = rotationMatrix.col(1).norm();
auto scaleZ = rotationMatrix.col(2).norm();
// Make my rotation matrix orthogonal.
rotationMatrix.col(0).normalize();
rotationMatrix.col(1).normalize();
rotationMatrix.col(2) = rotationMatrix.col(0).cross(rotationMatrix.col(1));
rotationMatrix.col(2).normalize();
rotationMatrix.col(0) = rotationMatrix.col(1).cross(rotationMatrix.col(2));
rotationMatrix.col(0).normalize();
Eigen::Quaterniond q(rotationMatrix);
But, when I try to convert back to rotation matrix i get the same matrix with some different values(I think it is an Eigen rounding problem):
rotationMatrix = q.normalized().matrix();
/*3.02303 0.484642 -0.124911
-0.559522 2.94976 -0.217941
0.259569 0.71415 0.984962 */
There is a method for conversion the rotation matrix with noise
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation
"Fitting quaternions"
with Eigen , it will look like
Eigen::Matrix<Scalar, 4, 4> c;
c(0, 0) = xx - yy - zz; c(0, 1) = yx + xy; c(0, 2) = zx + xz; c(0, 3) = yz - zy;
c(1, 0) = yx + xy; c(1, 1) = yy - xx - zz; c(1, 2) = zy + yz; c(1, 3) = zx - xz;
c(2, 0) = zx + xz; c(2, 1) = zy + yz; c(2, 2) = zz - xx - yy; c(2, 3) = xy - yx;
c(3, 0) = yz - zy; c(3, 1) = zx - xz; c(3, 2) = xy - yx; c(3, 3) = xx + yy + zz;
c /= 3.0;
Eigen::SelfAdjointEigenSolver<Matrix44> es(c);
typename Vector4::Index maxIdx;
es.eigenvalues().maxCoeff(&maxIdx);
Vector4 xyzw = es.eigenvectors().col(maxIdx);
Related
I'm a newbie that is driving nuts with a really simple problem :\ How to define a quad that has its center at position x=10 y=11 z=12 with side 5 and it is facing at camera.
My naive implementation:
Eigen::Vector3f quadPosition(10, 11, 12);
Eigen::Vector3f camPos(10, 5, 12);
float sideLength = 5;
Eigen::Affine3f transform = Eigen::Translation3f(position) * Eigen::AngleAxisf(3.141595, (camPos - quadPosition).normalized());
Eigen::Vector3f v0 = transform * Eigen::Vector3f(-sideLength / 2, -sideLength / 2, 0);
Eigen::Vector3f v1 = transform * Eigen::Vector3f(sideLength / 2, -sideLength / 2, 0);
Eigen::Vector3f v2 = transform * Eigen::Vector3f(sideLength / 2, sideLength / 2, 0);
Eigen::Vector3f v3 = transform * Eigen::Vector3f(-sideLength / 2, sideLength / 2, 0);
Eigen::MatrixXd verts;
verts.resize(4, 3);
verts(0, 0) = v0(0);
verts(0, 1) = v0(1);
verts(0, 2) = v0(2);
verts(1, 0) = v1(0);
verts(1, 1) = v1(1);
verts(1, 2) = v1(2);
verts(2, 0) = v2(0);
verts(2, 1) = v2(1);
verts(2, 2) = v2(2);
verts(3, 0) = v3(0);
verts(3, 1) = v3(1);
verts(3, 2) = v3(2);
Eigen::MatrixXi faces;
faces.resize(2, 3);
faces(0, 0) = 0;
faces(0, 1) = 1;
faces(0, 2) = 2;
faces(1, 0) = 0;
faces(1, 1) = 2;
faces(1, 2) = 3;
Any idea? Thank you in advance!
One common approach is to define the quad in terms of two opposed corners like: [0,0,0] and [1,1,1]. Now you know that anything in between 0 and 1 in X, Y and Z axis belong to the box ... as long as there is no rotation. Otherwise, you could also add it.
My function getHeightOfTerrain() is calling a barycentric formula function that is not returning the correct height for the one set test height in : heightMapFromArray[][].
I've tried watching OpenGL JAVA Game tutorials 14,21, 22, by "thin matrix" and I am confused on how to use my array: heightMapforBaryCentric in both of the supplied functions, and how to set the arguments that are passed to the baryCentic() function in some sort of manner so that I can solve the problem.
int creaateTerrain(int height, int width)
{
float holderY[6] = { 0.f ,0.f,0.f,0.f,0.f,0.f };
float scaleit = 1.5f;
float holder[6] = { 0.f,0.f,0.f,0.f,0.f,0.f };
for (int z = 0, z2 =0; z < iterationofHeightMap;z2++)
{
//each loop is two iterations and creates one quad (two triangles)
//however because each iteration is by two (i.e. : x=x+2) om bottom
//the amount of triangles is half the x value
//
//number of vertices : 80 x 80 x 6.
//column
for (int x = 0, x2 = 0; x < iterationofHeightMap;x2++)
{
//relevant - A : first triangle - on left triangle
//[row] [colum[]
holder[0] = heightMapFromArray[z][x];
//holder[0] = (float)imageData[(z / 2 * MAP_Z + (x / 2)) * 3];
//holder[0] = holder[0] / 255;// *scaleit;
vertices.push_back(glm::vec3(x, holder[0], z));
//match height map with online barycentric use
heightMapforBaryCentric[x2][z2] = holder[0];
holder[1] = heightMapFromArray[z+2][x];
//holder[1] = (float)imageData[(((z + 2) / 2 * MAP_Z + ((x) / 2))) * 3];
//holder[1] = holder[1] / 255;// 6 * scaleit;
vertices.push_back(glm::vec3(x, holder[1], z + 2));
//match height map with online barycentric use
heightMapforBaryCentric[x2][z2+1] = holder[1];
holder[2] = heightMapFromArray[z+2][x+2];
//holder[2] = (float)imageData[(((z + 2) / 2 * MAP_Z + ((x + 2) / 2))) * 3];
//holder[2] = holder[2] / 255;// *scaleit;
vertices.push_back(glm::vec3(x + 2, holder[2], z + 2));
////match height map with online barycentric use
heightMapforBaryCentric[x2+1][z2+1] = holder[2];
//relevant - B - second triangle (on right side)
holder[3] = heightMapFromArray[z][x];
//holder[3] = (float)imageData[((z / 2)*MAP_Z + (x / 2)) * 3];
//holder[3] = holder[3] / 255;// 256 * scaleit;
vertices.push_back(glm::vec3(x, holder[3], z));
holder[4] = heightMapFromArray[x+2][z+2];
//holder[4] = (float)imageData[(((z + 2) / 2 * MAP_Z + ((x + 2) / 2))) * 3];
//holder[4] = holder[4] / 255;// *scaleit;
vertices.push_back(glm::vec3(x + 2, holder[4], z + 2));
holder[5] = heightMapFromArray[x+2][z];
//holder[5] = (float)imageData[((z / 2)*MAP_Z + ((x + 2) / 2)) * 3];
//holder[5] = holder[5] / 255;// *scaleit;
vertices.push_back(glm::vec3(x + 2, holder[5], z));
x = x + 2;
}
z = z + 2;
}
return(1);
}
float getHeightOfTerrain(float worldX, float worldZ) {
float terrainX = worldX;
float terrainZ = worldZ;
int gridSquareSize = 2.0f;
gridX = (int)floor(terrainX / gridSquareSize);
gridZ = (int)floor(terrainZ / gridSquareSize);
xCoord = ((float)(fmod(terrainX, gridSquareSize)) / (float)gridSquareSize);
zCoord = ((float)(fmod(terrainZ, gridSquareSize)) / (float)gridSquareSize);
if (xCoord <= (1 - zCoord))
{
answer = baryCentric(
//left triangle
glm::vec3(0.0f, heightMapforBaryCentric[gridX][gridZ], 0.0f),
glm::vec3(0.0f, heightMapforBaryCentric[gridX][gridZ+1], 1.0f),
glm::vec3(1.0f, heightMapforBaryCentric[gridX+1][gridZ+1], 1.0f),
glm::vec2(xCoord, zCoord));
// if (answer != 1)
// {
// fprintf(stderr, "Z:gridx: %d gridz: %d answer: %f\n", gridX, gridZ,answer);
//
// }
}
else
{
//right triangle
answer = baryCentric(glm::vec3(0, heightMapforBaryCentric[gridX][gridZ], 0),
glm::vec3(1,heightMapforBaryCentric[gridX+1][gridZ+1], 1),
glm::vec3(1,heightMapforBaryCentric[gridX+1][gridZ], 0),
glm::vec2(xCoord, zCoord));
}
if (answer == 1)
{
answer = 0;
}
//answer = abs(answer - 1);
return(answer);
}
float baryCentric(glm::vec3 p1, glm::vec3 p2, glm::vec3 p3 , glm::vec2 pos) {
float det = (p2.z - p3.z) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.z - p3.z);
float l1 = ((p2.z - p3.z) * (pos.x - p3.x) + (p3.x - p2.x) * (pos.y - p3.z)) / det;
float l2 = ((p3.z - p1.z) * (pos.x - p3.x) + (p1.x - p3.x) * (pos.y - p3.z)) / det;
float l3 = 1.0f - l1 - l2;
return (l1 * p1.y + l2 * p2.y + l3 * p3.y);
}
My expected results were that the center of the test grid's height to be the set value .5 and gradually less as the heights declined. My results were the heights being all the same, varied, or increasing. Usually these heights were under the value of one.
What I try to achieve is to rotate a line around rectangle center so it always stays in its bounds touching them (or having some padding).
Now I have the following routine for this, as you see I use tan calculations dividing my rectangle into 8 parts (red lines)
It works so far, but for some reason I have inconsistency using other calculation for radius drawing (green line), the lines won't always match as expected and I wonder why.
Basically the same could be achieved using just sin/cos calculations and finding cross points between lines and rect borders, but for some reason I could not get it to work.
std::pair<Point, Point>
MathUtils::calculateRotatingLine(Size size, double degrees)
{
auto width = size.width;
auto height = size.height;
double diagonalAngleTopRight = radiansToDegrees(atan((width / 2) / (height / 2)));
double diagonalAngleBottomRight = 90 + (90 - diagonalAngleTopRight);
double diagonalAngleBottomLeft = 180 + diagonalAngleTopRight;
double diagonalAngleTopLeft = 180 + diagonalAngleBottomRight;
double x, y;
/*
* *8*1*
* 7* *2
* 6* *3
* *5*4*
*/
// 1
if (degrees >= 0 && degrees <= diagonalAngleTopRight) {
x = width / 2 + height / 2 * tan(degreesToRadians(degrees));
y = 0;
}
// 2
else if (degrees > diagonalAngleTopRight && degrees <= 90) {
x = width;
y = width / 2 * tan(degreesToRadians(degrees - diagonalAngleTopRight));
}
// 3
else if (degrees > 90 && degrees <= diagonalAngleBottomRight) {
x = width;
y = height / 2 + width / 2 * tan(degreesToRadians(degrees - 90));
}
// 4
else if (degrees > diagonalAngleBottomRight && degrees <= 180) {
x = width - height / 2 * tan(degreesToRadians(degrees - diagonalAngleBottomRight));
y = height;
}
// 5
else if (degrees > 180 && degrees <= diagonalAngleBottomLeft) {
x = width / 2 - height / 2 * tan(degreesToRadians(degrees - 180));
y = height;
}
// 6
else if (degrees > diagonalAngleBottomLeft && degrees <= 270) {
x = 0;
y = height - width / 2 * tan(degreesToRadians(degrees - diagonalAngleBottomLeft));
}
// 7
else if (degrees > 270 && degrees <= diagonalAngleTopLeft) {
x = 0;
y = height / 2 - width / 2 * tan(degreesToRadians(degrees - 270));
}
// 8
else {
x = height / 2 * tan(degreesToRadians(degrees - diagonalAngleTopLeft));
y = 0;
}
return {Point{width / 2, height / 2}, Point{x, y}};
}
Green line calculation
Point
MathUtils::calculateCirclePoint(double radius, double degrees)
{
return {radius * cos(degreesToRadians(degrees)), radius * sin(degreesToRadians(degrees))};
}
EDIT
Awesome, it works thanks to #MBo
Point
MathUtils::calculateCrossPoint(Size size, double degrees)
{
auto x0 = size.width / 2;
auto y0 = size.height / 2;
auto vx = cos(degreesToRadians(degrees - 90));
auto vy = sin(degreesToRadians(degrees - 90));
//potential border positions
auto ex = vx > 0 ? size.width : 0;
auto ey = vy > 0 ? size.height : 0;
//check for horizontal/vertical directions
if (vx == 0) {
return {x0, ey};
}
if (vy == 0) {
return {ex, y0};
}
// in general case find times of intersections with horizontal and vertical edge line
auto tx = (ex - x0) / vx;
auto ty = (ey - y0) / vy;
// and get intersection for smaller parameter value
if (tx <= ty) {
return {ex, y0 + tx * vy};
}
return {x0 + ty * vx, ey};
}
Pseudocode to find intersection of ray emitted from rectangle center (with angle an in radians) with edges. (Works also for other (x0,y0) positions)
x0 = width / 2;
y0 = height / 2;
vx = cos(an);
vy = sin(an);
//potential border positions
ex = vx > 0? width: 0
ey = vy > 0? height: 0
//check for horizontal/vertical directions
if vx = 0 then
return cx = x0, cy = ey
if vy = 0 then
return cx = ex, cy = y0
//in general case find times of intersections with horizontal and vertical edge line
tx = (ex - x0) / vx
ty = (ey - y0) / vy
//and get intersection for smaller parameter value
if tx <= ty then
return cx = ex, cy = y0 + tx * vy
else
return cx = x0 + ty * vx, cy = ey
I am having a bit of an issue with rotating a shape with given degrees.
void Shape::rotate(double degrees)
{
// rotates the vertices of a shape by a specified angle in degrees
int x, y, xx, yy;
double radians;
x = centroid.getX();
y = centroid.getY();
vertices.push_back(Vertex(x, y));
x = vertices.back().getX() - centroid.getX();
y = vertices.back().getY() - centroid.getY();
radians = (degrees * PI) / 180;
xx = round(x * cos(radians) - y * sin(radians));
yy = round(y * cos(radians) + x * sin(radians));
xx = xx + centroid.getX();
yy = yy + centroid.getY();
vertices.push_back(Vertex(xx, yy));
radians = (degrees * PI) / 180;
xx = round(x * cos(radians) - y * sin(radians));
yy = round(y * cos(radians) + x * sin(radians));
xx = xx + centroid.getX();
yy = yy + centroid.getY();
vertices.push_back(Vertex(xx, yy));
radians = (degrees * PI) / 180;
xx = round(x * cos(radians) - y * sin(radians));
yy = round(y * cos(radians) + x * sin(radians));
xx = xx + centroid.getX();
yy = yy + centroid.getY();
vertices.push_back(Vertex(xx, yy));
}
but the output i get is this:
Messed up rhombus
Any ideas where I'm going wrong?
I'm trying to calculate the points in a cuboid given its centre (which is a Vector3) and the lengths of the sides along the x, y and z axis. I found the following on math.stackexchange.com: https://math.stackexchange.com/questions/107778/simplest-equation-for-drawing-a-cube-based-on-its-center-and-or-other-vertices which says I can use the following formulae:
The constructor for the World class is:
World::World(Vector3 o, float d1, float d2, float d3) : origin(o)
{
// If we consider an edge length to be d, we need to find r such that
// 2r = d in order to calculate the positions of each vertex in the world.
float r1 = d1 / 2,
r2 = d2 / 2,
r3 = d3 / 2;
for (int i = 0; i < 8; i++)
{
/* Sets up the vertices of the cube.
*
* #see http://bit.ly/1cc2RPG
*/
float x = o.getX() + (std::pow(-1, i&1) * r1),
y = o.getY() + (std::pow(-1, i&2) * r2),
z = o.getZ() + (std::pow(-1, i&4) * r3);
points[i] = Vector3(x, y, z);
std::cout << points[i] << "\n";
}
}
And I passing the following parameters to the constructor:
Vector3 o(0, 0, 0);
World w(o, 100.f, 100.f, 100.f);
The coordinates being output for all 8 vertices are:
(50, 50, 50)
(-50, 50, 50)
(50, 50, 50)
(-50, 50, 50)
(50, 50, 50)
(-50, 50, 50)
(50, 50, 50)
(-50, 50, 50)
Which cannot be correct. Any guidance would be very much appreciated!
The problem lies in the bitwise & inside your pow calls:
In the y and z components, they always return 0 and 2 or 4, respectively. -1^2 = -1^4 = 1, which is why the sign of these components is always positive. You could try (i&2)!=0 or (i&2) >> 1 for the y component instead. The same goes for the z component.
Change this:
float x = o.getX() + (std::pow(-1, i&1) * r1),
y = o.getY() + (std::pow(-1, i&2) * r2),
z = o.getZ() + (std::pow(-1, i&4) * r3);
To this:
float x = o.getX() + (std::pow(-1, (i ) & 1) * r1), // pow(-1, 0) == 1, pow(-1, 1) == -1
y = o.getY() + (std::pow(-1, (i >> 1) & 1) * r2), // pow(-1, 0) == 1, pow(-1, 1) == -1
z = o.getZ() + (std::pow(-1, (i >> 2) & 1) * r3); // pow(-1, 0) == 1, pow(-1, 1) == -1
Or even to this:
float x = o.getX() + (std::pow(-1, (i )) * r1), // pow(-1, {0, 2, 4, 6}) == 1, pow(-1, {1, 3, 5, 7}) == -1
y = o.getY() + (std::pow(-1, (i >> 1)) * r2), // pow(-1, {0, 2}) == 1, pow(-1, {1, 3}) == -1
z = o.getZ() + (std::pow(-1, (i >> 2)) * r3); // pow(-1, 0) == 1, pow(-1, 1) == -1
The problem is that as written even though the values you mask out identify weather or not the lengths need to be negated. They are not in the correct place value to get the desired properties from the exponentiation of -1.
Rewriting the code as I have above will solve this issue, however it would be more readable and in general more permanent just to unroll the loop and manually write if each one is an addition or subtraction without using the pow function.