# What is the right method to convert quaternion to plane using rhinocommon

I converted some C++ code from this website http://bediyap.com/programming/convert-quaternion-to-euler-rotations/ to C# seems to work. Apparently swaping axis is not enough. Hope it helps for someone suffering from Unity Axis system and Rhino integration.

``````  private void RunScript(object y, ref object A)
{

Quaternion q = new Quaternion(0.1f, 0.2f, 0.4f, 0.5f);
//Quaternion q = new Quaternion(0, 0,1, 0);
// Quaternion q = new Quaternion(0, 0,0.7, -0.7);
q.normalize();
double[] res = new double[3]{0,0,0};
quaternion2Euler(q, ref res, RotSeq.yxz);//yxz
A = new double[]{res[1],res[2],res[0]};

}

///////////////////////////////
// Quaternion struct
// Simple incomplete quaternion struct for demo purpose
///////////////////////////////
public struct Quaternion{
//
//    public  Quaternion(){
//      this.x = 0;
//      this.y = 0;
//      this.z = 0;
//      this.w = 0;
//
//    }

public  Quaternion(double x, double y, double z, double w){
this.x = x;
this.y = y;
this.z = z;
this.w = w;

}

public  void normalize(){
double norm = Math.Sqrt(x * x + y * y + z * z + w * w);
x /= norm;
y /= norm;
z /= norm;
w /= norm;
}

public double norm(){
return Math.Sqrt(x * x + y * y + z * z + w * w);
}

public  double x;
public  double y;
public  double z;
public  double w;

};

///////////////////////////////
// Quaternion to Euler
///////////////////////////////
public enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};

void twoaxisrot(double r11, double r12, double r21, double r31, double r32, ref double[] res){
res[0] = Math.Atan2(r11, r12);
res[1] = Math.Acos(r21);
res[2] = Math.Atan2(r31, r32);
}

void threeaxisrot(double r11, double r12, double r21, double r31, double r32, ref double[] res){
res[0] = Math.Atan2(r31, r32);
res[1] = Math.Asin(r21);
res[2] = Math.Atan2(r11, r12);
}

// note:
// return values of res[] depends on rotSeq.
// i.e.
// for rotSeq zyx,
// x = res[0], y = res[1], z = res[2]
// for rotSeq xyz
// z = res[0], y = res[1], x = res[2]
// ...
void quaternion2Euler(Quaternion q, ref double[] res, RotSeq rotSeq)
{
switch(rotSeq){

case RotSeq.zyx:
threeaxisrot(2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
-2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
ref res);
break;

case RotSeq.zyz:
twoaxisrot(2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
ref res);
break;

case RotSeq.zxy:
threeaxisrot(-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
ref res);
break;

case RotSeq.zxz:
twoaxisrot(2 * (q.x * q.z + q.w * q.y),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
ref res);
break;

case RotSeq.yxz:
threeaxisrot(2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
-2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
ref res);
break;

case RotSeq.yxy:
twoaxisrot(2 * (q.x * q.y - q.w * q.z),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
ref res);
break;

case RotSeq.yzx:
threeaxisrot(-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
ref res);
break;

case RotSeq.yzy:
twoaxisrot(2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
ref res);
break;

case RotSeq.xyz:
threeaxisrot(-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
ref res);
break;

case RotSeq.xyx:
twoaxisrot(2 * (q.x * q.y + q.w * q.z),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
ref res);
break;

case RotSeq.xzy:
threeaxisrot(2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
-2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
ref res);
break;

case RotSeq.xzx:
twoaxisrot(2 * (q.x * q.z - q.w * q.y),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
ref res);
break;

default:
//std::cout << "Unknown rotation sequence" << std::endl;
break;

}
}

///////////////////////////////
// Helper functions
///////////////////////////////
public static Quaternion Multiply(Quaternion q1, Quaternion q2){
Quaternion q = new Quaternion(0, 0, 0, 0);
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
q.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
q.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return q;
}

return Math.Abs(rad * 180.0 / Math.PI+360)%360;
}``````
1 Like

Cool you could make it.

I looked at my code and for sure Axis transform are difficult. I had difficulties with my own code!!!
Here a C# that takes Euler Angles transform them to Quaternion and then recalculate Euler Angles and show the resulting planes. So you will see that often angles are not the same (180° difference) but the transforms are the same (geometry transformed are on the same place).
quaternion_euler_quat.gh (35.7 KB)

