How to calculate the angles XYZ from a Matrix4x4 - c#

I am trying to ascertain the X,Y,Z angles held within a Matrix by decomposing the matrix. I am using .net 4.5 c#.
I created a test to check the following:
If I create an Matrix4x4 with identity values only
Rotate the matrix by 45 degrees
Decompose the matrix and evaluate the quaternion returned (gives the x,y,z angles)
Check that the X value out matches the 45 degrees put in
I get the following results:
X:0.5 Y:0 Z:0
I was expecting:
X:0.45 Y:0 Z:0
Test Code
Quaternion quatDecomposed;
Vector3D translation;
Matrix4x4 rot = Matrix4x4.RotationAroundX(45);
rot.DecomposeNoScaling(out quatDecomposed, out translation);
I have created my own Matrix4x4, Vector3D and Angle3D structures shown in the examples below.
My Matrix4x4 rotate around x method is as follows:
public static Matrix4x4 RotationAroundX(double degrees)
{
// [1, 0, 0, 0]
// [0, cos,-sin,0]
// [0, sin,cos, 0]
// [0, 0, 0, 1]
// convert degrees to radians.
double radians = DoubleExtensions.DegreesToRadians(degrees);
// return matrix.
var matrixTransformed = Matrix4x4.Identity;
matrixTransformed.M22 = (float)Math.Cos(radians);
matrixTransformed.M23 = (float)-(Math.Sin(radians));
matrixTransformed.M32 = (float)Math.Sin(radians);
matrixTransformed.M33 = (float)Math.Cos(radians);
//return matrix;
return matrixTransformed;
}
My decompose no scaling method is as follows:
public void DecomposeNoScaling(out Quaternion rotation, out Vector3D translation)
{
translation.X = this[1, 4];
translation.Y = this[2, 4];
translation.Z = this[3, 4];
rotation = new Quaternion(new Matrix3x3(this));
}
What I am looking to get out is the angles contained within the Matrix4x4, I do this as follows:
Angle3D angles = new Angle3D(quatDecomposed.X, quatDecomposed.Y, quatDecomposed.Z);
Can anyone spot what I'm doing wrong? What I am REALLY trying to work out is the Euler angles from the matrix4x4 in ZYX order.
Thanks in advance!

Shouldn't be the last row of the matrix be "1" ?
[1 0 0 0]
[0 cos -sin 0]
[0 sin cos 0]
[0 0 0 1]
(last row last column should be 1)

Just in case anyone else needs to know, this is how I get the Euler angles directly from the Matrix:
public static Angle3D GetAngles(Matrix4x4 source)
{
double thetaX, thetaY, thetaZ = 0.0;
thetaX = Math.Asin(source.M32);
if (thetaX < (Math.PI / 2))
{
if (thetaX > (-Math.PI / 2))
{
thetaZ = Math.Atan2(-source.M12, source.M22);
thetaY = Math.Atan2(-source.M31, source.M33);
}
else
{
thetaZ = -Math.Atan2(-source.M13, source.M11);
thetaY = 0;
}
}
else
{
thetaZ = Math.Atan2(source.M13, source.M11);
thetaY = 0;
}
// Create return object.
Angle3D angles = new Angle3D(thetaX, thetaY, thetaZ);
// Convert to degrees.;
angles.Format = AngleFormat.Degrees;
// Return angles.
return angles;
}

Related

Normalize values to a range between -1 and 1

