# Copyright 2016 Pixar
#
# Licensed under the Apache License, Version 2.0 (the "Apache License")
# with the following modification; you may not use this file except in
# compliance with the Apache License and the following modification to it:
# Section 6. Trademarks. is deleted and replaced with:
#
# 6. Trademarks. This License does not grant permission to use the trade
#    names, trademarks, service marks, or product names of the Licensor
#    and its affiliates, except as required to comply with Section 4(c) of
#    the License and to reproduce the content of the NOTICE file.
#
# You may obtain a copy of the Apache License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the Apache License with the above modification is
# distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
# KIND, either express or implied. See the Apache License for the specific
# language governing permissions and limitations under the Apache License.
#

# Note: This is just pulled directly from USD and lightly modified
# https://github.com/PixarAnimationStudios/USD/blob/release/pxr/base/gf/testenv/testGfMatrix.py

from __future__ import division, print_function

__copyright__ = "Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved."
__license__ = """
NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
"""

import math
import sys
import unittest

##### Uncomment for better assert messages
# if 'unittest.util' in __import__('sys').modules:
#    # Show full diff in self.assertEqual.
#    __import__('sys').modules['unittest.util']._MAX_LENGTH = 999999999


def setUpModule():
    # pre-load USD before anything else to prevent crash
    # from weird carb conflict
    # FIXME ^^^
    try:
        from . import update_path_and_load_usd

        update_path_and_load_usd()
    except ImportError:
        # not needed in Kit
        pass


try:
    import omni.kit.test

    TestClass = omni.kit.test.AsyncTestCase
except ImportError:
    TestClass = unittest.TestCase


try:
    import numpy

    hasNumpy = True
except ImportError:
    print("numpy not available, skipping buffer protocol tests")
    hasNumpy = False


def makeValue(Value, vals):
    if Value == float:
        return Value(vals[0])
    else:
        v = Value()
        for i in range(v.dimension):
            v[i] = vals[i]
    return v


