Repository: FloatingArrayDesign/MoorDyn (language: python)

Issue:
Euler angle conventions and Body input/output angles
Hi all, 
I installed MoorDynv2 matlab wrapper. I was testing input and output angles for Body object. 
[lines_test.txt](https://github.com/FloatingArrayDesign/MoorDyn/files/12912904/lines_test.txt)

I am modelling a single Body as coupled object and trying to provide motion externally calculated. 


the script I am using is below:
clc;close all;clear
%% Create MoorDyn system
 system1 = MoorDynM_Create('lines_test.txt');
%% find num of DOF
DOF =MoorDynM_NCoupledDOF(system1);
x = zeros(1,DOF);
dx = zeros(1,DOF);

%% Setup the initial condition
MoorDynM_Init(system1, x, dx);

%% Simulate
dt =0.04;
t = 0:dt:10;
x =zeros(length(t),DOF);
dx =zeros(length(t),DOF);
% provide roll angle ansd speed
roll_speed = 5; % deg/sec
x(:,4) = t*roll_speed;
dx(:,4) = roll_speed;
% run the moordyn
tic
for i=1:length(t)-1
[sim_time, f] = MoorDynM_Step(system1, x(i,:), dx(i,:), t(i),dt);
end
%% Alright, time to finish!
MoorDynM_Close(system1);
toc
%% plot
data=readtable('lines_test_Body1.out','FileType','text');
VariableNames = {'Surge','Sway','Heave','Roll','Pitch','yaw'};
indx =4;
figure(1);plot(t,x(:,indx))
hold on
plot(t,data{:,1+indx})
hold off
title(VariableNames{indx})


I am only giving 5 deg/s angular speed in roll. Since there is no other element, i was expecting the roll angle will be same for input and ouput. Not only the roll angles saved by MoorDyn is quite different than expected, but unexplained pitch and yaw angles. 

![image](https://github.com/FloatingArrayDesign/MoorDyn/assets/106804145/bd1d6024-905c-49cc-9d3b-4ef4353cd150)

![image](https://github.com/FloatingArrayDesign/MoorDyn/assets/106804145/bfeda13a-4fb2-4693-9a9a-d360abe82fdc)

![image](https://github.com/FloatingArrayDesign/MoorDyn/assets/106804145/6db92ea1-cd2a-4d13-be01-f78d7b0246e2)

Can anyone explain whats going on here ? Thanks in advance. 



Discussion / hints:
Firstly I believe that MoorDyn expects angular values to be in radians (angles are radians, angular velocities are rad/s). So you are specifying a velocity of 5 radians per second, which looks like about what the graph is showing.

Secondly, MoorDyn internally uses quaternions to represent rotation, and converts to and from euler angles when taking input or returning output. This lets MoorDyn avoid any issues of gimbal lock. But this does mean that the exact set of angles used to define a rotation are not necessarily preserved between input and output. 
It just so happens that with the XYZ euler angle convention that MoorDyn uses, the angles
roll = 180 deg, pitch = 0 deg, yaw = 0 deg
and
roll = 0 deg, pitch = 180 deg, yaw = 180 deg
are in fact the same rotation. And so when the simulation reaches the point where roll = 180, the quaternion to euler angle conversion inside of MoorDyn ends up spitting out the second set of Euler angles instead of the first.
This online calculator I have found helpful in working with 3d rotations
https://www.andre-gaschler.com/rotationconverter/

In your specific case, besides the lack of a degrees to radians conversion, you probably want to fully specify the body orientation and not just the roll component. That should properly specify the motion you want, but it doesn't solve the issue of the orientation reported by MoorDyn potentially being different from the one you specified. In general solving that problem is challenging, but one way to address it would be to do your own conversion of the rotation values reported by MoorDyn into something like an axis-angle representation. Given that you are always rotating around the x axis this would give you the current angle the object is rotated around the x axis. 

Hope this helps, and feel free to follow up with any additional questions or concerns if things still look suspicious to you. 
@AlexWKinley thanks for this explanation, I had been wondering about this a bit. 

@hamzaJayy I would add that while the function inputs are radians, the MoorDyn input files are still in degrees. This is a convention that comes from MoorDyn's coupling with OpenFAST. 
Thanks guys. 

I changed the input angles to radians. I kept the angular velocity to zero. I provided x-y-z intrinsic euler angles. 

Few things: 

1.  Yes, the input for MoorDynM_Step is in radians. And the ouput for MoorDynM_GetBodySate is also in radians. But the output file (.out), generated by input file (.txt), stores angles in degrees. That I assume that's FAST convention.
2. I understand now the gimbel lock and issues with that. But now as you can see in graph below: something strange happens when roll angle becomes negative. Is it also expected issues with euler rotation or is it something else?

![plot](https://github.com/FloatingArrayDesign/MoorDyn/assets/106804145/28c3b608-7be3-4d06-8763-3f8376e9d7b8)

Okay so after some research I have tracked down the source of the confusion. 

Firstly, so far as I can tell, the euler angles moordyn is returning are valid representations of the angles you are using.

The reason roll doesn't go negative is because that is apparently the behavior of Eigen function that moordyn uses to convert a rotation matrix to euler angles.
If you look at the documentation for the `eulerAngle()` function here:
https://libeigen.gitlab.io/docs/group__Geometry__Module.html#ga5a61d4a29ac24ace7043a6301c21e52c
it says
```
The returned angles are in non-canonical ranges [0:pi]x[-pi:pi]x[-pi:pi]. 
For canonical Tait-Bryan/proper Euler ranges, use canonicalEulerAngles.
```

It looks like that canonicalEulerAngles function is relatively new to Eigen, being added in [May of this year](https://chromium.googlesource.com/external/gitlab.com/libeigen/eigen/+/c18f94e3b017104284cd541e553472e62e85e526)
and so is not available in the version of Eigen included with moordyn.

Long story short, the reason the roll seems constrained to a weird range is because it's constrained to a weird range. 
I'm fairly confident that the rotation of the object is still behaving correctly, but I agree it's a strange result to get.

@RyanDavies19 If we wanted to switch to a canonicalEulerAngles approach, we can just grab that specific function from Eigen and repurpose it for our needs.

```c++
moordyn::vec3
canonicalEulerAngles(const moordyn::quaternion& quat, int a0, int a1, int a2)
{
	moordyn::mat3 coeff = quat.toRotationMatrix();
	moordyn::vec3 res{};
	using Index = int;
	using Scalar = moordyn::real;
	const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1;
	const Index i = a0;
	const Index j = (a0 + 1 + odd) % 3;
	const Index k = (a0 + 2 - odd) % 3;
	if (a0 == a2) {
		// Proper Euler angles (same first and last axis).
		// The i, j, k indices enable addressing the input matrix as the XYX
		// archetype matrix (see Graphics Gems IV), where e.g. coeff(k, i) means
		// third column, first row in the XYX archetype matrix:
		//  c2      s2s1              s2c1
		//  s2s3   -c2s1s3 + c1c3    -c2c1s3 - s1c3
		// -s2c3    c2s1c3 + c1s3     c2c1c3 - s1s3
		// Note: s2 is always positive.
		Scalar s2 = Eigen::numext::hypot(coeff(j, i), coeff(k, i));
		if (odd) {
			res[0] = atan2(coeff(j, i), coeff(k, i));
			// s2 is always positive, so res[1] will be within the canonical [0,
			// pi] range
			res[1] = atan2(s2, coeff(i, i));
		} else {
			// In the !odd case, signs of all three angles are flipped at the
			// very end. To keep the solution within the canonical range, we
			// flip the solution and make res[1] always negative here (since s2
			// is always positive, -atan2(s2, c2) will always be negative). The
			// final flip at the end due to !odd will thus make res[1] positive
			// and canonical. NB: in the general case, there are two correct
			// solutions, but only one is canonical. For proper Euler angles,
			// flipping from one solution to the other involves flipping the
			// sign of the second angle res[1] and adding/subtracting pi to the
			// first and third angles. The addition/subtraction of pi to the
			// first angle res[0] is handled here by flipping the signs of
			// arguments to atan2, while the calculation of the third angle does
			// not need special adjustment since it uses the adjusted res[0] as
			// the input and produces a correct result.
			res[0] = atan2(-coeff(j, i), -coeff(k, i));
			res[1] = -atan2(s2, coeff(i, i));
		}
		// With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first
		// two angles, we can compute their respective rotation, and apply its
		// inverse to M. Since the result must be a rotation around x, we have:
		//
		//  c2  s1.s2 c1.s2                   1  0   0
		//  0   c1    -s1       *    M    =   0  c3  s3
		//  -s2 s1.c2 c1.c2                   0 -s3  c3
		//
		//  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
		Scalar s1 = sin(res[0]);
		Scalar c1 = cos(res[0]);
		res[2] = atan2(c1 * coeff(j, k) - s1 * coeff(k, k),
		               c1 * coeff(j, j) - s1 * coeff(k, j));
	} else {
		// Tait-Bryan angles (all three axes are different; typically used for
		// yaw-pitch-roll calculations). The i, j, k indices enable addressing
		// the input matrix as the XYZ archetype matrix (see Graphics Gems IV),
		// where e.g. coeff(k, i) means third column, first row in the XYZ
		// archetype matrix:
		//  c2c3    s2s1c3 - c1s3     s2c1c3 + s1s3
		//  c2s3    s2s1s3 + c1c3     s2c1s3 - s1c3
		// -s2      c2s1              c2c1
		res[0] = atan2(coeff(j, k), coeff(k, k));
		Scalar c2 = Eigen::numext::hypot(coeff(i, i), coeff(i, j));
		// c2 is always positive, so the following atan2 will always return a
		// result in the correct canonical middle angle range [-pi/2, pi/2]
		res[1] = atan2(-coeff(i, k), c2);
		Scalar s1 = sin(res[0]);
		Scalar c1 = cos(res[0]);
		res[2] = atan2(s1 * coeff(k, i) - c1 * coeff(j, i),
		               c1 * coeff(j, j) - s1 * coeff(k, j));
	}
	if (!odd) {
		res = -res;
	}
	return res;
}
```
And then change Quat2Euler to be
```c++
vec3
Quat2Euler(const quaternion& q)
{
	return canonicalEulerAngles(q.toRotationMatrix(), 0, 1, 2);
}
```

And I believe that would alleviate some of the confusion. 
@AlexWKinley I will try that out and see how it performs in some of my test cases. Thanks for the suggestion. 

Fix patch:
diff --git a/source/Misc.hpp b/source/Misc.hpp
index 90b3ccf6..1fab4ea5 100644
--- a/source/Misc.hpp
+++ b/source/Misc.hpp
@@ -142,10 +142,93 @@ typedef ivec3 ivec;
 /// COmplex numbers
 typedef std::complex<real> complex;
 
+inline vec3 
+canonicalEulerAngles(const quaternion& quat, int a0, int a1, int a2)
+{
+	// From issue #163: https://github.com/FloatingArrayDesign/MoorDyn/issues/163
+	mat3 coeff = quat.toRotationMatrix();
+	vec3 res{};
+	using Index = int;
+	using Scalar = real;
+	const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1;
+	const Index i = a0;
+	const Index j = (a0 + 1 + odd) % 3;
+	const Index k = (a0 + 2 - odd) % 3;
+	if (a0 == a2) {
+		// Proper Euler angles (same first and last axis).
+		// The i, j, k indices enable addressing the input matrix as the XYX
+		// archetype matrix (see Graphics Gems IV), where e.g. coeff(k, i) means
+		// third column, first row in the XYX archetype matrix:
+		//  c2      s2s1              s2c1
+		//  s2s3   -c2s1s3 + c1c3    -c2c1s3 - s1c3
+		// -s2c3    c2s1c3 + c1s3     c2c1c3 - s1s3
+		// Note: s2 is always positive.
+		Scalar s2 = Eigen::numext::hypot(coeff(j, i), coeff(k, i));
+		if (odd) {
+			res[0] = atan2(coeff(j, i), coeff(k, i));
+			// s2 is always positive, so res[1] will be within the canonical [0,
+			// pi] range
+			res[1] = atan2(s2, coeff(i, i));
+		} else {
+			// In the !odd case, signs of all three angles are flipped at the
+			// very end. To keep the solution within the canonical range, we
+			// flip the solution and make res[1] always negative here (since s2
+			// is always positive, -atan2(s2, c2) will always be negative). The
+			// final flip at the end due to !odd will thus make res[1] positive
+			// and canonical. NB: in the general case, there are two correct
+			// solutions, but only one is canonical. For proper Euler angles,
+			// flipping from one solution to the other involves flipping the
+			// sign of the second angle res[1] and adding/subtracting pi to the
+			// first and third angles. The addition/subtraction of pi to the
+			// first angle res[0] is handled here by flipping the signs of
+			// arguments to atan2, while the calculation of the third angle does
+			// not need special adjustment since it uses the adjusted res[0] as
+			// the input and produces a correct result.
+			res[0] = atan2(-coeff(j, i), -coeff(k, i));
+			res[1] = -atan2(s2, coeff(i, i));
+		}
+		// With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first
+		// two angles, we can compute their respective rotation, and apply its
+		// inverse to M. Since the result must be a rotation around x, we have:
+		//
+		//  c2  s1.s2 c1.s2                   1  0   0
+		//  0   c1    -s1       *    M    =   0  c3  s3
+		//  -s2 s1.c2 c1.c2                   0 -s3  c3
+		//
+		//  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
+		Scalar s1 = sin(res[0]);
+		Scalar c1 = cos(res[0]);
+		res[2] = atan2(c1 * coeff(j, k) - s1 * coeff(k, k),
+		               c1 * coeff(j, j) - s1 * coeff(k, j));
+	} else {
+		// Tait-Bryan angles (all three axes are different; typically used for
+		// yaw-pitch-roll calculations). The i, j, k indices enable addressing
+		// the input matrix as the XYZ archetype matrix (see Graphics Gems IV),
+		// where e.g. coeff(k, i) means third column, first row in the XYZ
+		// archetype matrix:
+		//  c2c3    s2s1c3 - c1s3     s2c1c3 + s1s3
+		//  c2s3    s2s1s3 + c1c3     s2c1s3 - s1c3
+		// -s2      c2s1              c2c1
+		res[0] = atan2(coeff(j, k), coeff(k, k));
+		Scalar c2 = Eigen::numext::hypot(coeff(i, i), coeff(i, j));
+		// c2 is always positive, so the following atan2 will always return a
+		// result in the correct canonical middle angle range [-pi/2, pi/2]
+		res[1] = atan2(-coeff(i, k), c2);
+		Scalar s1 = sin(res[0]);
+		Scalar c1 = cos(res[0]);
+		res[2] = atan2(s1 * coeff(k, i) - c1 * coeff(j, i),
+		               c1 * coeff(j, j) - s1 * coeff(k, j));
+	}
+	if (!odd) {
+		res = -res;
+	}
+	return res;
+}
+
 inline vec3
 Quat2Euler(const quaternion& q)
 {
-	return q.toRotationMatrix().eulerAngles(0, 1, 2);
+	return canonicalEulerAngles(q, 0, 1, 2); // 0, 1, 2 correspond to axes leading to XYZ rotation 
 }
 
 inline quaternion
diff --git a/source/MoorDyn2.cpp b/source/MoorDyn2.cpp
index 9e801455..152ca6d3 100644
--- a/source/MoorDyn2.cpp
+++ b/source/MoorDyn2.cpp
@@ -1616,8 +1616,8 @@ moordyn::MoorDyn::readRodProps(string inputText)
 	obj->Can = atof(entries[4].c_str());
 	obj->CdEnd = atof(entries[5].c_str());
 	obj->CaEnd = atof(entries[6].c_str());
-	obj->Cdt = 0.0;
-	obj->Cat = 0.0;
+	obj->Cdt = 0.0; // No end effects from internal segments. CdEnd captures end effects.
+	obj->Cat = 0.0; // No end effects from internal segments. CaEnd captures end effects.
 
 	LOGDBG << "\t'" << obj->type << "'"
 	       << " - with id " << RodPropList.size() << endl


Test patch:
diff --git a/tests/bodies.cpp b/tests/bodies.cpp
index 39267ba7..c116a75d 100644
--- a/tests/bodies.cpp
+++ b/tests/bodies.cpp
@@ -384,7 +384,7 @@ rotatingBody(SeriesWriter* series_writer)
 	double angle = moordyn::rad2deg * 2 * acos(q.w());
 	double denom = (sqrt(1 - q.w() * q.w()));
 	moordyn::vec3 axis{ q.x() / denom, q.y() / denom, q.z() / denom };
-	if (!(axis.x() > 0.85 && axis.y() < 0.2 && axis.z() < 0.2)) {
+	if (!(abs(axis.x()) > 0.85 && abs(axis.y()) < 0.2 && abs(axis.z()) < 0.2)) { // if we are just checking axis direction then +/- does not matter
 		cerr << "The final rotation of the body in angle axis form should "
 		        "have an axis in the x direction, but axis is "
 		     << axis.transpose() << endl;
@@ -397,7 +397,7 @@ rotatingBody(SeriesWriter* series_writer)
 	while (angle < -180) {
 		angle += 360;
 	}
-	if (!(abs(angle - (-90)) < 10)) {
+	if (!(abs(angle - 90) < 10)) {
 		cerr << "The final rotation of the body in angle-axis form should "
 		        "have a angle near 90 degrees but angle is "
 		     << angle << endl;