I want to process joystick values in such a way that the "normalized" values lay between -1 and 1 (including numbers with decimal places, for example 0.0129).
Specifically, I'm dealing with the input a Wiimote Controller stick.
The X axis has a range between 35 and 228, and the Y axis has range between 27 and 220.
The center for both is 128.
Now I would like to make it so that a value of 35 on the X axis would result in -1, the value of 128 should results in 0, and the value of 228 should result in 1.
Is there a special way to do with this?
The best I could come up with was:
public float[] GetStickNormalizedDataXY()
{
float[] ret = new float[2];
ret[0] = _stick[0];
ret[0] -= 35;
ret[1] = stick[1];
ret[1] -= 27;
for (int x = 0; x < 2; x++)
{
ret[x] /= 193f;
}
return ret;
}
But my results only vary between 0 and 1, so I guess I must be doing something wrong here.
This should do the trick:
float[] NormalizeStickData(float[] stickData)
{
return new[]
{
Normalize(stickData[0], 35, 228, 128, -1, 1, 0),
Normalize(stickData[1], 27, 220, 128, -1, 1, 0)
};
}
float Normalize(float value, float oldMin, float oldMax, float oldMid, float newMin, float newMax, float newMid)
{
if (value < oldMid)
return Interpolate(value, oldMin, oldMid, newMin, newMid);
else if (value > oldMid)
return Interpolate(value, oldMid, oldMax, newMid, newMax);
else
return newMid;
}
float Interpolate(float value, float oldMin, float oldMax, float newMin, float newMax)
{
return (float)(newMin + (newMax - newMin)*(value - oldMin)/(oldMax - oldMin));
}
Example
If you want to make a nice smooth function that takes on specific output values at 3 specific input values, the simplest thing to do is to use a quadratic polynomial. That means something of the form out=Ax^2 + Bx + C, where out is the output and x is the input. Plug in (x,out) = (35,-1), (128, 0), and (228,1) to get 3 equations, and solve for A, B, and C to get your x-axis mapping function.
If you'd like a more intuitive way to do exactly the same thing, then you can interpolate between 2 simpler linear functions like this:
float mapX(float x)
{
float xmin=35, xc=128, xmax=228;
// this line is correct for xmin and xc
float out1 -(x-xc)/(xmin-xc);
// this line is correct for xmax and xc
float out2 = (x-xc)/(xmax-xc);
// interpolate to use out1 at xmin and out2 at xmax
return out1 + (out2-out1)*(x-xmin)/(xmax-xmin);
}

Find which vec3 rotation to apply to align 2 triangles

I have a situation, I have 2 bodies and let's suppose one of them is in T-Pose and the other is idle, with it's arms looking down.
I have to find which rotation was applied to your elbow, for example, to match first body's (elbow-to-hand bone) with second body's (elbow-to-hand bone).
I thought about taking three vertex's (they have almost the same meshes, so that's not a problem but I cannot use vertex's normals bcause they're not the same).
Then the thing is, I have three points forming a triangle on first body and 3 points forming another triangle on second body and I want to find which rotation needs to be applied to one of the triangles to match the other.
If it helps, I'm doing it with unity and c#.
EDIT: IMPORTANT! triangles might not have the same dimensions but they are 2d so I only need to know it's rotation
If I understand your problem correctly, you need to find the transformation between two configurations of the same triangle in 2D: {v1,v2,v3} and {w1, w2,w3}. I assume you know vertex correspondance, for example v1->w1, v2->w2, v3->w3.
Here is what you can do:
1. find rotation angle between the two configurations.
2. rotate a point, for example v1 and obtain rotatedV1
3. find translation between rotatedV1 and w1.
Here is the code. For the sake of simplicity, I used WPF struct Vector3D. The final result is a standard double array collecting a 4x4 transformation matrix
static void miniTest() {
// first triangle
Vector3D v1 = new Vector3D();
Vector3D v2 = new Vector3D(2, 0, 0);
Vector3D v3 = new Vector3D(2, 3, 0);
Vector3D w1 = new Vector3D(0,-1, 0);
Vector3D w2 = new Vector3D(0 , -3, 0);
Vector3D w3 = new Vector3D(3, -1, 0);
double[,] transofrmation = getTrMatrix(v1, v2, v3, w1, w2, w3);
}
public static double[,] getTrMatrix(Vector3D V1, Vector3D V2, Vector3D V3, Vector3D W1, Vector3D W2, Vector3D W3) {
Vector3D s1 = V2 - V1;
s1.Normalize();
Vector3D z1 = W2 - W1;
z1.Normalize();
double angle = Math.Acos(Vector3D.DotProduct(s1, z1));
double[,] rotT = new double[,] {
{ Math.Cos(angle), -Math.Sin(angle), 0},
{ Math.Sin(angle), Math.Cos(angle), 0},
{ 0,0,1},
};
double[] rotatedV1 = multiply(rotT, V1);
Vector3D translation = new Vector3D( W1.X - rotatedV1[0], W1.Y - rotatedV1[1], W1.Z - rotatedV1[2]);
double[,] T = new double[,] {
{ Math.Cos(angle), -Math.Sin(angle), 0, translation.X},
{ Math.Sin(angle), Math.Cos(angle), 0, translation.Y},
{ 0,0,1, translation.Z},
{0,0,0,1 } };
return T;
}
// apply rotation matrix to vector
static double[] multiply(double[,] rotMat, Vector3D vec) {
double[] result = new double[3];
result[0] = rotMat[0, 0] * vec.X + rotMat[0, 1] * vec.Y + rotMat[0, 2] * vec.Z;
result[1] = rotMat[1, 0] * vec.X + rotMat[1, 1] * vec.Y + rotMat[1, 2] * vec.Z;
result[1] = rotMat[2, 0] * vec.X + rotMat[2, 1] * vec.Y + rotMat[2, 2] * vec.Z;
return result;
}

Rotating a dynamic 3D array of positions

So I have a system that holds a cluster of items in positions. The cluster is stored in an array as follows:
int[,,] = int[length, width, height];
Length, width, and height can all be different depending on the cluster. If I wanted to rotate the entire cluster by a set of degrees (ranging 0 to 360):
double rX, double rZ, double rY
How can I determine the new positions of each item and export in a new array?
My busted attempts all start like this:
int iX = Math.Abs(rX / 90), iZ = Math.Abs(rZ / 90), iY = Math.Abs(rY / 90);
if (iY == 1) // 90 or -90 degrees
{
group.Length = (rY / 90) * back.Width;
group.Width = (rY / 90) * back.Length;
}
else if (iY == 2) // 180 degrees
{
group.Length *= -1;
group.Width *= -1;
}
if (iZ == 1) // 90 or -90 degrees
{
group.Length = (rZ / 90) * back.Height;
group.Height = (rZ / 90) * back.Length;
}
else if (iZ == 2) // 180 degrees
{
group.Length *= -1;
group.Height *= -1;
}
if (iX == 1) // 90 or -90 degrees
{
group.Width = (rX / 90) * back.Height;
group.Height = (rX / 90) * back.Width;
}
else if (iX == 2) // 180 degrees
{
group.Width *= -1;
group.Height *= -1;
}
for(int gX = 0; gX < group.Length; gX++)
{
for (int gZ = 0; gZ < group.Width; gZ++)
{
for (int gY = 0; gY < group.Height; gY++)
{
//I lose track here.
}
}
}
From there I don't know where to go. group is the cluster I'm trying to rotate, and back is a copy of group before these operations. The array in this cluster is like this:
Cluster.Items[,,]
And it's sizes are set to the dimensions of the group. The array is based on a X (Length) Z (Width) Y (Height) axis.
I'm guessing the answer has something to do with matrices and flipping certain axis.
You will need a rotation matrix.
A rotation matrix is a matrix which when multiplied with a vector, will result is a rotation of that vector.
there are three types of rotation matrices
Rotation around x-Axis
Rx(a) = [ 1 0 0 0,
0 cos(a) -sin(a) 0,
0 sin(a) cos(a) 0,
0 0 0 1]
around y-Axis
Ry(a) = [ cos(a) 0 sin(a) 0,
0 1 0 0,
-sin(a) 0 cos(a) 0,
0 0 0 1]
ans rotation around z-Axis
Rz(a) = [ cos(a) -sin(a) 0 0,
sin(a) cos(y) 0 0,
0 0 1 0,
0 0 0 0]
More maths about rotation matrices you will find here
I'm still not convinced of your data structure. But let me just answer your question.
First, specify an order of rotations. In the following, I assume the order x, z, y. Then, find the according rotation matrix (e.g. from here). Then, multiply the position vector with the matrix to get the new vector.
If the old vector has coordinates x, y, z, then the new vector's x-coordinate would be (first row of the matrix):
newX = x * cos(rZ) * cos(rY) - y * sin(rZ) + z * cos(rZ) * sin(rY)
So the first entry in the row is multiplied with x, the second with y and so on. Insert the correct angles any you're done.
Since cosine and sine are always -1, 0, or 1 for degrees that are multiples of 90°, the according calculation can be improved to not use the actual sine and cosine functions.

