Initial commit

Gems/PhysX/Code/NumericalMethods/Source/Eigenanalysis/EigenanalysisUtilities.cpp

@@ -0,0 +1,176 @@
+/*
+* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
+* its licensors.
+*
+* For complete copyright and license terms please see the LICENSE at the root of this
+* distribution (the "License"). All use of this software is governed by the License,
+* or, if provided, by the license below or the license accompanying this file. Do not
+* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*
+*/
+
+#include <NumericalMethods_precompiled.h>
+
+#include <cmath>
+
+#include <AzCore/std/algorithm.h>
+
+#include <LinearAlgebra.h>
+#include <Eigenanalysis/Utilities.h>
+
+namespace NumericalMethods::Eigenanalysis
+{
+    VectorVariable CrossProduct(const VectorVariable& lhs, const VectorVariable& rhs)
+    {
+        AZ_Assert(
+            lhs.GetDimension() == 3 && rhs.GetDimension() == 3, "VectorVariable dimensions invalid for cross product."
+        );
+
+        return VectorVariable::CreateFromVector({
+            lhs[1] * rhs[2] - lhs[2] * rhs[1],
+            lhs[2] * rhs[0] - lhs[0] * rhs[2],
+            lhs[0] * rhs[1] - lhs[1] * rhs[0]
+        });
+    }
+
+    void ComputeOrthogonalComplement(
+        const VectorVariable& vecW, VectorVariable& vecU, VectorVariable& vecV
+    )
+    {
+        // Robustly computes a right-handed orthogonal basis {vecU, vecV, vecW}.
+        double invLength = 1.0;
+
+        if (fabs(vecW[0]) > fabs(vecW[1]))
+        {
+            // The component of maximum absolute value is either vecW[0] or vecW[2].
+            invLength /= sqrt(vecW[0] * vecW[0] + vecW[2] * vecW[2]);
+            vecU = VectorVariable::CreateFromVector({ -vecW[2] * invLength, 0.0, vecW[0] * invLength });
+        }
+        else
+        {
+            // The component of maximum absolute value is either vecW[1] or vecW[2].
+            invLength /= sqrt(vecW[1] * vecW[1] + vecW[2] * vecW[2]);
+            vecU = VectorVariable::CreateFromVector({ 0.0, vecW[2] * invLength, -vecW[1] * invLength });
+        }
+
+        vecV = CrossProduct(vecW, vecU);
+    }
+
+    VectorVariable ComputeEigenvector0(
+        double a00, double a01, double a02, double a11, double a12, double a22, double val
+    )
+    {
+        // By definition, (A−e∗I)v = 0, where e is the eigenvalue and v is the corresponding eigenvector to be found.
+        // This condition implies that the rows (A−e∗I) must be perpendicular to v. This matrix must have rank 2, so two
+        // rows will be linearly dependent. For those two rows, the cross product will be (nearly) zero. So to find v,
+        // we can simply take the cross product of the two rows that maximize its magnitude.
+        VectorVariable row0 = VectorVariable::CreateFromVector({ a00 - val, a01, a02 });
+        VectorVariable row1 = VectorVariable::CreateFromVector({ a01, a11 - val, a12 });
+        VectorVariable row2 = VectorVariable::CreateFromVector({ a02, a12, a22 - val });
+
+        VectorVariable r0xr1 = CrossProduct(row0, row1);
+        VectorVariable r0xr2 = CrossProduct(row0, row2);
+        VectorVariable r1xr2 = CrossProduct(row1, row2);
+
+        double d0 = r0xr1.Dot(r0xr1);
+        double d1 = r0xr2.Dot(r0xr2);
+        double d2 = r1xr2.Dot(r1xr2);
+
+        return d0 >= d1 && d0 >= d2 ? r0xr1 * (1.0 / sqrt(d0)) :
+               d1 >= d0 && d1 >= d2 ? r0xr2 * (1.0 / sqrt(d1)) :
+                                      r1xr2 * (1.0 / sqrt(d2)) ;
+    }
+
+    VectorVariable ComputeEigenvector1(
+        double a00,
+        double a01,
+        double a02,
+        double a11,
+        double a12,
+        double a22,
+        double val,
+        const VectorVariable& vec
+    )
+    {
+        // Real symmetric matrices must have orthogonal eigenvectors. Thus, if we generate two vectors vecU and vecV
+        // orthogonal to the eigenvector vec already found, the remaining eigenvectors must be a circular combination
+        // of vecU and vecW. This reduces the problem to a 2D system. For details see Eberly.
+        VectorVariable vecU(3);
+        VectorVariable vecV(3);
+        ComputeOrthogonalComplement(vec, vecU, vecV);
+
+        MatrixVariable matA(3, 3);
+
+        matA.Element(0, 0) = a00;
+        matA.Element(0, 1) = a01;
+        matA.Element(0, 2) = a02;
+
+        matA.Element(1, 0) = a01;
+        matA.Element(1, 1) = a11;
+        matA.Element(1, 2) = a12;
+
+        matA.Element(2, 0) = a02;
+        matA.Element(2, 1) = a12;
+        matA.Element(2, 2) = a22;
+
+        double m00 = vecU.Dot(matA * vecU) - val;
+        double absM00 = fabs(m00);
+
+        double m01 = vecU.Dot(matA * vecV);
+        double absM01 = fabs(m01);
+
+        double m11 = vecV.Dot(matA * vecV) - val;
+        double absM11 = fabs(m11);
+
+        auto discardComponentAndNormalize = [](double& factor, double& other) {
+            other /= factor;
+            factor = 1.0 / sqrt(1.0 + other * other);
+            other *= factor;
+        };
+
+        if (absM00 > absM11)
+        {
+            if (AZStd::max(absM00, absM01) > 0.0)
+            {
+                if (absM00 >= absM01)
+                {
+                    discardComponentAndNormalize(m00, m01);
+                }
+                else
+                {
+                    discardComponentAndNormalize(m01, m00);
+                }
+                return vecU * m01 - vecV * m00;
+            }
+            else
+            {
+                return vecU;
+            }
+        }
+        else
+        {
+            if (AZStd::max(absM11, absM01) > 0.0)
+            {
+                if (absM11 >= absM01)
+                {
+                    discardComponentAndNormalize(m11, m01);
+                }
+                else
+                {
+                    discardComponentAndNormalize(m01, m11);
+                }
+                return vecU * m11 - vecV * m01;
+            }
+            else
+            {
+                return vecU;
+            }
+        }
+    }
+
+    VectorVariable ComputeEigenvector2(const VectorVariable& vec0, const VectorVariable& vec1)
+    {
+        return CrossProduct(vec0, vec1);
+    }
+} // namespace NumericalMethods::Eigenanalysis

Gems/PhysX/Code/NumericalMethods/Source/Eigenanalysis/Solver3x3.cpp

@@ -0,0 +1,137 @@
+/*
+* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
+* its licensors.
+*
+* For complete copyright and license terms please see the LICENSE at the root of this
+* distribution (the "License"). All use of this software is governed by the License,
+* or, if provided, by the license below or the license accompanying this file. Do not
+* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*
+*/
+
+#include <NumericalMethods_precompiled.h>
+
+#include <algorithm>
+#include <cmath>
+
+#include <AzCore/std/algorithm.h>
+
+#include <LinearAlgebra.h>
+#include <Eigenanalysis/Solver3x3.h>
+#include <Eigenanalysis/Utilities.h>
+
+namespace NumericalMethods::Eigenanalysis
+{
+    SolverResult<Real, 3> NonIterativeSymmetricEigensolver3x3(
+        double a00, double a01, double a02, double a11, double a12, double a22
+    )
+    {
+        // Using the notation from Eberly:
+        // A        - the symmetric input matrix
+        // a<ij>    - the upper elements of the matrix (0 <= i <= j <= 2).
+        // B        - a matrix derived from A, such that B = (A - q*I)/p where
+        //                p = sqrt( tr( (A−q∗I)^2 ) / 6 )
+        //                q = tr(A) / 3
+        // beta<i>  - the eigenvalues of B (0 <= i <= 2)
+        // alpha<i> - the eigenvalues of A (not explicit, stored in the result) (0 <= i <= 2)
+
+        double alpha0 = 0.0;
+        double alpha1 = 0.0;
+        double alpha2 = 0.0;
+
+        VectorVariable vec0 = VectorVariable::CreateFromVector({ 1.0, 0.0, 0.0 });
+        VectorVariable vec1 = VectorVariable::CreateFromVector({ 0.0, 1.0, 0.0 });
+        VectorVariable vec2 = VectorVariable::CreateFromVector({ 0.0, 0.0, 1.0 });
+
+        // Precondition the matrix by factoring out the element of biggest magnitude. This is to guard against
+        // floating-point overflow/underflow.
+        double maxAbsElem = std::max({fabs(a00), fabs(a01), fabs(a02), fabs(a11), fabs(a12), fabs(a22)});
+
+        if (maxAbsElem != 0.0)
+        {
+            // A is not the zero matrix.
+            double invMaxAbsElem = 1.0 / maxAbsElem;
+            a00 *= invMaxAbsElem;
+            a01 *= invMaxAbsElem;
+            a02 *= invMaxAbsElem;
+            a11 *= invMaxAbsElem;
+            a12 *= invMaxAbsElem;
+            a22 *= invMaxAbsElem;
+
+            double norm = a01 * a01 + a02 * a02 + a12 * a12;
+            if (norm > 0.0)
+            {
+                // Compute the eigenvalues of A. For a detailed explanation of how the algorithm works, see Eberly.
+                double q = (a00 + a11 + a22) / 3.0;
+
+                double b00 = a00 - q;
+                double b11 = a11 - q;
+                double b22 = a22 - q;
+
+                double p = sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
+
+                double c00 = b11 * b22 - a12 * a12;
+                double c01 = a01 * b22 - a12 * a02;
+                double c02 = a01 * a12 - b11 * a02;
+                double det = (b00 * c00 - a01 * c01 + a02 * c02) / (p * p * p);
+
+                double halfDet = AZStd::clamp(det * 0.5, -1.0, 1.0);
+                double angle = acos(halfDet) / 3.0;
+
+                static const double twoThirdsPi = 2.09439510239319549;
+
+                // The eigenvalues of B are ordered such that beta0 <= beta1 <= beta2.
+                double beta2 = cos(angle) * 2.0;
+                double beta0 = cos(angle + twoThirdsPi) * 2.0;
+                double beta1 = -(beta0 + beta2);
+
+                // The eigenvalues of A are ordered such that alpha0 <= alpha1 <= alpha2.
+                alpha0 = q + p * beta0;
+                alpha1 = q + p * beta1;
+                alpha2 = q + p * beta2;
+
+                // Compute the eigenvectors. We either have
+                //     beta0 <= beta1 < 0 < beta2 (if halfDet >= 0); or
+                //     beta0 < 0 < beta1 <= beta2 (if halfDef < 0).
+                // For numerical stability, we use different approaches to compute the eigenvector corresponding to the
+                // eigenvalue that is definitely not repeated and the other two.
+                if (halfDet >= 0.0)
+                {
+                    vec2 = ComputeEigenvector0(a00, a01, a02, a11, a12, a22, alpha2);
+                    vec1 = ComputeEigenvector1(a00, a01, a02, a11, a12, a22, alpha1, vec2);
+                    vec0 = ComputeEigenvector2(vec1, vec2);
+                }
+                else
+                {
+                    vec0 = ComputeEigenvector0(a00, a01, a02, a11, a12, a22, alpha0);
+                    vec1 = ComputeEigenvector1(a00, a01, a02, a11, a12, a22, alpha1, vec0);
+                    vec2 = ComputeEigenvector2(vec0, vec1);
+                }
+            }
+            else
+            {
+                // A is a diagonal matrix. The eigenvalues in this case are the elements along the main diagonal, and
+                // the eigenvectors are the standard Cartesian basis vectors.
+                alpha0 = a00;
+                alpha1 = a11;
+                alpha2 = a22;
+            }
+
+            // The scaling applied to A in the precondition scales the eigenvalues by the same amount and must be
+            // reverted.
+            alpha0 *= maxAbsElem;
+            alpha1 *= maxAbsElem;
+            alpha2 *= maxAbsElem;
+        }
+
+        return SolverResult<Real, 3>{
+            SolverOutcome::Success,
+            {
+                Eigenpair<Real, 3>{alpha0, {{vec0[0], vec0[1], vec0[2]}}},
+                Eigenpair<Real, 3>{alpha1, {{vec1[0], vec1[1], vec1[2]}}},
+                Eigenpair<Real, 3>{alpha2, {{vec2[0], vec2[1], vec2[2]}}}
+            }
+        };
+    }
+} // namespace NumericalMethods::Eigenanalysis

Gems/PhysX/Code/NumericalMethods/Source/Eigenanalysis/Solver3x3.h

@@ -0,0 +1,27 @@
+/*
+* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
+* its licensors.
+*
+* For complete copyright and license terms please see the LICENSE at the root of this
+* distribution (the "License"). All use of this software is governed by the License,
+* or, if provided, by the license below or the license accompanying this file. Do not
+* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*
+*/
+
+#pragma once
+
+#include <NumericalMethods/Eigenanalysis.h>
+
+namespace NumericalMethods::Eigenanalysis
+{
+    //! Finds the eigenvalues and vectors of the symmetric matrix whose unique elements are given (see Eberly).
+    //! @param a<ij> The element of the matrix in row i, column j.
+    //! @return Orthonormal eigenbasis of the matrix and the corresponding eigenvalues.
+    SolverResult<Real, 3> NonIterativeSymmetricEigensolver3x3(
+        double a00, double a01, double a02,
+                    double a11, double a12,
+                                double a22
+    );
+} // namespace NumericalMethods::Eigenanalysis

Gems/PhysX/Code/NumericalMethods/Source/Eigenanalysis/Utilities.h

@@ -0,0 +1,70 @@
+/*
+* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
+* its licensors.
+*
+* For complete copyright and license terms please see the LICENSE at the root of this
+* distribution (the "License"). All use of this software is governed by the License,
+* or, if provided, by the license below or the license accompanying this file. Do not
+* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*
+*/
+
+#pragma once
+
+namespace NumericalMethods
+{
+    class VectorVariable;
+
+    namespace Eigenanalysis
+    {
+        //! Compute the cross product between two 3D vectors.
+        //! @param lhs The left-hand side vector.
+        //! @param rhs The right-hand side vector.
+        //! @return The 3D vector equal to the cross product if both input vectors are 3-dimensional.
+        VectorVariable CrossProduct(const VectorVariable& lhs, const VectorVariable& rhs);
+
+        //! Robustly computes a right-handed orthonormal basis containing a given unit-length 3D input vector.
+        //! @param vecW[in] A 3D input vector which must be of unit length.
+        //! @param vecU[out] The first of the computed unit-length orthogonal vectors.
+        //! @param vecV[out] The second of the computed unit-length orthogonal vectors.
+        //! @return {vecU, vecV, vecW} will be a right-handed orthogonal set.
+        void ComputeOrthogonalComplement(const VectorVariable& vecW, VectorVariable& vecU, VectorVariable& vecV);
+
+        //! Given elements of a symmetric 3x3 matrix and one of its eigenvalues, computes the corresponding eigenvector.
+        //! For numerical stability, this function should only be used to find the eigenvector corresponding to
+        //! eigenvalues that are unique and numerically not close to other eigenvalues.
+        //! @param a<ij> The element of the matrix in row i, column j.
+        //! @param val One of the eigenvalues of the matrix.
+        //! @return The corresponding eigenvector.
+        VectorVariable ComputeEigenvector0(
+            double a00, double a01, double a02, double a11, double a12, double a22, double val
+        );
+
+        //! Given elements of a symmetric 3x3 matrix, one of its eigenvalues and an unrelated eigenvector, computes the
+        //! eigenvector corresponding to the eigenvalue.
+        //! This algorithm is numerically stable even if the eigenvalue is repeated.
+        //! @param a<ij> The element of the matrix in row i, column j.
+        //! @param val The eigenvalue whose corresponding eigenvector is to be found.
+        //! @param vec The unrelated eigenvector that is already known.
+        //! @return The eigenvector corresponding to the given eigenvalue.
+        VectorVariable ComputeEigenvector1(
+            double a00,
+            double a01,
+            double a02,
+            double a11,
+            double a12,
+            double a22,
+            double val,
+            const VectorVariable& vec
+        );
+
+        // Given two eigenvectors of a symmetric 3x3 matrix, computes the third.
+        // The third eigenvector is found by taking the cross product of the known eigenvectors (the eigenvectors of a
+        // real symmetric 3x3 matrix are always orthogonal).
+        //! @param vec0 The first of the already known eigenvectors.
+        //! @param vec1 The second of the already known eigenvectors.
+        //! @return The computed eigenvector.
+        VectorVariable ComputeEigenvector2(const VectorVariable& vec0, const VectorVariable& vec1);
+    } // namespace Eigenanalysis
+} // namespace NumericalMethods