class TestGfMatrix(TestClass):
    def test_Basic(self):
        from usdrt import Gf

        Matrices = [  # (Gf.Matrix2d, Gf.Vec2d),
            # (Gf.Matrix2f, Gf.Vec2f),
            (Gf.Matrix3d, Gf.Vec3d),
            (Gf.Matrix3f, Gf.Vec3f),
            (Gf.Matrix4d, Gf.Vec4d),
            (Gf.Matrix4f, Gf.Vec4f),
        ]

        for (Matrix, Vec) in Matrices:
            # constructors
            self.assertIsInstance(Matrix(), Matrix)
            self.assertIsInstance(Matrix(1), Matrix)
            self.assertIsInstance(Matrix(Vec()), Matrix)

            # python default constructor produces identity.
            self.assertEqual(Matrix(), Matrix(1))

            if hasNumpy:
                # Verify population of numpy arrays.
                emptyNumpyArray = numpy.empty((1, Matrix.dimension[0], Matrix.dimension[1]), dtype="float32")
                emptyNumpyArray[0] = Matrix(1)

                if Matrix.dimension == (2, 2):
                    self.assertIsInstance(Matrix(1, 2, 3, 4), Matrix)
                    self.assertEqual(Matrix().Set(1, 2, 3, 4), Matrix(1, 2, 3, 4))
                    array = numpy.array(Matrix(1, 2, 3, 4))
                    self.assertEqual(array.shape, (2, 2))
                    # XXX: Currently cannot round-trip Gf.Matrix*f through
                    #      numpy.array
                    if Matrix != Gf.Matrix2f:
                        self.assertEqual(Matrix(array), Matrix(1, 2, 3, 4))
                elif Matrix.dimension == (3, 3):
                    self.assertIsInstance(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9), Matrix)
                    self.assertEqual(Matrix().Set(1, 2, 3, 4, 5, 6, 7, 8, 9), Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
                    array = numpy.array(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
                    self.assertEqual(array.shape, (3, 3))
                    # XXX: Currently cannot round-trip Gf.Matrix*f through
                    #      numpy.array
                    if Matrix != Gf.Matrix3f:
                        self.assertEqual(Matrix(array), Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
                elif Matrix.dimension == (4, 4):
                    self.assertIsInstance(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16), Matrix)
                    self.assertEqual(
                        Matrix().Set(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16),
                        Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16),
                    )
                    array = numpy.array(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16))
                    self.assertEqual(array.shape, (4, 4))
                    # XXX: Currently cannot round-trip Gf.Matrix*f through
                    #      numpy.array
                    if Matrix != Gf.Matrix4f:
                        self.assertEqual(Matrix(array), Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16))
                else:
                    self.fail()

            self.assertEqual(Matrix().SetIdentity(), Matrix(1))

            self.assertEqual(Matrix().SetZero(), Matrix(0))

            self.assertEqual(Matrix().SetDiagonal(0), Matrix().SetZero())
            self.assertEqual(Matrix().SetDiagonal(1), Matrix().SetIdentity())

    def test_Comparisons(self):
        from usdrt import Gf

        Matrices = [(Gf.Matrix3d, Gf.Matrix3f), (Gf.Matrix4d, Gf.Matrix4f)]  # (Gf.Matrix2d, Gf.Matrix2f),

        for (Matrix, Matrixf) in Matrices:
            # Test comparison of Matrix and Matrixf
            #
            size = Matrix.dimension[0] * Matrix.dimension[1]
            contents = list(range(1, size + 1))
            md = Matrix(*contents)
            mf = Matrixf(*contents)
            self.assertEqual(md, mf)

            contents.reverse()
            md.Set(*contents)
            mf.Set(*contents)
            self.assertEqual(md, mf)

            # Convert to double precision floating point values
            contents = [1.0 / x for x in contents]
            mf.Set(*contents)
            md.Set(*contents)
            # These should *NOT* be equal due to roundoff errors in the floats.
            self.assertNotEqual(md, mf)

    def test_Other(self):
        from usdrt import Gf

        Matrices = [  # (Gf.Matrix2d, Gf.Vec2d),
            # (Gf.Matrix2f, Gf.Vec2f),
            (Gf.Matrix3d, Gf.Vec3d),
            (Gf.Matrix3f, Gf.Vec3f),
            (Gf.Matrix4d, Gf.Vec4d),
            (Gf.Matrix4f, Gf.Vec4f),
        ]

        for (Matrix, Vec) in Matrices:
            v = Vec()
            for i in range(v.dimension):
                v[i] = i
            m1 = Matrix().SetDiagonal(v)
            m2 = Matrix(0)
            for i in range(m2.dimension[0]):
                m2[i, i] = i
            self.assertEqual(m1, m2)

            v = Vec()
            for i in range(v.dimension):
                v[i] = 10
            self.assertEqual(Matrix().SetDiagonal(v), Matrix().SetDiagonal(10))

            self.assertEqual(type(Matrix()[0]), Vec)

            m = Matrix()
            m[0] = makeValue(Vec, (3, 1, 4, 1))
            self.assertEqual(m[0], makeValue(Vec, (3, 1, 4, 1)))

            m = Matrix()
            m[-1] = makeValue(Vec, (3, 1, 4, 1))
            self.assertEqual(m[-1], makeValue(Vec, (3, 1, 4, 1)))

            m = Matrix()
            m[0, 0] = 1
            m[1, 0] = 2
            m[0, 1] = 3
            m[1, 1] = 4
            self.assertTrue(m[0, 0] == 1 and m[1, 0] == 2 and m[0, 1] == 3 and m[1, 1] == 4)

            m = Matrix()
            m[-1, -1] = 1
            m[-2, -1] = 2
            m[-1, -2] = 3
            m[-2, -2] = 4
            self.assertTrue(m[-1, -1] == 1 and m[-2, -1] == 2 and m[-1, -2] == 3 and m[-2, -2] == 4)

            m = Matrix()
            for i in range(m.dimension[0]):
                for j in range(m.dimension[1]):
                    m[i, j] = i * m.dimension[1] + j
            m = m.GetTranspose()
            for i in range(m.dimension[0]):
                for j in range(m.dimension[1]):
                    self.assertEqual(m[j, i], i * m.dimension[1] + j)

            self.assertEqual(Matrix(1).GetInverse(), Matrix(1))
            self.assertEqual(Matrix(4).GetInverse() * Matrix(4), Matrix(1))
            # nv edit - intentionally diverge from pixar's implementation
            # GetInverse for zero matrix returns zero matrix instead of max float scale matrix
            # "so that multiplying by this is less likely to be catastrophic."
            self.assertEqual(Matrix(0).GetInverse(), Matrix(0))

            self.assertEqual(Matrix(3).GetDeterminant(), 3 ** Matrix.dimension[0])

            self.assertEqual(len(Matrix()), Matrix.dimension[0])

            # Test GetRow, GetRow3, GetColumn
            m = Matrix(1)
            for i in range(m.dimension[0]):
                for j in range(m.dimension[1]):
                    m[i, j] = i * m.dimension[1] + j
            for i in range(m.dimension[0]):
                j0 = i * m.dimension[1]
                self.assertEqual(m.GetRow(i), makeValue(Vec, tuple(range(j0, j0 + m.dimension[1]))))
                if Matrix == Gf.Matrix4d:
                    self.assertEqual(m.GetRow3(i), Gf.Vec3d(j0, j0 + 1, j0 + 2))

            for j in range(m.dimension[1]):
                self.assertEqual(
                    m.GetColumn(j), makeValue(Vec, tuple(j + x * m.dimension[0] for x in range(m.dimension[0])))
                )

            # Test SetRow, SetRow3, SetColumn
            m = Matrix(1)
            for i in range(m.dimension[0]):
                j0 = i * m.dimension[1]
                v = makeValue(Vec, tuple(range(j0, j0 + m.dimension[1])))
                m.SetRow(i, v)
                self.assertEqual(v, m.GetRow(i))

            m = Matrix(1)
            if Matrix == Gf.Matrix4d:
                for i in range(m.dimension[0]):
                    j0 = i * m.dimension[1]
                    v = Gf.Vec3d(j0, j0 + 1, j0 + 2)
                    m.SetRow3(i, v)
                    self.assertEqual(v, m.GetRow3(i))

            m = Matrix(1)
            for j in range(m.dimension[0]):
                v = makeValue(Vec, tuple(j + x * m.dimension[0] for x in range(m.dimension[0])))
                m.SetColumn(i, v)
                self.assertEqual(v, m.GetColumn(i))

            m = Matrix(4)
            m *= Matrix(1.0 / 4)
            self.assertEqual(m, Matrix(1))
            m = Matrix(4)
            self.assertEqual(m * Matrix(1.0 / 4), Matrix(1))

            self.assertEqual(Matrix(4) * 2, Matrix(8))
            self.assertEqual(2 * Matrix(4), Matrix(8))
            m = Matrix(4)
            m *= 2
            self.assertEqual(m, Matrix(8))

            m = Matrix(3)
            m += Matrix(2)
            self.assertEqual(m, Matrix(5))
            m = Matrix(3)
            m -= Matrix(2)
            self.assertEqual(m, Matrix(1))

            self.assertEqual(Matrix(2) + Matrix(3), Matrix(5))
            self.assertEqual(Matrix(4) - Matrix(4), Matrix(0))

            self.assertEqual(-Matrix(-1), Matrix(1))

            self.assertEqual(Matrix(3) / Matrix(2), Matrix(3) * Matrix(2).GetInverse())

            self.assertEqual(Matrix(2) * makeValue(Vec, (3, 1, 4, 1)), makeValue(Vec, (6, 2, 8, 2)))
            self.assertEqual(makeValue(Vec, (3, 1, 4, 1)) * Matrix(2), makeValue(Vec, (6, 2, 8, 2)))

            Vecf = {Gf.Vec2d: Gf.Vec2f, Gf.Vec3d: Gf.Vec3f, Gf.Vec4d: Gf.Vec4f}.get(Vec)
            if Vecf is not None:
                self.assertEqual(Matrix(2) * makeValue(Vecf, (3, 1, 4, 1)), makeValue(Vecf, (6, 2, 8, 2)))
                self.assertEqual(makeValue(Vecf, (3, 1, 4, 1)) * Matrix(2), makeValue(Vecf, (6, 2, 8, 2)))

            self.assertTrue(2 in Matrix(2) and not 4 in Matrix(2))

            m = Matrix(1)
            try:
                m[m.dimension[0] + 1] = Vec()
            except:
                pass
            else:
                self.fail()

            m = Matrix(1)
            try:
                m[m.dimension[0] + 1, m.dimension[1] + 1] = 10
            except:
                pass
            else:
                self.fail()

            m = Matrix(1)
            try:
                m[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] = 3
                self.fail()
            except:
                pass

            try:
                x = m[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
                self.fail()
            except:
                pass

            m = Matrix(1)
            try:
                m["foo"] = 3
            except:
                pass
            else:
                self.fail()

            self.assertEqual(m, eval(repr(m)))

            self.assertTrue(len(str(Matrix())))

    def test_Matrix3Transforms(self):
        from usdrt import Gf

        # TODO - Gf.Rotation not supported,
        # so this test is currently a noop
        Matrices = [  # (Gf.Matrix3d, Gf.Vec3d, Gf.Quatd),
            # (Gf.Matrix3f, Gf.Vec3f, Gf.Quatf)
        ]

        for (Matrix, Vec, Quat) in Matrices:

            def _VerifyOrthonormVecs(mat):
                v0 = Vec(mat[0][0], mat[0][1], mat[0][2])
                v1 = Vec(mat[1][0], mat[1][1], mat[1][2])
                v2 = Vec(mat[2][0], mat[2][1], mat[2][2])
                self.assertTrue(
                    Gf.IsClose(Gf.Dot(v0, v1), 0, 0.0001)
                    and Gf.IsClose(Gf.Dot(v0, v2), 0, 0.0001)
                    and Gf.IsClose(Gf.Dot(v1, v2), 0, 0.0001)
                )

            m = Matrix()
            m.SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30))
            m2 = Matrix(m)
            m_o = m.GetOrthonormalized()
            self.assertEqual(m_o, m2)
            m.Orthonormalize()
            self.assertEqual(m, m2)
            m = Matrix(3)
            m_o = m.GetOrthonormalized()
            # GetOrthonormalized() should not mutate m
            self.assertNotEqual(m, m_o)
            self.assertEqual(m_o, Matrix(1))
            m.Orthonormalize()
            self.assertEqual(m, Matrix(1))

            m = Matrix(1, 0, 0, 1, 0, 0, 1, 0, 0)
            # should print a warning
            print("expect a warning about failed convergence in OrthogonalizeBasis:")
            m.Orthonormalize()

            m = Matrix(1, 0, 0, 1, 0, 0.0001, 0, 1, 0)
            m_o = m.GetOrthonormalized()
            _VerifyOrthonormVecs(m_o)
            m.Orthonormalize()
            _VerifyOrthonormVecs(m)

            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)).ExtractRotation()
            r2 = Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1), 60)).ExtractRotation()
            r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 60)
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1), 90)).ExtractRotation()
            r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 90)
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)).ExtractRotation()
            r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            # Setting rotation using a quaternion should give the same results
            # as setting a GfRotation.
            rot = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
            quat = Quat(rot.GetQuaternion().GetReal(), Vec(rot.GetQuaternion().GetImaginary()))
            r = Matrix().SetRotate(rot).ExtractRotation()
            r2 = Matrix().SetRotate(quat).ExtractRotation()
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            self.assertEqual(Matrix().SetScale(10), Matrix(10))
            m = Matrix().SetScale(Vec(1, 2, 3))
            self.assertTrue(m[0, 0] == 1 and m[1, 1] == 2 and m[2, 2] == 3)

            # Initializing with GfRotation should give same results as SetRotate.
            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)).ExtractRotation()
            r2 = Matrix(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)).ExtractRotation()
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            # Initializing with a quaternion should give same results as SetRotate.
            rot = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
            quat = Quat(rot.GetQuaternion().GetReal(), Vec(rot.GetQuaternion().GetImaginary()))
            r = Matrix().SetRotate(quat).ExtractRotation()
            r2 = Matrix(quat).ExtractRotation()
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and Gf.IsClose(r.angle, r2.angle, 0.0001))

            self.assertEqual(Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), 1.0)
            self.assertEqual(Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), -1.0)
            self.assertEqual(Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0).GetHandedness(), 0.0)
            self.assertTrue(Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).IsRightHanded())
            self.assertTrue(Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).IsLeftHanded())

            # Test that this does not generate a nan in the angle (bug #12744)
            mx = Gf.Matrix3d(
                0.999999999982236,
                -5.00662622471027e-06,
                2.07636574601397e-06,
                5.00666175191934e-06,
                1.0000000000332,
                -2.19113616402155e-07,
                -2.07635686422463e-06,
                2.19131379884019e-07,
                1,
            )
            r = mx.ExtractRotation()
            # math.isnan won't be available until python 2.6
            if sys.version_info[0] >= 2 and sys.version_info[1] >= 6:
                self.assertFalse(math.isnan(r.angle))
            else:
                # If this fails, then r.angle is Nan. Works on linux, may not be portable.
                self.assertEqual(r.angle, r.angle)

    def test_Matrix4Transforms(self):
        from usdrt import Gf

        # nv edit - TODO support Quatd and Quatf
        Matrices = [
            (Gf.Matrix4d, Gf.Vec4d, Gf.Matrix3d, Gf.Vec3d),  # , Gf.Quatd),
            (Gf.Matrix4f, Gf.Vec4f, Gf.Matrix3f, Gf.Vec3f),
        ]  # , Gf.Quatf)]

        # for (Matrix, Vec, Matrix3, Vec3, Quat) in Matrices:
        for (Matrix, Vec, Matrix3, Vec3) in Matrices:

            def _VerifyOrthonormVecs(mat):
                v0 = Vec3(mat[0][0], mat[0][1], mat[0][2])
                v1 = Vec3(mat[1][0], mat[1][1], mat[1][2])
                v2 = Vec3(mat[2][0], mat[2][1], mat[2][2])
                self.assertTrue(
                    Gf.IsClose(Gf.Dot(v0, v1), 0, 0.0001)
                    and Gf.IsClose(Gf.Dot(v0, v2), 0, 0.0001)
                    and Gf.IsClose(Gf.Dot(v1, v2), 0, 0.0001)
                )

            m = Matrix()
            m.SetLookAt(Vec3(1, 0, 0), Vec3(0, 0, 1), Vec3(0, 1, 0))
            # nv edit - break this across multiple statements
            self.assertTrue(Gf.IsClose(m[0], Vec(-0.707107, 0, 0.707107, 0), 0.0001))
            self.assertTrue(Gf.IsClose(m[1], Vec(0, 1, 0, 0), 0.0001))
            self.assertTrue(Gf.IsClose(m[2], Vec(-0.707107, 0, -0.707107, 0), 0.0001))
            self.assertTrue(Gf.IsClose(m[3], Vec(0.707107, 0, -0.707107, 1), 0.0001))

            # Transform
            v = Gf.Vec3f(1, 1, 1)
            v2 = Matrix(3).Transform(v)
            self.assertEqual(v2, Gf.Vec3f(1, 1, 1))
            self.assertEqual(type(v2), Gf.Vec3f)

            v = Gf.Vec3d(1, 1, 1)
            v2 = Matrix(3).Transform(v)
            self.assertEqual(v2, Gf.Vec3d(1, 1, 1))
            self.assertEqual(type(v2), Gf.Vec3d)

            # TransformDir
            v = Gf.Vec3f(1, 1, 1)
            v2 = Matrix(3).TransformDir(v)
            self.assertEqual(v2, Gf.Vec3f(3, 3, 3))
            self.assertEqual(type(v2), Gf.Vec3f)

            v = Gf.Vec3d(1, 1, 1)
            v2 = Matrix(3).TransformDir(v)
            self.assertEqual(v2, Gf.Vec3d(3, 3, 3))
            self.assertEqual(type(v2), Gf.Vec3d)

            # TransformAffine
            v = Gf.Vec3f(1, 1, 1)
            # nv edit - no implict conversion from tuple yet
            v2 = Matrix(3).SetTranslateOnly(Vec3(1, 2, 3)).TransformAffine(v)
            self.assertEqual(v2, Gf.Vec3f(4, 5, 6))
            self.assertEqual(type(v2), Gf.Vec3f)

            v = Gf.Vec3d(1, 1, 1)
            # nv edit - no implict conversion from tuple yet
            v2 = Matrix(3).SetTranslateOnly(Vec3(1, 2, 3)).TransformAffine(v)
            self.assertEqual(v2, Gf.Vec3d(4, 5, 6))
            self.assertEqual(type(v2), Gf.Vec3d)

            # Constructor, SetRotate, and SetRotateOnly w/GfQuaternion
            """ nv edit Gf.Rotation not supported
            m = Matrix()
            r = Gf.Rotation(Gf.Vec3d(1,0,0), 30)
            quat = r.GetQuaternion()
            m.SetRotate(Quat(quat.GetReal(), Vec3(quat.GetImaginary())))
            m2 = Matrix(r, Vec3(0,0,0))
            m_o = m.GetOrthonormalized()
            self.assertEqual(m_o, m2)
            m.Orthonormalize()
            self.assertEqual(m, m2)
            m3 = Matrix(1)
            m3.SetRotateOnly(Quat(quat.GetReal(), Vec3(quat.GetImaginary())))
            self.assertEqual(m2, m3)

            # Constructor, SetRotate, and SetRotateOnly w/GfRotation
            m = Matrix()
            r = Gf.Rotation(Gf.Vec3d(1,0,0), 30)
            m.SetRotate(r)
            m2 = Matrix(r, Vec3(0,0,0))
            m_o = m.GetOrthonormalized()
            self.assertEqual(m_o, m2)
            m.Orthonormalize()
            self.assertEqual(m, m2)
            m3 = Matrix(1)
            m3.SetRotateOnly(r)
            self.assertEqual(m2, m3)

            # Constructor, SetRotate, and SetRotateOnly w/mx
            m3d = Matrix3()
            m3d.SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), 30))
            m = Matrix(m3d, Vec3(0,0,0))
            m2 = Matrix()
            m2 = m2.SetRotate(m3d)
            m3 = Matrix()
            m3 = m2.SetRotateOnly(m3d)
            self.assertEqual(m, m2)
            self.assertEqual(m2, m3)
            """

            m = Matrix().SetTranslate(Vec3(12, 13, 14)) * Matrix(3)
            m.Orthonormalize()
            t = Matrix().SetTranslate(m.ExtractTranslation())
            mnot = m * t.GetInverse()
            self.assertEqual(mnot, Matrix(1))

            m = Matrix()
            m.SetTranslate(Vec3(1, 2, 3))
            m2 = Matrix(m)
            m3 = Matrix(1)
            m3.SetTranslateOnly(Vec3(1, 2, 3))
            m_o = m.GetOrthonormalized()
            self.assertEqual(m_o, m2)
            self.assertEqual(m_o, m3)
            m.Orthonormalize()
            self.assertEqual(m, m2)
            self.assertEqual(m, m3)

            v = Vec3(11, 22, 33)
            m = Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16).SetTranslateOnly(v)
            self.assertEqual(m.ExtractTranslation(), v)

            # Initializing with GfRotation should give same results as SetRotate
            # and SetTransform
            """ nv edit TODO
            r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0),30)).ExtractRotation()
            r2 = Matrix(Gf.Rotation(Gf.Vec3d(1,0,0),30), Vec3(1,2,3)).ExtractRotation()
            r3 = Matrix().SetTransform(Gf.Rotation(Gf.Vec3d(1,0,0),30), Vec3(1,2,3)).ExtractRotation()
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r.angle, r2.angle, 0.0001))
            self.assertTrue(Gf.IsClose(r3.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r3.angle, r2.angle, 0.0001))

            # Initializing with GfRotation should give same results as
            # SetRotate(quaternion) and SetTransform
            rot = Gf.Rotation(Gf.Vec3d(1,0,0),30)
            quat = Quat(rot.GetQuaternion().GetReal(),
                        Vec3(rot.GetQuaternion().GetImaginary()))
            r = Matrix().SetRotate(quat).ExtractRotation()
            r2 = Matrix(rot, Vec3(1,2,3)).ExtractRotation()
            r3 = Matrix().SetTransform(rot, Vec3(1,2,3)).ExtractRotation()
            self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r.angle, r2.angle, 0.0001))
            self.assertTrue(Gf.IsClose(r3.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r3.angle, r2.angle, 0.0001))

            # Same test w/mx instead of GfRotation
            mx3d = Matrix3(Gf.Rotation(Gf.Vec3d(1,0,0),30))
            r4 = Matrix().SetTransform(mx3d, Vec3(1,2,3)).ExtractRotation()
            r5 = Matrix(mx3d, Vec3(1,2,3)).ExtractRotation()
            self.assertTrue(Gf.IsClose(r4.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r4.angle, r2.angle, 0.0001))
            self.assertTrue(Gf.IsClose(r4.axis, r5.axis, 0.0001) and \
                Gf.IsClose(r4.angle, r5.angle, 0.0001))

            # ExtractQuat() and ExtractRotation() should yield
            # equivalent rotations.
            m = Matrix(mx3d, Vec3(1,2,3))
            r1 = m.ExtractRotation()
            r2 = Gf.Rotation(m.ExtractRotationQuat())
            self.assertTrue(Gf.IsClose(r1.axis, r2.axis, 0.0001) and \
                Gf.IsClose(r2.angle, r2.angle, 0.0001))

            m4 = Matrix(mx3d, Vec3(1,2,3)).ExtractRotationMatrix()
            self.assertEqual(m4, mx3d)
            """

            # Initializing with GfMatrix3d
            # nv edit - dumb, not supported
            # m = Matrix(1,2,3,0,4,5,6,0,7,8,9,0,10,11,12,1)
            # m2 = Matrix(Matrix3(1,2,3,4,5,6,7,8,9), Vec3(10, 11, 12))
            # assert(m == m2)

            m = Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1)
            # should print a warning - nv edit - our implementation doesn't
            # print("expect a warning about failed convergence in OrthogonalizeBasis:")
            m.Orthonormalize()

            m = Matrix(1, 0, 0, 0, 1, 0, 0.0001, 0, 0, 1, 0, 0, 0, 0, 0, 1)
            m_o = m.GetOrthonormalized()
            _VerifyOrthonormVecs(m_o)
            m.Orthonormalize()
            _VerifyOrthonormVecs(m)

            m = Matrix()
            m[3, 0] = 4
            m[3, 1] = 5
            m[3, 2] = 6
            self.assertEqual(m.ExtractTranslation(), Vec3(4, 5, 6))

            self.assertEqual(Matrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1).GetHandedness(), 1.0)
            self.assertEqual(Matrix(-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1).GetHandedness(), -1.0)
            self.assertEqual(Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).GetHandedness(), 0.0)
            self.assertTrue(Matrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1).IsRightHanded())
            self.assertTrue(Matrix(-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1).IsLeftHanded())

            # RemoveScaleShear
            """ nv edit Gf.Rotation not supported
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), 45))
            m = Matrix().SetScale(Vec3(3,4,2)) * m
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), -30)) * m
            r = m.RemoveScaleShear()
            ro = r
            ro.Orthonormalize()
            self.assertEqual(ro, r)
            shear = Matrix(1,0,1,0, 0,1,0,0, 0,0,1,0, 0,0,0,1)
            r = shear.RemoveScaleShear()
            ro = r
            ro.Orthonormalize()
            self.assertEqual(ro, r)
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), 45))
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), -30)) * m
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,1,0), 59)) * m
            r = m.RemoveScaleShear()
            maxEltErr = 0
            for i in range(4):
                for j in range(4):
                    maxEltErr = max(maxEltErr, abs(r[i][j] - m[i][j]))
            self.assertTrue(Gf.IsClose(maxEltErr, 0.0, 1e-5))

            # IsClose
            self.assertFalse(Gf.IsClose(Matrix(1), Matrix(1.0001), 1e-5))
            self.assertTrue(Gf.IsClose(Matrix(1), Matrix(1.000001), 1e-5))
            """

    def test_Matrix4Factoring(self):
        from usdrt import Gf

        """ nv edit Gf.Rotation not supported
        Matrices = [(Gf.Matrix4d, Gf.Vec3d),
                    (Gf.Matrix4f, Gf.Vec3f)]
        for (Matrix, Vec3) in Matrices:
            def testFactor(m, expectSuccess, eps=None):
                factor = lambda m : m.Factor()
                if eps is not None:
                    factor = lambda m : m.Factor(eps)
                (success, scaleOrientation, scale, rotation, \
                 translation, projection) = factor(m)
                self.assertEqual(success, expectSuccess)
                factorProduct = scaleOrientation * \
                                Matrix().SetScale(scale) * \
                                scaleOrientation.GetInverse() * \
                                rotation * \
                                Matrix().SetTranslate(translation) * \
                                projection
                maxEltErr = 0
                for i in range(4):
                    for j in range(4):
                        maxEltErr = max(maxEltErr, abs(factorProduct[i][j] - m[i][j]))
                self.assertTrue(Gf.IsClose(maxEltErr, 0.0, 1e-5), maxEltErr)

            # A rotate
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), 45))
            testFactor(m,True)

            # A couple of rotates
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), -45))
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), 45)) * m
            testFactor(m,True)

            # A scale
            m = Matrix().SetScale(Vec3(3,1,4))
            testFactor(m,True)

            # A scale in a rotated space
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), 45))
            m = Matrix().SetScale(Vec3(3,4,2)) * m
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), -45)) * m
            testFactor(m,True)

            # A nearly degenerate scale
            if Matrix == Gf.Matrix4d:
                eps = 1e-10
            elif Matrix == Gf.Matrix4f:
                eps = 1e-5

            m = Matrix().SetScale((eps*2, 1, 1))
            testFactor(m,True)
            # Test with epsilon.
            m = Matrix().SetScale((eps*2, 1, 1))
            testFactor(m,False,eps*3)

            # A singular (1) scale in a rotated space
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,1,1), Gf.Vec3d(1,0,0) ))
            m = m * Matrix().SetScale(Vec3(0,1,1))
            m = m * Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), Gf.Vec3d(1,1,1) ))
            testFactor(m,False)

            # A singular (2) scale in a rotated space
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,1,1), Gf.Vec3d(1,0,0) ))
            m = m * Matrix().SetScale(Vec3(0,0,1))
            m = m * Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1,0,0), Gf.Vec3d(1,1,1) ))
            testFactor(m,False)

            # A scale in a sheared space
            shear = Matrix(1)
            shear.SetRow3(0, Vec3(1, 1, 1).GetNormalized())
            m = shear.GetInverse() * Matrix().SetScale(Vec3(2,3,4)) * shear
            testFactor(m,True)

            # A singular (1) scale in a sheared space
            shear = Matrix(1)
            shear.SetRow3(0, Vec3(1, 1, 1).GetNormalized())
            m = shear.GetInverse() * Matrix().SetScale(Vec3(2,0,4)) * shear
            testFactor(m,False)

            # A singular (2) scale in a sheared space
            shear = Matrix(1)
            shear.SetRow3(0, Vec3(1, 1, 1).GetNormalized())
            m = shear.GetInverse() * Matrix().SetScale(Vec3(0,3,0)) * shear
            testFactor(m,False)

            # A scale and a rotate
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), Gf.Vec3d(1,1,-1) ))
            m = Matrix().SetScale(Vec3(1,2,3)) * m
            testFactor(m,True)

            # A singular (1) scale and a rotate
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), Gf.Vec3d(1,1,-1) ))
            m = Matrix().SetScale(Vec3(0,2,3)) * m
            testFactor(m,False)

            # A singular (2) scale and a rotate
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), Gf.Vec3d(1,1,-1) ))
            m = Matrix().SetScale(Vec3(0,0,3)) * m
            testFactor(m,False)

            # A singular scale (1), rotate, translate
            m = Matrix().SetTranslate(Vec3(3,1,4))
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), Gf.Vec3d(1,1,-1) )) * m
            m = Matrix().SetScale(Vec3(0,2,3)) * m
            testFactor(m,False)

            # A translate, rotate, singular scale (1), translate
            m = Matrix().SetTranslate(Vec3(3,1,4))
            m = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(0,0,1), Gf.Vec3d(1,1,-1) )) * m
            m = Matrix().SetScale(Vec3(0,2,3)) * m
            m = Matrix().SetTranslate(Vec3(-10,-20,-30)) * m
            testFactor(m,False)
            """

    def test_Matrix4Determinant(self):
        from usdrt import Gf

        Matrices = [Gf.Matrix4d, Gf.Matrix4f]
        for Matrix in Matrices:
            # Test GetDeterminant and GetInverse on Matrix4
            def AssertDeterminant(m, det):
                # Unfortunately, we don't have an override of Gf.IsClose
                # for Gf.Matrix4*
                for row1, row2 in zip(m * m.GetInverse(), Matrix()):
                    self.assertTrue(Gf.IsClose(row1, row2, 1e-5))
                self.assertTrue(Gf.IsClose(m.GetDeterminant(), det, 1e-5))

            m1 = Matrix(0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0)
            det1 = -1.0

            m2 = Matrix(0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
            det2 = -1.0

            m3 = Matrix(0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0)
            det3 = -1.0

            m4 = Matrix(1.0, 2.0, -1.0, 3.0, 2.0, 1.0, 4.0, 5.1, 2.0, 3.0, 1.0, 6.0, 3.0, 2.0, 1.0, 1.0)
            det4 = 16.8

            AssertDeterminant(m1, det1)
            AssertDeterminant(m2, det2)
            AssertDeterminant(m3, det3)
            AssertDeterminant(m4, det4)
            AssertDeterminant(m1 * m1, det1 * det1)
            AssertDeterminant(m1 * m4, det1 * det4)
            AssertDeterminant(m1 * m3 * m4, det1 * det3 * det4)
            AssertDeterminant(m1 * m3 * m4 * m2, det1 * det3 * det4 * det2)
            AssertDeterminant(m2 * m3 * m4 * m2, det2 * det3 * det4 * det2)