How to adjust the distance of one 3D point from another 3D point by a given distance

I am working with point3D and vector3D classes and I need some help adjusting a point by a given distance.
Point A - point residing at coordinate 0,0,0.
Point B - point residing at coordinate 1,1,1.
Vector AB - vector AB which tells me the length between the two points A and B is distance = 1.73205078.
Code:
Point3D A = new Point3D { X = 0, Y = 0, Z = 0 };
Point3D B = new Point3D { X = 1, Y = 1, Z = 1 };
Vector3D AtoB = A - B;
Double distanceBetweenAandB = AtoB.Length; // the distance will be 1.73205078 here.
I would like to adjust point B. I would like to reduce the distance between point A and point B to 0.5 instead of 1 (adjusting to position C as shown in the diagram). I am trying to work out how to do this.
Point A (0,0,0) is known, point B (1,1,1) is known and the distance to adjust by is known (0.5). How do I calculate?
Pseudo code:
Point3D A = new Point3D { X = 0, Y = 0, Z = 0 };
Point3D B = new Point3D { X = 1, Y = 1, Z = 1 };
Double distanceToAdjust = 0.5;
Point3D newCoordinate = B - distanceToAdjust; // this doesnt work!
Adjusted point B shown in diagram below:
I am using my own defined Point3D class and Vector3D class.
Let's assume your given parameters for your points, and create a 3rd, which we'll call newCoordinate, and that point A will be your reference:
Point3D A = new Point3D { X = 0, Y = 0, Z = 0 };
Point3D B = new Point3D { X = 1, Y = 1, Z = 1 };
Double distanceToAdjust = 0.5;
Point3D newCoordinate = new Point3D {
A.X + ((B.X - A.X) * distanceToAdjust),
A.Y + ((B.Y - A.Y) * distanceToAdjust),
A.Z + ((B.Z - A.Z) * distanceToAdjust)
}
Here we see the original points:
Assuming this values, newCoordinate would sit at X=0.5, Y=0.5, Z=0.5. Nice graph follows:
There it is, sitting right in between the two original points.
As a simulation, if you change A and B and assume this values instead:
Point3D A = new Point3D { X = -8, Y = 4, Z = 3 };
Point3D B = new Point3D { X = 3, Y = 2, Z = 1 };
Then newCoordinate position would be X=-2.5, Y=3, Z=2.
Now, same points, but using distanceToAdjust = 1.2:
Keep this two things in mind:
Changes in distance always need a reference point. In my sample, I assumed A; that's why it appears as the first portion of each newCoordinate parameter initialization.
distanceToAdjust was taken as a multiplier factor.
Addendum: The nifty tool I used to help visualization can be found here.
Assuming you implemented vector operations:
if point A is always [0,0,0]
Point3D new = B.Normalize() * distance;
for any two points
Point3D newCoord = A + ((B - A).Normalize() * distance); //move to origin, normalize, scale and move back
not fast solution though.
"the length between the two points A and B is distance = 1"
No, the distance is the square root of three, about 1.732.
The distance from (0,0,0) to (0,0,1) is 1. The distance from (0,0,0) to (0,1,1) is the square root of two. (Think a triangle in two dimensions, and Pythagoas theorem.) The distance from (0,0,0) to (1,1,1) is the square root of three. (Think a triangle in two dimensions, where that dimension is on a plane along the hypothenuse of the previous triangle. AB = √(1² + (√2)²).)
I assume that you don't want to subtract 0.5 from anything, but actually multiply the distance by 0.5, i.e. getting halfways from A to B. You can calculate the point C by taking that part of the distance between point A and point B in each dimension:
Point3D C = new Point3D {
A.X + (B.X - A.X) * distanceToAdjust,
A.Y + (B.Y - A.Y) * distanceToAdjust,
A.Z + (B.Z - A.Z) * distanceToAdjust
};
In pseudo code, here's how I ended up implementing
pointA = …
pointB = …
vectorAB = B-A
desiredDistance = 0.5; // where 0.5 is vectorAB.Length/desiredDistance
vectorAC = vectorAB * desiredDistance ;
pointC = A+vectorAC;
Actual code:
Vector3D pointC = (Vector3D)(A + (float)desiredDistance * (B - A));
I'm unsure if this is what you would need but is it possible to create a method within your Point3D class to allow subtraction/addition?
(Just guessing the Point3D class as simply as it could be)Something like
public class Point3D
{
public double X,Y,Z
public void ChangeCord(Point3D point)
{
X =- point.X;
Y =- point.Y;
Z =- point.Z;
}
}
So it could just be:
Point3D A = new Point3D { X = 0, Y = 0, Z = 0 };
Point3D B = new Point3D { X = 1, Y = 1, Z = 1 };
Double distanceToAdjust = 0.5;
Point3D newCoordinate = B.ChangeCord(new Point3d{ X = 0.5, Y = 0.5, Z = 0.5 });