For your problem in Unity if they choose to not follow the “classical” order Yaw Pitch Roll (Z, Y, X) for sure it is difficult.

Thank you for the code, really seeing all this script with conversion helps a lot!

For conversions back from Euler to Quaternion can I use this method or it also needs somehow be adapted?

``````public static Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
// Abbreviations for the various angular functions
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;
q = Quaternion.Zero;
q.A = cy * cp * cr + sy * sp * sr;
q.B = cy * cp * sr - sy * sp * cr;
q.C = sy * cp * sr + cy * sp * cr;
q.D = sy * cp * cr - cy * sp * sr;

return q;
}``````

For the conversion from Euler to Quaternion if you want to better control things you can use that

``````   Quaternion r = Quaternion.Rotation(roll, Vector3d.XAxis);
Quaternion p = Quaternion.Rotation(pitch, Vector3d.YAxis);
Quaternion y = Quaternion.Rotation(yaw, Vector3d.ZAxis);

Quaternion _rot = y * p * r;

Quaternion xbody = new Quaternion(0, 1, 0, 0);
Quaternion xRotated = _rot * xbody * _rot.Conjugate;

Quaternion ybody = new Quaternion(0, 0, 1, 0);
Quaternion yRotated = _rot * ybody * _rot.Conjugate;

Quaternion zbody = new Quaternion(0, 0, 0, 1);
Quaternion zRotated = _rot * zbody * _rot.Conjugate;

Plane _plane = Plane.WorldXY;
_rot.GetRotation(out _plane);
``````

What is important here is that r, p, y are simple transform around each axis.
ypr is the classical way. But if you want Unity Quaternion you probably need something like p * y * r.

here a script with transform using previous example and using the implementation I use.
quaternion.gh (29.4 KB)

1 Like

Thank you

Dear @laurent_delrieu ,

I would like to reopen this case, since you know a lot about euler rotations.
Is there any way to compute axis angles for non orthogonal systems.
In the image below there is a cnc machine, where the 5th axis is tilted by 50 degrees. I know the target direction / normal of a plane and searching for a way to compute 4th and 5th axis rotations.

Do you know any method or where to start to solve this puzzle?

Hello @Petras_Vestartas, I am not used to do such things but, I think one strategy will be to first find the elevation/slope and azimuth of your normal.

With the slope find the angle of b
Slope = 0° => normal = Z => b=0°

each time b is not 0° or 180° an azimuth is generated, measure or calculate it

Then find a

a = Normal Azimuth - Azimuth(b)

Here a Grasshopper file with multiples transform

• shift of 7.5 units between the 5th axis rotation and axis of Drill
• shift of 13.5 between the end of drill and the 5th axis rotation

The head with the rotation around Z 4th axis is shifted (3.5,0,14.5).

So here a complete GH

The logic of transforms are simple
For the 5th dimension,

• The end of drill is at (0,0,0)
• Move the head to put the rotation center to (0,0,0)
• Rotate -50° on XZ plane
• Rotate 5th axis on XY plane
• Rotate 50° on XZ plane
• Move to put 4 axis on (0,0,0)
• Rotate 4th axis on XY plane
• Translate XY

Less transformations for 1,2,3, 4 DoF head

• Rotate 4th axis on XY plane
• Translate XY

Next work write the rotation matrix in order to calculate the rotations and be able to calculate 4th and 5th axis rotation.

Not sure I don’t make error, but I will use this page as remainder of steps (better use Jupyter notebook)
I used that to write the various matrix of rotation and multiply them

a is 4th axis
b is 5th axis
h is 50°

I multiply by vector (0,0,1) that is the normal vector of surface. So if this vector is rotate to slope and azimuth it will become (sin(elevation)*cos(azimuth); sin(elevation)sin(azimuth), cos (elevation))
let first find elevation
cos (elevation) = cos (h)² +sin(h)²
cos(b)
b = acos((cos(elevation)-cos(h)²) /sin(h)²)

A bit lazy to solve the second equation, so I did the brute force method, all angles of a with 0.01 deg precision !!! But it seems to work. So now I input Elevation/Slope and azimuth and calculate the b and a angle. As you have position of drill you can then compute X,Y and Z translations.
You are limited to 100° of slope.

