From 1fd23afcfa64032e2fea1f48e4e9e5ada6b529a5 Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Wed, 29 May 2024 12:01:55 +0200
Subject: [PATCH] [refactoring-ode][angleTransformation] Implemented
scalar_last convention for angle transformations
---
functions/angleTransformation/angleToQuat.m | 50 +++++++-------
functions/angleTransformation/dcmToAngle.m | 42 ++++++------
functions/angleTransformation/dcmToQuat.m | 21 ------
functions/angleTransformation/quatToDcm.m | 72 ++++++++++++---------
4 files changed, 87 insertions(+), 98 deletions(-)
delete mode 100644 functions/angleTransformation/dcmToQuat.m
diff --git a/functions/angleTransformation/angleToQuat.m b/functions/angleTransformation/angleToQuat.m
index ff10576..c09bd2a 100644
--- a/functions/angleTransformation/angleToQuat.m
+++ b/functions/angleTransformation/angleToQuat.m
@@ -1,25 +1,28 @@
-function q = angleToQuat(yaw, pitch, roll)
-%{
-angleToQuat - This function converts Euler angles to quaternion.
-
-INPUTS:
- - yaw, double [1,1], rotation around z-axis;
- - pitch, double [1,1], rotation around y-axis;
- - roll, double [1,1], rotation around x-axis.
-
-OUTPUTS:
- - q, double [4,1], quaternion.
-
-CALLED FUNCTIONS: -
-
-REVISIONS:
--
-Copyright © 2021, Skyward Experimental Rocketry, AFD department
-All rights reserved
-
-SPDX-License-Identifier: GPL-3.0-or-later
-
-%}
+function q = angleToQuat(yaw, pitch, roll, convention)
+arguments
+ yaw (:, 1) double
+ pitch (:, 1) double
+ roll (:, 1) double
+ convention {mustBeMember(convention, {'scalar_first', 'scalar_last'})} = 'scalar_first'
+end
+% angleToQuat - This function converts Euler angles to quaternion.
+%
+% INPUTS:
+% - yaw, double [1,1], rotation around z-axis;
+% - pitch, double [1,1], rotation around y-axis;
+% - roll, double [1,1], rotation around x-axis.
+%
+% OUTPUTS:
+% - q, double [4,1], quaternion.
+%
+% CALLED FUNCTIONS: -
+%
+% REVISIONS:
+% -
+% Copyright © 2021, Skyward Experimental Rocketry, AFD department
+% All rights reserved
+%
+% SPDX-License-Identifier: GPL-3.0-or-later
dcm = zeros(3);
cr = cos(roll); sr = sin(roll);
@@ -37,4 +40,5 @@ dcm(3, 1) = cr.*sp.*cy + sr.*sy;
dcm(3, 2) = cr.*sp.*sy - sr.*cy;
dcm(3, 3) = cr.*cp;
-q = dcm2quat(dcm);
\ No newline at end of file
+q = dcm2quat(dcm);
+if strcmp(convention, 'scalar_last'), q = [q(:, 2:4), q(:, 1)]; end
\ No newline at end of file
diff --git a/functions/angleTransformation/dcmToAngle.m b/functions/angleTransformation/dcmToAngle.m
index 1448560..75963dc 100644
--- a/functions/angleTransformation/dcmToAngle.m
+++ b/functions/angleTransformation/dcmToAngle.m
@@ -1,26 +1,23 @@
function [yaw, pitch, roll] = dcmToAngle(dcm)
-%{
-dcmToAngle - This function converts direction cosine matrix (dcm) to Euler
- angles.
-
-INPUTS:
- - dcm, double [3,3], direction cosine matrix.
-
-OUTPUTS:
- - yaw, double [1,1], rotation around z-axis;
- - pitch, double [1,1], rotation around y-axis;
- - roll, double [1,1], rotation around x-axis.
-
-CALLED FUNCTIONS: -
-
-REVISIONS:
--
-Copyright © 2021, Skyward Experimental Rocketry, AFD department
-All rights reserved
-
-SPDX-License-Identifier: GPL-3.0-or-later
-
-%}
+% dcmToAngle - This function converts direction cosine matrix (dcm) to Euler
+% angles.
+%
+% INPUTS:
+% - dcm, double [3,3], direction cosine matrix.
+%
+% OUTPUTS:
+% - yaw, double [1,1], rotation around z-axis;
+% - pitch, double [1,1], rotation around y-axis;
+% - roll, double [1,1], rotation around x-axis.
+%
+% CALLED FUNCTIONS: -
+%
+% REVISIONS:
+% -
+% Copyright © 2021, Skyward Experimental Rocketry, AFD department
+% All rights reserved
+%
+% SPDX-License-Identifier: GPL-3.0-or-later
pitch = squeeze(-asin(dcm(1, 3, :)));
yaw = squeeze(atan2(dcm(1, 2, :), dcm(1, 1, :)));
@@ -35,5 +32,4 @@ if any(singularities)
pitch(singularities) = NaN;
yaw(singularities) = NaN;
roll(singularities) = NaN;
-
end
\ No newline at end of file
diff --git a/functions/angleTransformation/dcmToQuat.m b/functions/angleTransformation/dcmToQuat.m
deleted file mode 100644
index 57e33e9..0000000
--- a/functions/angleTransformation/dcmToQuat.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function q = dcmToQuat(dmc)
-%{
-dcmToQuat - This function convert direction cosine matrix (dcm) to
- quaternion.
-
-INPUTS:
- - dcm, double [3,3], direction cosine matrix.
-
-OUTPUTS:
- - q, double [4,1], quaternion.
-
-CALLED FUNCTIONS: -
-
-REVISIONS:
--
-Copyright © 2021, Skyward Experimental Rocketry, AFD department
-All rights reserved
-
-SPDX-License-Identifier: GPL-3.0-or-later
-
-%}
\ No newline at end of file
diff --git a/functions/angleTransformation/quatToDcm.m b/functions/angleTransformation/quatToDcm.m
index 003d777..824d562 100644
--- a/functions/angleTransformation/quatToDcm.m
+++ b/functions/angleTransformation/quatToDcm.m
@@ -1,34 +1,44 @@
-function dcm = quatToDcm(q)
-
-%{
-quatToDcm - This function converts quaternion to direction cosine
- martrix (dcm).
-
-INPUTS:
- - q, double [4,1], quaternion.
-
-OUTPUTS:
- - dcm, double [3,3], direction cosine matrix.
-
-CALLED FUNCTIONS: -
-
-REVISIONS:
--
-Copyright © 2021, Skyward Experimental Rocketry, AFD department
-All rights reserved
-
-SPDX-License-Identifier: GPL-3.0-or-later
-
-%}
+function dcm = quatToDcm(q, convention)
+arguments
+ q (1, :)
+ convention {mustBeMember(convention, {'scalar_first', 'scalar_last'})} = 'scalar_first'
+end
+% quatToDcm - This function converts quaternion to direction cosine
+% martrix (dcm).
+%
+% INPUTS:
+% - q, double [4,1], quaternion.
+%
+% OUTPUTS:
+% - dcm, double [3,3], direction cosine matrix.
+%
+% CALLED FUNCTIONS: -
+%
+% REVISIONS:
+% -
+% Copyright © 2021, Skyward Experimental Rocketry, AFD department
+% All rights reserved
+%
+% SPDX-License-Identifier: GPL-3.0-or-later
+
+
+switch convention
+ case 'scalar_first'
+ qw = q(1);
+ qx = q(2); qy = q(3); qz = q(4);
+ case 'scalar_last'
+ qw = q(4);
+ qx = q(1); qy = q(2); qz = q(3);
+end
dcm = zeros(3);
-dcm(1, 1) = q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2;
-dcm(1, 2) = 2*(q(2)*q(3) + q(1)*q(4));
-dcm(1, 3) = 2*(q(2)*q(4) - q(1)*q(3));
-dcm(2, 1) = 2*(q(2)*q(3) - q(1)*q(4));
-dcm(2, 2) = q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2;
-dcm(2, 3) = 2*(q(3)*q(4) + q(1)*q(2));
-dcm(3, 1) = 2*(q(2)*q(4) + q(1)*q(3));
-dcm(3, 2) = 2*(q(3)*q(4) - q(1)*q(2));
-dcm(3, 3) = q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2;
+dcm(1, 1) = qw^2 + qx^2 - qy^2 - qz^2;
+dcm(1, 2) = 2*(qx*qy + qw*qz);
+dcm(1, 3) = 2*(qx*qz - qw*qy);
+dcm(2, 1) = 2*(qx*qy - qw*qz);
+dcm(2, 2) = qw^2 - qx^2 + qy^2 - qz^2;
+dcm(2, 3) = 2*(qy*qz + qw*qx);
+dcm(3, 1) = 2*(qx*qz + qw*qy);
+dcm(3, 2) = 2*(qy*qz - qw*qx);
+dcm(3, 3) = qw^2 - qx^2 - qy^2 + qz^2;
\ No newline at end of file
--
GitLab