Last active
July 4, 2022 08:50
-
-
Save Aerilius/0cbc46271c163746717902b36bea8fd4 to your computer and use it in GitHub Desktop.
Decomposing a transformation matrix into translation, scaling and Euler angles (using Trimble SketchUp Ruby API)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# Decompose a 4×4 augmented rotation matrix without shear into translation, scaling and rotation components. | |
# | |
# @param m [Array(16)] matrix | |
# @return [Array(3),Array(3),Array(3)] three arrays representing | |
# the translation vector, | |
# the scaling factor per axis and | |
# the Euler rotation angles in radians | |
def separate_translation_scaling_rotation(m) | |
m = m.clone | |
# Extract translation | |
translation = m.values_at(12, 13, 14) | |
# Extract scaling, considering uniform scale factor (last matrix element) | |
scaling = Array.new(3) | |
scaling[0] = m[15] * Math.sqrt(m[0]**2 + m[1]**2 + m[2]**2) | |
scaling[1] = m[15] * Math.sqrt(m[4]**2 + m[5]**2 + m[6]**2) | |
scaling[2] = m[15] * Math.sqrt(m[8]**2 + m[9]**2 + m[10]**2) | |
# Remove scaling to prepare for extraction of rotation | |
[0, 1, 2].each{ |i| m[i] /= scaling[0] } unless scaling[0] == 0.0 | |
[4, 5, 6].each{ |i| m[i] /= scaling[1] } unless scaling[1] == 0.0 | |
[8, 9,10].each{ |i| m[i] /= scaling[2] } unless scaling[2] == 0.0 | |
m[15] = 1.0 | |
# Verify orientation, if necessary invert it. | |
tmp_z_axis = Geom::Vector3d.new(m[0], m[1], m[2]).cross(Geom::Vector3d.new(m[4], m[5], m[6])) | |
if tmp_z_axis.dot( Geom::Vector3d.new(m[8], m[9], m[10]) ) < 0 | |
scaling[0] *= -1 | |
m[0] = -m[0] | |
m[1] = -m[1] | |
m[2] = -m[2] | |
end | |
# Extract rotation | |
# Source: Extracting Euler Angles from a Rotation Matrix, Mike Day, Insomniac Games | |
# http://www.insomniacgames.com/mike-day-extracting-euler-angles-from-a-rotation-matrix/ | |
theta1 = Math.atan2(m[6], m[10]) | |
c2 = Math.sqrt(m[0]**2 + m[1]**2) | |
theta2 = Math.atan2(-m[2], c2) | |
s1 = Math.sin(theta1) | |
c1 = Math.cos(theta1) | |
theta3 = Math.atan2(s1*m[8] - c1*m[4], c1*m[5] - s1*m[9]) | |
rotation = [-theta1, -theta2, -theta3] | |
return translation, scaling, rotation | |
end |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
There is a bug in line 14: it should be m[2]**2 not m[3]**2