Calculating the Area of a Closed Polygon on a Plane

I'm attempting to calculate the area of a polygon that lies on a plane (a collection co-planar points forming a non-intersecting closed shape), and I know a method that can calculate the area of an irregular (or any) polygon in two dimensions - but not three. My solution is to rotate the plane so that it's normal is 0 in the z direction (so I can treat it like it's 2D) and then run the 2D area function.
The problem is I have NO idea how to actually determine the rotation axes and amounts to flatten a plane on it's Z-axis. I do my rotation through the easiest method I could find for 3 dimensional rotation: Rotation Matrices. So, given that I'm trying to use rotation matrices to do my rotation, how do I figure out the angles to rotate my plane by to be oriented in the same direction as another vector? I don't actually know much calculus or Euclidean geometry, so whichever solution requires me to teach myself the least of both is the ideal solution. Is there a better way?
Here's my attempt below, which doesn't even come close to getting the plane flat on the Z axis. This is an instance method of my "Surface" class, which is a derivative of my "Plane" class, and has an array of co-planar points (IntersectPoints) forming a closed polygon.
public virtual double GetArea()
{
Vector zUnit = new Vector(0, 0, 1); //vector perprendicualr to z
Vector nUnit = _normal.AsUnitVector();
Surface tempSurface = null;
double result = 0;
if (nUnit != zUnit && zUnit.Dot(nUnit) != 0) //0 = perprendicular to z
{
tempSurface = (Surface)Clone();
double xAxisAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.X);
double yAxisAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.Y);
double rotationAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.Z);
tempSurface.Rotate(xAxisAngle, yAxisAngle, rotationAngle); //rotating plane so that it is flat on the Z axis
}
else
{
tempSurface = this;
}
for (int x = 0; x < tempSurface.IntersectPoints.Count; x++) //doing a cross sum of each point
{
Point curPoint = tempSurface.IntersectPoints[x];
Point nextPoint;
if (x == tempSurface.IntersectPoints.Count - 1)
{
nextPoint = tempSurface.IntersectPoints[0];
}
else
{
nextPoint = tempSurface.IntersectPoints[x + 1];
}
double cross1 = curPoint.X * nextPoint.Y;
double cross2 = curPoint.Y * nextPoint.X;
result += (cross1 - cross2); //add the cross sum of each set of points to the result
}
return Math.Abs(result / 2); //divide cross sum by 2 and take its absolute value to get the area.
}
And here are my core rotation and get axis angle methods:
private Vector Rotate(double degrees, int axis)
{
if (degrees <= 0) return this;
if (axis < 0 || axis > 2) return this;
degrees = degrees * (Math.PI / 180); //convert to radians
double sin = Math.Sin(degrees);
double cos = Math.Cos(degrees);
double[][] matrix = new double[3][];
//normalizing really small numbers to actually be zero
if (Math.Abs(sin) < 0.00000001)
{
sin = 0;
}
if (Math.Abs(cos) < 0.0000001)
{
cos = 0;
}
//getting our rotation matrix
switch (axis)
{
case 0: //x axis
matrix = new double[][]
{
new double[] {1, 0, 0},
new double[] {0, cos, sin * -1},
new double[] {0, sin, cos}
};
break;
case 1: //y axis
matrix = new double[][]
{
new double[] {cos, 0, sin},
new double[] {0, 1, 0},
new double[] {sin * -1, 0, cos}
};
break;
case 2: //z axis
matrix = new double[][]
{
new double[] {cos, sin * -1, 0},
new double[] {sin, cos, 0},
new double[] {0, 0, 1}
};
break;
default:
return this;
}
return Physics.Formulae.Matrix.MatrixByVector(this, matrix);
}
public static double GetAxisAngle(Point a, Point b, Axes axis, bool inDegrees = true)
{ //pretty sure this doesnt actually work
double distance = GetDistance(a, b);
double difference;
switch (axis)
{
case Axes.X:
difference = b.X - a.X;
break;
case Axes.Y:
difference = b.Y - a.Y;
break;
case Axes.Z :
difference = b.Z - a.Z;
break;
default:
difference = 0;
break;
}
double result = Math.Acos(difference / distance);
if (inDegrees == true)
{
return result * 57.2957; //57.2957 degrees = 1 radian
}
else
{
return result;
}
}
A robust way to do this is to do a sum of the cross-products of the vertices of each edge. If your vertices are co-planar, this will produce a normal to the plane, whose length is 2 times the area of the closed polygon.
Note that this method is very similar to the 2D method linked in your question, which actually calculates a 2D equivalent of the 3D cross-product, summed for all edges, then divides by 2.
Vector normal = points[count-1].cross(points[0]);
for(int i=1; i<count; ++i) {
normal += points[i-1].cross(points[i]);
}
double area = normal.length() * 0.5;
Advantages of this method:
If your vertices are only approximately planar, it still gives the right answer
It doesn't depend on the angle of the plane.
In fact you don't need to deal with the angle at all.
If you want to know the plane orientation, you've got the normal already.
One possible difficulty: if your polygon is very small, and a long way away from the origin, you can get floating point precision problems. If that case is likely to arise, you should first translate all of your vertices so that one is at the origin, like so:
Vector normal(0,0,0);
Vector origin = points[count-1];
for(int i=1; i<count-1; ++i) {
normal += (points[i-1]-origin).cross(points[i]-origin);
}
double area = normal.length() * 0.5;
You need not to rotate the plane (or all points). Just calculate an area of polygon projection to Z-plane (if it is not perpendicular to polygon plane), for example, with you GetArea function, and divide result by cosinus of Poly-plane - Z-plane angle - it is equal to scalar product of zUnit and nUnit (I suggest that nUnit is normal vector to polygon plane)
TrueArea = GetArea() / zUnit.Dot(nUnit)

Categories