I am currently creating a rubiks cube project. The cube solves, but now I'm trying to implement a 3d model of this cube.
At the moment the x axis and z axis rotations work correctly, but the y axis rotation seems to start of as a cube but as it rotates round becomes more of a trapezium as it rotates 180'.
I have this code:
Point3D final;
double x = rotation.x;
final.x = original.x;
final.y = original.y * Math.Cos(x) - original.z * Math.Sin(x);
final.z = original.y * Math.Sin(x) + original.z * Math.Cos(x);
original.x = final.x;
original.y = final.y;
original.z = final.z;
x = rotation.y;
final.x = original.z * Math.Sin(x) + original.x * Math.Cos(x);
final.y = original.y;
final.z = original.y * Math.Cos(x) - original.x * Math.Sin(x);
original.x = final.x;
original.y = final.y;
original.z = final.z;
x = rotation.z;
final.x = original.x * Math.Cos(x) - original.y * Math.Sin(x);
final.y = original.x * Math.Sin(x) + original.y * Math.Cos(x);
final.z = original.z;
typo. Change line for y-rotation to
final.z = original.z * Math.Cos(x) - original.x * Math.Sin(x);
You were using original.y instead of original.z, but for a y-rotation the value of y does not play into the rotation.
May I suggest you define the rotations in methods
public static class Rotations
{
public static Point3D RotateAboutX(this Point3D point, double angle)
{
return new Point3D(
point.X,
Math.Cos(angle) * point.Y- Math.Sin(angle) * point.Z,
Math.Sin(angle) * point.Y+ Math.Cos(angle) * point.Z);
}
public static Point3D RotateAboutY(this Point3D point, double angle)
{
return new Point3D(
Math.Cos(angle) * point.X + Math.Sin(angle) * point.Z,
point.Y,
-Math.Sin(angle) * point.X + Math.Cos(angle) * point.Z);
}
public static Point3D RotateAboutZ(this Point3D point, double angle)
{
return new Point3D(
Math.Cos(angle) * point.X - Math.Sin(angle) * point.Y,
Math.Sin(angle) * point.X + Math.Cos(angle) * point.Y,
point.Z);
}
}
and then used them as needed. For Example
Point3D final = original.RotateAboutX(rotation.x)
.RotateAboutY(rotation.y)
.RotateAboutZ(rotation.z);
or the remain true to the original code
Point3D final = original.RotateAboutX(rotation.x);
original = final;
final = original.RotateAboutY(rotation.y);
original = final;
final = original.RotateAboutZ(rotation.z);
Related
I have seen many questions to conversions between Euler angles and Quaternion, but I never found any working solution. Maybe you can help me why this is not returning the right values. I need the conversion between Quaternions(XYZ) to Euler angles and this is the code I am currently using:
public static Vector3 Q2E(Quaternion q) // Returns the XYZ in ZXY
{
Vector3 angles;
angles.X = (float)Math.Atan2(2 * (q.W * q.X + q.Y * q.Z), 1 - 2 * (q.X * q.X + q.Y * q.Y));
if (Math.Abs(2 * (q.W * q.Y - q.Z * q.X)) >= 1) angles.Y = (float)Math.CopySign(Math.PI / 2, 2 * (q.W * q.Y - q.Z * q.X));
else angles.Y = (float)Math.Asin(2 * (q.W * q.Y - q.Z * q.X));
angles.Z = (float)Math.Atan2(2 * (q.W * q.Z + q.X * q.Y), 1 - 2 * (q.Y * q.Y + q.Z * q.Z));
return new Vector3()
{
X = (float)(180 / Math.PI) * angles.X,
Y = (float)(180 / Math.PI) * angles.Y,
Z = (float)(180 / Math.PI) * angles.Z
};
}
Thx everyone.
Your title is from Euler angles to Quaternions but you sample code is 'supposed' to convert from Quaternion to Euler.
Is below what you are looking for?
public class Program
{
public static void Main(string[] args)
{
EulerAngles e = new();
e.roll = 0.14;
e.pitch = 1.21;
e.yaw = 2.1;
// convert the Euler angles to Quaternions:
Quaternion q = ToQuaternion(e.yaw,e.pitch,e.roll);
// convert the same Quaternion back to Euler angles:
EulerAngles n = ToEulerAngles(q);
// verify conversion
Console.WriteLine($"Q: {q.x} {q.y} {q.z} {q.w}");
Console.WriteLine($"E: {n.roll} {n.pitch} {n.yaw}");
}
public class Quaternion
{
public double w;
public double x;
public double y;
public double z;
}
public class EulerAngles
{
public double roll; // x
public double pitch; // y
public double yaw; // z
}
public static Quaternion ToQuaternion(double yaw, double pitch, double roll)
{
double cy = Math.Cos(yaw * 0.5);
double sy = Math.Sin(yaw * 0.5);
double cp = Math.Cos(pitch * 0.5);
double sp = Math.Sin(pitch * 0.5);
double cr = Math.Cos(roll * 0.5);
double sr = Math.Sin(roll * 0.5);
Quaternion q = new Quaternion();
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;
return q;
}
public static EulerAngles ToEulerAngles(Quaternion q)
{
EulerAngles angles = new();
// roll (x-axis rotation)
double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
angles.roll = Math.Atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (q.w * q.y - q.z * q.x);
if (Math.Abs(sinp) >= 1)
{
angles.pitch = Math.CopySign(Math.PI / 2, sinp);
}
else
{
angles.pitch = Math.Asin(sinp);
}
// yaw (z-axis rotation)
double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
angles.yaw = Math.Atan2(siny_cosp, cosy_cosp);
return angles;
}
}
UPDATE: Using built-in classes for Quaternion and Euler Angles (Vector3):
using System.Numerics;
public static void Main()
{
Vector3 v = new() { X = 0.14F, Y = 1.21F, Z = 2.1F };
Quaternion q = ToQuaternion(v);
Vector3 n = ToEulerAngles(q);
Console.WriteLine($"Q: {q.X} {q.Y} {q.Z} {q.W}");
Console.WriteLine($"E: {n.X} {n.Y} {n.Z}");
}
public static Quaternion ToQuaternion(Vector3 v)
{
float cy = (float)Math.Cos(v.Z * 0.5);
float sy = (float)Math.Sin(v.Z * 0.5);
float cp = (float)Math.Cos(v.Y * 0.5);
float sp = (float)Math.Sin(v.Y * 0.5);
float cr = (float)Math.Cos(v.X * 0.5);
float sr = (float)Math.Sin(v.X * 0.5);
return new Quaternion
{
W = (cr * cp * cy + sr * sp * sy),
X = (sr * cp * cy - cr * sp * sy),
Y = (cr * sp * cy + sr * cp * sy),
Z = (cr * cp * sy - sr * sp * cy)
};
}
public static Vector3 ToEulerAngles(Quaternion q)
{
Vector3 angles = new();
// roll / x
double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
angles.X = (float)Math.Atan2(sinr_cosp, cosr_cosp);
// pitch / y
double sinp = 2 * (q.W * q.Y - q.Z * q.X);
if (Math.Abs(sinp) >= 1)
{
angles.Y = (float)Math.CopySign(Math.PI / 2, sinp);
}
else
{
angles.Y = (float)Math.Asin(sinp);
}
// yaw / z
double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
angles.Z = (float)Math.Atan2(siny_cosp, cosy_cosp);
return angles;
}
I am using the following code to calculate points for a circle arc drawn with a Line Renderer.
for (int i = 0; i <= pts; i++)
{
float x = center.x + radius * Mathf.Cos(ang * Mathf.Deg2Rad);
float y = center.y + radius * Mathf.Sin(ang * Mathf.Deg2Rad);
arcLine.positionCount = i + 1;
arcLine.SetPosition(i, new Vector2(x, y));
ang += (float)totalAngle / pts;
}
How can I change the angle ang to create a reflected arc along the line P1P2 as in the image below?
Please note that totalAngle represents the portion of the circle that is to be drawn between 0 and 360.
I'm not entirely sure that's possible, but I've come up with another way that works like this:
First, a helper function:
Vector2 GetPosition (float radius, float angle)
{
angle *= Mathf.Deg2Rad;
return new Vector2
{
x = radius * Mathf.Cos(angle),
y = radius * Mathf.Sin(angle)
};
}
Then, compute positions p1 and p2:
var p1 = GetPosition(radius, ang);
var p2 = GetPosition(radius, totalAngle);
To derive the mid-point p3:
var p3 = (p1 + p2) * 0.5f;
And finally rotate the original point about p3 to obtain the reflected point:
var pos = p3 * 2f - GetPosition(radius, ang);
And that's it! Your code should look something like this:
void Draw ()
{
var p1 = GetPosition(radius, ang);
var p2 = GetPosition(radius, totalAngle);
var p3 = (p1 + p2) * 0.5f;
for (int i = 0; i <= pts; i++)
{
var pos = p3 * 2f - GetPosition(radius, ang);
arcLine.positionCount = i + 1;
arcLine.SetPosition(i, center + pos);
ang += totalAngle / pts;
}
}
Vector2 GetPosition (float radius, float angle)
{
angle *= Mathf.Deg2Rad;
return new Vector2
{
x = radius * Mathf.Cos(angle),
y = radius * Mathf.Sin(angle)
};
}
Here's it in action:
I am trying to write an algorithm to convert my mouse click to 3D coordinates (to insert an object at this point).
I have "ground" level where Y = 0 and I want to calculate X and Z based on my mouse click. My function currently looks like that:
Point p = this.control.PointToClient(new Point(System.Windows.Forms.Cursor.Position.X, System.Windows.Forms.Cursor.Position.Y));
Vector3 pos = GeometryHelper.Unproject(new Vector3(p.X, 0f, p.Y), viewport.X, viewport.Y, viewport.Width, viewport.Height, projectionPlane.Near, projectionPlane.Far, Matrix4.Invert(mProjectionMatrix * camera.GetViewMatrix()));
active.applyGeometry(pos);
function applyGeometry simply sets the position of an object. I believe passed arguments are self-explanatory.
My Unproject function looks this way:
public static Vector3 Unproject(Vector3 vector, float x, float y, float width, float height, float minZ, float maxZ, Matrix4 inverseWorldViewProjection)
{
Vector4 result;
result.X = ((((vector.X - x) / width) * 2.0f) - 1.0f);
result.Y = ((((vector.Y - y) / height) * 2.0f) - 1.0f);
result.Z = (((vector.Z / (maxZ - minZ)) * 2.0f) - 1.0f);
result.X =
result.X * inverseWorldViewProjection.M11 +
result.Y * inverseWorldViewProjection.M21 +
result.Z * inverseWorldViewProjection.M31 +
inverseWorldViewProjection.M41;
result.Y =
result.X * inverseWorldViewProjection.M12 +
result.Y * inverseWorldViewProjection.M22 +
result.Z * inverseWorldViewProjection.M32 +
inverseWorldViewProjection.M42;
result.Z =
result.X * inverseWorldViewProjection.M13 +
result.Y * inverseWorldViewProjection.M23 +
result.Z * inverseWorldViewProjection.M33 +
inverseWorldViewProjection.M43;
result.W =
result.X * inverseWorldViewProjection.M14 +
result.Y * inverseWorldViewProjection.M24 +
result.Z * inverseWorldViewProjection.M34 +
inverseWorldViewProjection.M44;
result /= result.W;
return new Vector3(result.X, result.Y, result.Z);
}
The problem is that Unproject function returns result close to 0,0,0 and that makes my object appear around 0,0,0. Any idea how to modify it to work properly?
Update
I believe I have given enough details on this case, but in case you'd need something else to help me out just do not hesitate to tell me ; )
This question already has answers here:
Rotating a point about another point (2D)
(6 answers)
Closed 9 years ago.
I have list of points containing x and y locations of a page. I want to apply rotation on all these points relative to any pivot point of page (currently lets assume its center).
var points = new List<Point>();
points.Add(1,1);
points.Add(15,18);
points.Add(25,2);
points.Add(160,175);
points.Add(150,97);
const int pageHeight = 300;
const int pageWidth = 400;
var pivotPoint = new Point(200, 150); //Center
var angle = 45; // its in degree.
// Apply rotation.
Do I need some formula here?
public static Point Rotate(Point point, Point pivot, double angleDegree)
{
double angle = angleDegree * Math.PI / 180;
double cos = Math.Cos(angle);
double sin = Math.Sin(angle);
int dx = point.X - pivot.X;
int dy = point.Y - pivot.Y;
double x = cos * dx - sin * dy + pivot.X;
double y = sin * dx + cos * dy + pivot.X;
Point rotated = new Point((int)Math.Round(x), (int)Math.Round(y));
return rotated;
}
static void Main(string[] args)
{
Console.WriteLine(Rotate(new Point(1, 1), new Point(0, 0), 45));
}
If you have a large number of points to rotate, you might want to precompute the rotation matrix…
[C -S U]
[S C V]
[0 0 1]
…where…
C = cos(θ)
S = sin(θ)
U = (1 - C) * pivot.x + S * pivot.y
V = (1 - C) * pivot.y - S * pivot.x
You then rotate each point as follows:
rotated.x = C * original.x - S * original.y + U;
rotated.x = S * original.x + C * original.y + V;
The above formula is the result of combining three transforms…
rotated = translate(pivot) * rotate(θ) * translate(-pivot) * original
…where…
translate([x y]) = [1 0 x]
[0 1 y]
[0 0 1]
rotate(θ) = [cos(θ) -sin(θ) 0]
[sin(θ) cos(θ) 0]
[ 0 0 1]
if u rotate a point(x,y) around point (x1,y1) by an angle some a then you need a formula...
x2 = cos(a) * (x-x1) - sin(a) * (y-y1) + x1
y2 = sin(a) * (x-x1) + cos(a) * (y-y1) + y1
Point newRotatedPoint = new Point(x2,y2)
I need to find a point where a line (its origin is ellipse' center) intersects an ellipse in 2D... I can easily find a point on a circle, because I know an angle F and the circle' radius (R):
x = x0 + R * cosF
y = y0 + R * sinF
However I just can't figure how am I supposed to deal with an ellipse... I know it's dimensions (A & B), but what is the way of finding parameter T?!
x = x0 + A * cosT
y = y0 + B * sinT
From what I understand the parameter T (T angle) is not far from the F angle (approximately +-15 degrees in some cases), but I just can't figure how to calculate it!!!
If there is a kind hearted soul, please help me with this problem...
The standard equation of an ellipse, stationed at 0,0, is:
1 = (x)^2 / (a) + (y)^2 / (b)
Where a is 1/2 the diameter on the horizontal axis, and b is 1/2 the diameter on the vertical axis.
you have a line, assuming an equation:
y = (m)(x - x0) + y0
So, let us plug-and-play!
1 = (x)^2 / (a) + (m(x - x0) + y0)^2 / (b)
1 = x^2 / a + (mx + (y0 - mx0))^2 / b
1 = x^2 / a + (m^2 * x^2 + 2mx*(y0 - mx0) + (y0 - mx0)^2) / b
1 = x^2 / a + (m^2 x^2) / b + (2mx*(y0 - mx0) + (y0^2 - 2y0mx0 + m^2*x0^2)) / b
1 = ((x^2 * b) / (a * b)) + ((m^2 * x^2 * a) / (a * b)) + (2mxy0 - 2m^2xx0)/b + (y0^2 - 2y0mx0 + m^2*x0^2)/b
1 = ((bx^2 + am^2x^2)/(ab)) + (x*(2my0 - 2m^2x0))/b + (y0^2 - 2y0mx0 + m^2*x0^2)/b
0 = x^2*((b + a*m^2)/(ab)) + x*((2my0 - 2m^2x0)/b) + (((y0^2 - 2y0mx0 + m^2*x0^2)/b) - 1)
That last equation follows the form of a standard quadratic equation.
So just use the quadratic formula, with:
((b + a*m^2)/(ab))
((2my0 - 2m^2x0)/b)
and
(((y0^2 - 2y0mx0 + m^2*x0^2)/b) - 1)
to get the X values at the intersections; Then, plug in those values into your original line equation to get the Y values.
Good luck!
Don't do it this way. Instead check the equation that forms an ellipse and that forming a line and solve the set:
The ellipse: (x/a)^2 + (y/b)^2 = 1
Your line: y = cx
You know a, b and c, so finding a solution is going to be easy. You'll find two solutions, because the line crosses the ellipse twice.
EDIT: Note I moved your ellipse's center to (0,0). It makes everything easier. Just add (x0,y0) to the solution.
public Hits<float2> EllipseLineIntersection ( float rx , float ry , float2 p1 , float2 p2 )
{
Hits<float2> hits = default(Hits<float2>);
float2 p3, p4;
Rect rect = default(Rect);
{
rect.xMin = math.min(p1.x,p2.x);
rect.xMax = math.max(p1.x,p2.x);
rect.yMin = math.min(p1.y,p2.y);
rect.yMax = math.max(p1.y,p2.y);
}
float s = ( p2.y - p1.y )/( p2.x - p1.x );
float si = p2.y - ( s * p2.x );
float a = ( ry*ry )+( rx*rx * s*s );
float b = 2f * rx*rx * si * s;
float c = rx*rx * si*si - rx*rx * ry*ry;
float radicand_sqrt = math.sqrt( ( b*b )-( 4f * a * c) );
p3.x = ( -b - radicand_sqrt )/( 2f*a );
p4.x = ( -b + radicand_sqrt )/( 2f*a );
p3.y = s*p3.x + si;
p4.y = s*p4.x + si;
if( rect.Contains(p3) ) hits.Push( p3 );
if( rect.Contains(p4) ) hits.Push( p4 );
return hits;
}
public struct Hits<T>
{
public byte count;
public T point0, point1;
public void Push ( T val )
{
if( count==0 ) { point0 = val; count ++; }
else if( count==1 ) { point1 = val; count ++; }
else print("This structure can only fit 2 values");
}
}
I wrote a C# code for your problem and I hope you can find it helpful. the distance function inside this code calculates euclidean distance between two points in space.
wX denotes horizontal radios of ellipse and wY denotes vertical radios.
private PointF LineIntersectEllipse(PointF A, PointF B, float wX, float wY)
{
double dx = B.X - A.X;
double dy = B.Y - A.Y;
double theta = Math.Atan2(dy, dx);
double r = distance(A, B) - ((wX * wY) / Math.Sqrt(Math.Pow(wY * Math.Cos(theta), 2) + Math.Pow(wX * Math.Sin(theta), 2)));
return PointF((float)(A.X + r * Math.Cos(theta)), (float)(A.Y + r * Math.Sin(theta)));
}
Andrew Łukasik posted a good and useful answer, however it is not using regular C# types. As I wrote in the comments, I converted the code using System.Drawing objects PointF and RectangleF. I found out that if the points given as parameters are aligned as a vertical or horizontal line, then "rect" will have a width or a height equal to 0. Then, rect.Contains(point) will return false even if the point is on this line.
I also modified the "Hits" structure to check if the point pushed is not already existing, which is the case if the line is perfectly tangent, then p3 and p4 will have same coordinates, as the exact tangent point is the only crossing point.
Here is the new code taking care of all the cases :
public static Hits<PointF> EllipseLineIntersection0(float rx, float ry, PointF p1, PointF p2)
{
Hits<PointF> hits = default(Hits<PointF>);
PointF p3 = new PointF();
PointF p4 = new PointF();
var rect = default(RectangleF);
rect.X = Math.Min(p1.X, p2.X);
rect.Width = Math.Max(p1.X, p2.X) - rect.X;
rect.Y = Math.Min(p1.Y, p2.Y);
rect.Height = Math.Max(p1.Y, p2.Y) - rect.Y;
float s = (p2.Y - p1.Y) / (p2.X - p1.X);
float si = p2.Y - (s * p2.X);
float a = (ry * ry) + (rx * rx * s * s);
float b = 2f * rx * rx * si * s;
float c = rx * rx * si * si - rx * rx * ry * ry;
float radicand_sqrt = (float)Math.Sqrt((b * b) - (4f * a * c));
p3.X = (-b - radicand_sqrt) / (2f * a);
p4.X = (-b + radicand_sqrt) / (2f * a);
p3.Y = s * p3.X + si;
p4.Y = s * p4.X + si;
if (rect.Width == 0)
{
if (p3.Y >= rect.Y && p3.Y <= rect.Y + rect.Height) hits.Push(p3);
if (p4.Y >= rect.Y && p4.Y <= rect.Y + rect.Height) hits.Push(p4);
}
else if (rect.Height == 0)
{
if (p3.X >= rect.X && p3.X <= rect.X + rect.Width) hits.Push(p3);
if (p4.X >= rect.X && p4.X <= rect.X + rect.Width) hits.Push(p4);
}
else
{
if (rect.Contains(p3)) hits.Push(p3);
if (rect.Contains(p4)) hits.Push(p4);
}
return hits;
}
public struct Hits<T>
{
public byte Count;
public T P0, P1;
public void Push(T val)
{
if (Count == 0) { P0 = val; Count++; }
else if (Count == 1) { if (!P0.Equals(val)) { P1 = val; Count++; } }
else throw new OverflowException("Structure Hits can only fit 2 values.");
}
}