Routine to decompose an affine CATransform3D into to scale, rotate, translate components

CATransform3D+Decomposition.swift

//
// Copyright 2018 Warren Moore
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//

import QuartzCore
import simd

extension CATransform3D {
    func decomposeTRS() -> (float3, float3, float3) {
        let m0 = float3(Float(self.m11), Float(self.m12), Float(self.m13))
        let m1 = float3(Float(self.m21), Float(self.m22), Float(self.m23))
        let m2 = float3(Float(self.m31), Float(self.m32), Float(self.m33))
        let m3 = float3(Float(self.m41), Float(self.m42), Float(self.m43))
        
        let t = m3
        
        let sx = length(m0)
        let sy = length(m1)
        let sz = length(m2)
        let s = float3(sx, sy, sz)
        
        let rx = m0 / sx
        let ry = m1 / sy
        let rz = m2 / sz
        
        let pitch = atan2(ry.z, rz.z)
        let yaw = atan2(-rx.z, hypot(ry.z, rz.z))
        let roll = atan2(rx.y, rx.x)
        let r = float3(pitch, yaw, roll)
        
        return (t, r, s)
    }
}