Must made a particular discussion as it could be helpful for CNC enthusiast.

2 Likes

Wow… Thank you a lot.
Trying to understand your work right now.

CNC Model.gh (1.7 MB)

Do you have any ideas how to find the angle without brute force method in C# loop?

Here the script with your model
You can move the slider of slope (limited now to 100°) and Azimuth angle, it will move the model

The 2 angles calculated are put in “Number” component

You have the drill position

Thank you.

It is interesting that the first part of your grasshopper components is this math atan2 formula:

``````
private void RunScript(Vector3d normal, double y, double a, double b, ref object A, ref object B)
{
Tuple<double, double> result = AB180(normal);

A = result.Item1 + 90;
B = -result.Item2;

}

public Tuple<double, double> AB180(Vector3d n0)
{

double a = Math.Atan2(n0.X, n0.Y);
double b = Math.Atan2(Math.Sqrt(n0.X * n0.X + n0.Y * n0.Y), n0.Z);

double A = Math.Round(-1 * Rhino.RhinoMath.ToDegrees(a), 3);
double B = Math.Round(-1 * Rhino.RhinoMath.ToDegrees(b), 3);

return new Tuple<double, double>(A, B);
}

}
``````

Do you have any clue how to mathematically calculate the second angle instead of for loop ? Atan2.gh (1.8 MB)

The sin / cos conversion seems a magic for me how to end up finding the correct value.

It is far more simple, with say “a =0”, the rotation “b” gives an azimuth, the rotation of head is just the difference between Azimuth of vector and Azimuth for head with a = 0.
Atan2_LD.gh (1.8 MB)

So quite simple at the end.

1 Like

It is so neat…
Thank you a ton.

Do you know why the calculation cannot be performed when the vector is pointing upwards?
This is the limit:

This is all in one:

`````` private void RunScript(Vector3d normal, ref object A, ref object B)
{

//Step1
Tuple<double, double> result = AB180(normal);
double a = result.Item1 + Math.PI * 0.5;
double b = -result.Item2;

//Step2
double cEl = Math.Cos(b);
double sH = Math.Sin(tilt_angle);
double cH = Math.Cos(tilt_angle);
double rotation = Math.Acos((cEl - cH * cH) / (sH * sH));

//Step3 "Orientation with a=0"
//Search for 4th axis angle, named a
Vector3d newNormal = CalculateVector(0, rotation, tilt_angle);

//Step4 recalculate b-axis (azimuth)
result = AB180(newNormal);
b = result.Item1 + Math.PI * 0.5;

//Step5 Result
b = a - b;
a = rotation;

A = Rhino.RhinoMath.ToDegrees(a);
B = Rhino.RhinoMath.ToDegrees(b);

}

public Tuple<double, double> AB180(Vector3d n0)
{
double a = -Math.Atan2(n0.X, n0.Y);
double b = -Math.Atan2(Math.Sqrt(n0.X * n0.X + n0.Y * n0.Y), n0.Z);
return new Tuple<double, double>(a, b);
}

/// <summary>
///
/// </summary>
/// <param name="a">a angle in radians(4th degree of freedom)</param>
/// <param name="b">b angle in radians (5th degree of freedom)</param>
/// <param name="h">h angle of the head</param>
/// <returns></returns>
Vector3d CalculateVector(double a, double b, double h)
{

double x = -Math.Cos(a) * Math.Sin(h) * Math.Cos(-h) - Math.Sin(-h) * (Math.Cos(a) * Math.Cos(h) * Math.Cos(b) - Math.Sin(a) * Math.Sin(b));
double y = -Math.Sin(a) * Math.Sin(h) * Math.Cos(-h) - Math.Sin(-h) * (Math.Sin(a) * Math.Cos(h) * Math.Cos(b) + Math.Cos(a) * Math.Sin(b));
double z = Math.Cos(h) * Math.Cos(-h) - Math.Sin(h) * Math.Cos(b) * Math.Sin(-h);

return new Vector3d(x, y, z);
}
``````

Atan2_LD2.gh (1.8 MB)

The limit is 100 degree related to the angle of 50 degree

Thank you:)