Skip to content

Instantly share code, notes, and snippets.

@atteneder
Last active November 1, 2022 09:05
  • Star 7 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save atteneder/b6675c9a73860c00d795dcea7149e8d2 to your computer and use it in GitHub Desktop.
Matrix Decomposition (more robust than Matrix4x4.rotation/Matrix4x4.lossyScale)
// Copyright 2020-2022 Andreas Atteneder
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
using UnityEngine;
using static Unity.Mathematics.math;
using static Unity.Mathematics.quaternion;
using Unity.Mathematics;
public static class Matrix4x4Extension {
/// <summary>
/// Decomposes a 4x4 TRS matrix into separate transforms (translation * rotation * scale)
/// Matrix may not contain skew
/// </summary>
/// <param name="translation">Translation</param>
/// <param name="rotation">Rotation</param>
/// <param name="scale">Scale</param>
public static void Decompose(
this Matrix4x4 m,
out Vector3 translation,
out Quaternion rotation,
out Vector3 scale
)
{
translation = new Vector3( m.m03, m.m13, m.m23 );
var mRotScale = new float3x3(
m.m00,m.m01,m.m02,
m.m10,m.m11,m.m12,
m.m20,m.m21,m.m22
);
mRotScale.Decompose(out float4 mRotation, out float3 mScale);
rotation = new Quaternion(mRotation.x,mRotation.y,mRotation.z,mRotation.w);
scale = new Vector3(mScale.x,mScale.y,mScale.z);
}
/// <summary>
/// Decomposes a 4x4 TRS matrix into separate transforms (translation * rotation * scale)
/// Matrix may not contain skew
/// </summary>
/// <param name="translation">Translation</param>
/// <param name="rotation">Rotation</param>
/// <param name="scale">Scale</param>
public static void Decompose(
this float4x4 m,
out float3 translation,
out float4 rotation,
out float3 scale
)
{
var mRotScale = new float3x3(
m.c0.xyz,
m.c1.xyz,
m.c2.xyz
);
mRotScale.Decompose(out rotation, out scale);
translation = m.c3.xyz;
}
/// <summary>
/// Decomposes a 3x3 matrix into rotation and scale
/// </summary>
/// <param name="rotation">Rotation quaternion values</param>
/// <param name="scale">Scale</param>
public static void Decompose( this float3x3 m, out float4 rotation, out float3 scale ) {
var lenC0 = length(m.c0);
var lenC1 = length(m.c1);
var lenC2 = length(m.c2);
float3x3 rotationMatrix;
rotationMatrix.c0 = m.c0 / lenC0;
rotationMatrix.c1 = m.c1 / lenC1;
rotationMatrix.c2 = m.c2 / lenC2;
scale.x = lenC0;
scale.y = lenC1;
scale.z = lenC2;
if (rotationMatrix.IsNegative()) {
rotationMatrix *= -1f;
scale *= -1f;
}
// Inlined normalize(rotationMatrix)
rotationMatrix.c0 = math.normalize(rotationMatrix.c0);
rotationMatrix.c1 = math.normalize(rotationMatrix.c1);
rotationMatrix.c2 = math.normalize(rotationMatrix.c2);
rotation = new quaternion(rotationMatrix).value;
}
static float normalize(float3 input,out float3 output) {
float len = math.length(input);
output = input/len;
return len;
}
static void normalize(ref float3x3 m) {
m.c0 = math.normalize(m.c0);
m.c1 = math.normalize(m.c1);
m.c2 = math.normalize(m.c2);
}
static bool IsNegative(this float3x3 m) {
var cross = math.cross(m.c0,m.c1);
return math.dot(cross,m.c2)<0f;
}
}
@OneYoungMean
Copy link

thanks!

@weichx
Copy link

weichx commented Nov 15, 2021

Thanks! I think the normalize(float3x3) function doesn't work though, I think you need to pass by ref or the values get lost

static void normalize(ref float3x3 m) {
    m.c0 = math.normalize(m.c0);
    m.c1 = math.normalize(m.c1);
    m.c2 = math.normalize(m.c2);
}

I inlined some of the methods and fixed the return by reference

public static void Decompose(this float3x3 m, out float4 rotation, out float3 scale) {
    float lenC0 = math.length(m.c0);
    float lenC1 = math.length(m.c1);
    float lenC2 = math.length(m.c2);
    
    float3x3 rotationMatrix;
    rotationMatrix.c0 = m.c0 / lenC0;
    rotationMatrix.c1 = m.c1 / lenC1;
    rotationMatrix.c2 = m.c2 / lenC2;
    
    scale.x = lenC0;
    scale.y = lenC1;
    scale.z = lenC2;

    if (rotationMatrix.IsNegative()) {
        rotationMatrix *= -1f;
        scale *= -1f;
    }

    rotationMatrix.c0 = math.normalize(rotationMatrix.c0);
    rotationMatrix.c1 = math.normalize(rotationMatrix.c1);
    rotationMatrix.c2 = math.normalize(rotationMatrix.c2);
    
    rotation = new quaternion(rotationMatrix).value;
}

@atteneder
Copy link
Author

atteneder commented Jan 12, 2022

@weichx Thanks for the fix and feedback!

I finally found time to incorporated it both, although the inlined Decompose gave close to no performance gain here (wrote a performance test; macOS+IL2CPP; sub 1% improvement).

@weichx
Copy link

weichx commented Jan 17, 2022

Happy to help. Not surprised the inlining didn't help, I would expect the compiler to have done that in that case, especially if you're running it through burst / IL2CPP. I do suspect that running it via mono would show a small improvement. I prefer to have that kind logic in the same function, but its mostly a question of taste. Thanks for posting the original snippet, it helped me a lot.

@christianvoigt
Copy link

Thank you very much for the gist and your article!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment