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

Gems/PhysX/Code/NumericalMethods/Source/LinearAlgebra.cpp

@@ -0,0 +1,300 @@
+/*
+* 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 <LinearAlgebra.h>
+#include <AzCore/Memory/SystemAllocator.h>
+#include <AzCore/Math/MathUtils.h>
+
+namespace NumericalMethods
+{
+    VectorVariable::VectorVariable(AZ::u32 dimension)
+    {
+        m_values.resize(dimension, 0.0);
+    }
+
+    VectorVariable VectorVariable::CreateFromVector(const AZStd::vector<double>& values)
+    {
+        VectorVariable result;
+        result.m_values = values;
+        return result;
+    }
+
+    AZ::u32 VectorVariable::GetDimension() const
+    {
+        return static_cast<AZ::u32>(m_values.size());
+    }
+
+    double& VectorVariable::operator[](AZ::u32 index)
+    {
+        AZ_Assert(index < m_values.size(), "Invalid VectorVariable index.");
+        return m_values[index];
+    }
+
+    double VectorVariable::operator[](AZ::u32 index) const
+    {
+        AZ_Assert(index < m_values.size(), "Invalid VectorVariable index.");
+        return m_values[index];
+    }
+
+    VectorVariable VectorVariable::operator+(const VectorVariable& rhs) const
+    {
+        const AZ::u32 dimension = GetDimension();
+        AZ_Assert(dimension == rhs.GetDimension(), "VectorVariable dimensions do not match.");
+        VectorVariable result(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result.m_values[i] = m_values[i] + rhs[i];
+        }
+        return result;
+    }
+
+    VectorVariable VectorVariable::operator+=(const VectorVariable& rhs)
+    {
+        const AZ::u32 dimension = GetDimension();
+        AZ_Assert(dimension == rhs.GetDimension(), "VectorVariable dimensions do not match.");
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            m_values[i] += rhs[i];
+        }
+        return *this;
+    }
+
+    VectorVariable VectorVariable::operator-() const
+    {
+        const AZ::u32 dimension = GetDimension();
+        VectorVariable result(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result[i] = -m_values[i];
+        }
+        return result;
+    }
+
+    VectorVariable VectorVariable::operator-(const VectorVariable& rhs) const
+    {
+        const AZ::u32 dimension = GetDimension();
+        AZ_Assert(dimension == rhs.GetDimension(), "VectorVariable dimensions do not match.");
+        VectorVariable result(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result[i] = m_values[i] - rhs[i];
+        }
+        return result;
+    }
+
+    VectorVariable VectorVariable::operator-=(const VectorVariable& rhs)
+    {
+        const AZ::u32 dimension = GetDimension();
+        AZ_Assert(dimension == rhs.GetDimension(), "VectorVariable dimensions do not match.");
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            m_values[i] -= rhs[i];
+        }
+        return *this;
+    }
+
+    VectorVariable VectorVariable::operator*(const double rhs) const
+    {
+        const AZ::u32 dimension = GetDimension();
+        VectorVariable result(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result[i] = m_values[i] * rhs;
+        }
+        return result;
+    }
+
+    double VectorVariable::Norm() const
+    {
+        const AZ::u32 dimension = GetDimension();
+        double sumSquares = 0.0;
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            sumSquares += m_values[i] * m_values[i];
+        }
+        return sqrt(sumSquares);
+    }
+
+    double VectorVariable::Dot(const VectorVariable& rhs) const
+    {
+        const AZ::u32 dimension = GetDimension();
+        AZ_Assert(dimension == rhs.GetDimension(), "VectorVariable dimensions do not match.");
+        double result = 0.0;
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result += m_values[i] * rhs[i];
+        }
+        return result;
+    }
+
+    const AZStd::vector<double>& VectorVariable::GetValues() const
+    {
+        return m_values;
+    }
+
+    VectorVariable operator*(const double lhs, const VectorVariable& rhs)
+    {
+        const AZ::u32 dimension = rhs.GetDimension();
+        VectorVariable result(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            result[i] = lhs * rhs[i];
+        }
+        return result;
+    }
+
+    MatrixVariable::MatrixVariable(AZ::u32 numRows, AZ::u32 numColumns)
+    {
+        m_numRows = numRows;
+        m_numColumns = numColumns;
+        m_values.clear();
+        m_values.resize(m_numRows * m_numColumns, 0.0);
+    }
+
+    double& MatrixVariable::Element(AZ::u32 row, AZ::u32 column)
+    {
+        AZ_Assert(row < m_numRows && column < m_numColumns, "Invalid matrix index.");
+        return m_values[row * m_numColumns + column];
+    }
+
+    double MatrixVariable::Element(AZ::u32 row, AZ::u32 column) const
+    {
+        AZ_Assert(row < m_numRows && column < m_numColumns, "Invalid matrix index.");
+        return m_values[row * m_numColumns + column];
+    }
+
+    AZ::u32 MatrixVariable::GetNumRows() const
+    {
+        return m_numRows;
+    }
+
+    AZ::u32 MatrixVariable::GetNumColumns() const
+    {
+        return m_numColumns;
+    }
+
+    MatrixVariable MatrixVariable::operator+(const MatrixVariable& rhs) const
+    {
+        AZ_Assert(m_numRows == rhs.m_numRows && m_numColumns == rhs.m_numColumns, "Matrix dimensions do not match.");
+        MatrixVariable result(m_numRows, m_numColumns);
+        for (AZ::u32 row = 0; row < m_numRows; row++)
+        {
+            for (AZ::u32 column = 0; column < m_numColumns; column++)
+            {
+                result.Element(row, column) = Element(row, column) + rhs.Element(row, column);
+            }
+        }
+        return result;
+    }
+
+    MatrixVariable MatrixVariable::operator+=(const MatrixVariable& rhs)
+    {
+        AZ_Assert(m_numRows == rhs.m_numRows && m_numColumns == rhs.m_numColumns, "Matrix dimensions do not match.");
+        for (AZ::u32 row = 0; row < m_numRows; row++)
+        {
+            for (AZ::u32 column = 0; column < m_numColumns; column++)
+            {
+                Element(row, column) += rhs.Element(row, column);
+            }
+        }
+        return *this;
+    }
+
+    MatrixVariable MatrixVariable::operator-(const MatrixVariable& rhs) const
+    {
+        MatrixVariable result(m_numRows, m_numColumns);
+        for (AZ::u32 row = 0; row < m_numRows; row++)
+        {
+            for (AZ::u32 column = 0; column < m_numColumns; column++)
+            {
+                result.Element(row, column) = Element(row, column) - rhs.Element(row, column);
+            }
+        }
+        return result;
+    }
+
+    MatrixVariable MatrixVariable::operator/(const double divisor) const
+    {
+        MatrixVariable result(m_numRows, m_numColumns);
+        for (AZ::u32 row = 0; row < m_numRows; row++)
+        {
+            for (AZ::u32 column = 0; column < m_numColumns; column++)
+            {
+                result.Element(row, column) = Element(row, column) / divisor;
+            }
+        }
+        return result;
+    }
+
+    VectorVariable operator*(const MatrixVariable& lhs, const VectorVariable& rhs)
+    {
+        AZ_Assert(lhs.GetNumColumns() == rhs.GetDimension(), "Matrix and vector dimensions do not match.");
+        VectorVariable result(lhs.GetNumRows());
+        for (AZ::u32 row = 0; row < lhs.GetNumRows(); row++)
+        {
+            result[row] = 0.0;
+            for (AZ::u32 column = 0; column < lhs.GetNumColumns(); column++)
+            {
+                result[row] += lhs.Element(row, column) * rhs[column];
+            }
+        }
+
+        return result;
+    }
+
+    MatrixVariable operator*(const MatrixVariable& lhs, const MatrixVariable& rhs)
+    {
+        AZ_Assert(lhs.GetNumColumns() == rhs.GetNumRows(), "Invalid matrix dimensions for multiplication.");
+        MatrixVariable result(lhs.GetNumRows(), rhs.GetNumColumns());
+        for (AZ::u32 row = 0; row < lhs.GetNumRows(); row++)
+        {
+            for (AZ::u32 column = 0; column < rhs.GetNumColumns(); column++)
+            {
+                result.Element(row, column) = 0.0;
+                for (AZ::u32 i = 0; i < lhs.GetNumColumns(); i++)
+                {
+                    result.Element(row, column) += lhs.Element(row, i) * rhs.Element(i, column);
+                }
+            }
+        }
+        return result;
+    }
+
+    MatrixVariable operator*(double lhs, const MatrixVariable& rhs)
+    {
+        MatrixVariable result(rhs.GetNumRows(), rhs.GetNumColumns());
+        const AZ::u32 numRows = rhs.GetNumRows();
+        const AZ::u32 numColumns = rhs.GetNumColumns();
+        for (AZ::u32 row = 0; row < numRows; row++)
+        {
+            for (AZ::u32 column = 0; column < numColumns; column++)
+            {
+                result.Element(row, column) = lhs * rhs.Element(row, column);
+            }
+        }
+        return result;
+    }
+
+    MatrixVariable OuterProduct(const VectorVariable& x, const VectorVariable& y)
+    {
+        MatrixVariable result(x.GetDimension(), y.GetDimension());
+        for (AZ::u32 r = 0; r < x.GetDimension(); r++)
+        {
+            for (AZ::u32 c = 0; c < y.GetDimension(); c++)
+            {
+                result.Element(r, c) = x[r] * y[c];
+            }
+        }
+        return result;
+    }
+} // namespace NumericalMethods

Gems/PhysX/Code/NumericalMethods/Source/LinearAlgebra.h

@@ -0,0 +1,76 @@
+/*
+* 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 <AzCore/base.h>
+#include <AzCore/std/containers/vector.h>
+
+// This provides just the functionality for arbitrary dimension matrices and vectors that is required by this gem.
+// It is not intended to be a complete or optimized implementation.
+// If we add support for arbitrary dimension matrices or use a third party library, that should replace what is here.
+
+namespace NumericalMethods
+{
+    using ScalarVariable = double;
+
+    //! Class for arbitrary sized vectors, providing only the functionality required by the numerical methods supported
+    //! in this gem.
+    class VectorVariable
+    {
+    public:
+        VectorVariable() = default;
+        explicit VectorVariable(AZ::u32 dimension);
+        static VectorVariable CreateFromVector(const AZStd::vector<double>& values);
+        AZ::u32 GetDimension() const;
+        double& operator[](AZ::u32 index);
+        double operator[](AZ::u32 index) const;
+        VectorVariable operator+(const VectorVariable& rhs) const;
+        VectorVariable operator+=(const VectorVariable& rhs);
+        VectorVariable operator-() const;
+        VectorVariable operator-(const VectorVariable& rhs) const;
+        VectorVariable operator-=(const VectorVariable& rhs);
+        VectorVariable operator*(const double rhs) const;
+        double Norm() const;
+        double Dot(const VectorVariable& rhs) const;
+        const AZStd::vector<double>& GetValues() const;
+    private:
+        AZStd::vector<double> m_values;
+    };
+
+    VectorVariable operator*(const double lhs, const VectorVariable& rhs);
+
+    //! Class for arbitrary sized matrices, providing only the functionality required by the numerical methods supported
+    //! in this gem.
+    class MatrixVariable
+    {
+    public:
+        MatrixVariable() = default;
+        MatrixVariable(AZ::u32 numRows, AZ::u32 numColumns);
+        double& Element(AZ::u32 row, AZ::u32 column);
+        double Element(AZ::u32 row, AZ::u32 column) const;
+        AZ::u32 GetNumRows() const;
+        AZ::u32 GetNumColumns() const;
+        MatrixVariable operator+(const MatrixVariable& rhs) const;
+        MatrixVariable operator+=(const MatrixVariable& rhs);
+        MatrixVariable operator-(const MatrixVariable& rhs) const;
+        MatrixVariable operator/(const double divisor) const;
+    private:
+        AZStd::vector<double> m_values;
+        AZ::u32 m_numRows = 0;
+        AZ::u32 m_numColumns = 0;
+    };
+
+    VectorVariable operator*(const MatrixVariable& lhs, const VectorVariable& rhs);
+    MatrixVariable operator*(const MatrixVariable& lhs, const MatrixVariable& rhs);
+    MatrixVariable operator*(double lhs, const MatrixVariable& rhs);
+    MatrixVariable OuterProduct(const VectorVariable& x, const VectorVariable& y);
+} // namespace NumericalMethods

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

@@ -0,0 +1,48 @@
+/*
+* 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 <NumericalMethods/Eigenanalysis.h>
+#include <NumericalMethods/Optimization.h>
+#include <Optimization/SolverBFGS.h>
+#include <Eigenanalysis/Solver3x3.h>
+
+namespace NumericalMethods
+{
+    namespace Optimization
+    {
+        SolverResult SolverBFGS(const Function& function, const AZStd::vector<double>& initialGuess)
+        {
+            return MinimizeBFGS(function, initialGuess);
+        }
+    }
+
+    namespace Eigenanalysis
+    {
+        SolverResult<Real, 3> Solver3x3RealSymmetric(const SquareMatrix<Real, 3>& matrix)
+        {
+            // The matrix must be symmetric.
+            if (matrix[0][1] == matrix[1][0] && matrix[0][2] == matrix[2][0] && matrix[1][2] == matrix[2][1])
+            {
+                return NonIterativeSymmetricEigensolver3x3(
+                    matrix[0][0], matrix[0][1], matrix[0][2],
+                                  matrix[1][1], matrix[1][2],
+                                                matrix[2][2]
+                );
+            }
+            else
+            {
+                return SolverResult<Real, 3>{SolverOutcome::FailureInvalidInput};
+            }
+        }
+    }
+} // namespace NumericalMethods

Gems/PhysX/Code/NumericalMethods/Source/NumericalMethods_precompiled.cpp

@@ -0,0 +1,13 @@
+/*
+* 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>

Gems/PhysX/Code/NumericalMethods/Source/NumericalMethods_precompiled.h

@@ -0,0 +1,13 @@
+/*
+* 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

Gems/PhysX/Code/NumericalMethods/Source/Optimization/Constants.h

@@ -0,0 +1,32 @@
+/*
+* 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 <AzCore/base.h>
+
+namespace NumericalMethods::Optimization
+{
+    const AZ::u32 lineSearchIterations = 100;
+    const AZ::u32 solverIterations = 500;
+
+    // value of the gradient norm used for terminating the search
+    const double gradientTolerance = 1e-6;
+
+    // used in finite difference evaluation of derivatives
+    // a value close to the square root of the machine precision is recommended in Nocedal and Wright
+    const double epsilon = 1e-7;
+
+    // values recommended in Nocedal and Wright for constants in the Wolfe conditions for satisfactory solution improvement
+    const double c1 = 1e-4;
+    const double c2 = 0.9;
+} // namespace NumericalMethods::Optimization

Gems/PhysX/Code/NumericalMethods/Source/Optimization/LineSearch.cpp

@@ -0,0 +1,206 @@
+/*
+* 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 <Optimization/LineSearch.h>
+#include <Optimization/Utilities.h>
+#include <Optimization/Constants.h>
+#include <float.h>
+#include <math.h>
+
+namespace NumericalMethods::Optimization
+{
+    bool IsFailure(const LineSearchResult& result)
+    {
+        return result.m_outcome >= LineSearchOutcome::FailureExceededIterations;
+    }
+
+    ScalarVariable CubicMinimum(const double a, const double f_a, const double df_a, const double b, const double f_b,
+        const double c, const double f_c)
+    {
+        double coefficients[4] = {};
+        coefficients[1] = df_a;
+        const double db = b - a;
+        const double dc = c - a;
+        const double denominator = (db * db * dc * dc) * (db - dc);
+        double e[2];
+        e[0] = f_b - f_a - coefficients[1] * db;
+        e[1] = f_c - f_a - coefficients[1] * dc;
+        coefficients[3] = (dc * dc * e[0] - db * db * e[1]) / denominator;
+        coefficients[2] = (-dc * dc * dc * e[0] + db * db * db * e[1]) / denominator;
+        const double radical = coefficients[2] * coefficients[2] - 3.0 * coefficients[3] * coefficients[1];
+
+        return a + (-coefficients[2] + sqrt(radical)) / (3.0 * coefficients[3]);
+    }
+
+    ScalarVariable QuadraticMinimum(const double a, const double f_a, const double df_a, const double b, const double f_b)
+    {
+        double coefficients[3] = {};
+        const double db = b - a;
+        coefficients[1] = df_a;
+        coefficients[2] = (f_b - f_a - coefficients[1] * db) / (db * db);
+        return a - coefficients[1] / (2.0 * coefficients[2]);
+    }
+
+    bool ValidateStepSize(const ScalarVariable alphaNew, const double alpha0, const double alpha1, const double edgeThreshold)
+    {
+        const double alphaMin = AZStd::GetMin(alpha0, alpha1);
+        const double alphaMax = AZStd::GetMax(alpha0, alpha1);
+        const double range = alphaMax - alphaMin;
+        return (azisfinite(alphaNew) && (alphaNew > alphaMin + edgeThreshold * range) &&
+            (alphaNew < alphaMax - edgeThreshold * range));
+    }
+
+    LineSearchResult SelectStepSizeFromInterval(double alpha0, double alpha1, double f_alpha0, double f_alpha1,
+        double df_alpha0, const Function& f, const VectorVariable& x0, const VectorVariable& searchDirection,
+        const double f_x0, const double df_x0, const double c1, const double c2)
+    {
+        const double cubicEdgeThreshold = 0.2;
+        const double quadraticEdgeThreshold = 0.1;
+        double alphaLast = 0.0;
+        double f_alphaLast = f_x0;
+        LineSearchResult result;
+
+        for (AZ::u32 iteration = 0; iteration < lineSearchIterations; iteration++)
+        {
+            ScalarVariable alphaNew;
+            if (iteration > 0)
+            {
+                // first try selecting a new alpha value based on cubic interpolation through the most recent points
+                alphaNew = CubicMinimum(alpha0, f_alpha0, df_alpha0, alpha1, f_alpha1, alphaLast, f_alphaLast);
+            }
+
+            // if this is the first iteration, or the cubic method failed or is invalid, try a quadratic
+            if (iteration == 0 || !ValidateStepSize(alphaNew, alpha0, alpha1, cubicEdgeThreshold))
+            {
+                alphaNew = QuadraticMinimum(alpha0, f_alpha0, df_alpha0, alpha1, f_alpha1);
+
+                // if the quadratic is invalid, use bisection
+                if (!ValidateStepSize(alphaNew, alpha0, alpha1, quadraticEdgeThreshold))
+                {
+                    alphaNew = ScalarVariable(0.5 * (alpha0 + alpha1));
+                }
+            }
+
+            // Check if alphaNew satisfies the Wolfe conditions
+            // First the sufficient decrease condition
+            const double f_alphaNew = FunctionValue(f, x0 + alphaNew * searchDirection);
+            if ((f_alphaNew > f_x0 + c1 * alphaNew * df_x0) || (f_alphaNew >= f_alpha0))
+            {
+                // The decrease is not sufficient, so set up the parameters for the next iteration
+                f_alphaLast = f_alpha1;
+                alphaLast = alpha1;
+                alpha1 = alphaNew;
+                f_alpha1 = f_alphaNew;
+            }
+            else
+            {
+                // There is sufficient decrease, so test the second Wolfe condition i.e. whether the derivative
+                // corresponding to alphaNew is shallower than the derivative at x0
+                double df_alphaNew = DirectionalDerivative(f, x0 + alphaNew * searchDirection, searchDirection);
+                if (fabs(df_alphaNew) <= -c2 * df_x0)
+                {
+                    // alphaNew satisfies the Wolfe conditions, so return it
+                    result.m_outcome = LineSearchOutcome::Success;
+                    result.m_stepSize = alphaNew;
+                    result.m_functionValue = f_alphaNew;
+                    result.m_derivativeValue = df_alphaNew;
+                    return result;
+                }
+
+                if (df_alphaNew * (alpha1 - alpha0) >= 0.0)
+                {
+                    f_alphaLast = f_alpha1;
+                    alphaLast = alpha1;
+                    alpha1 = alpha0;
+                    f_alpha1 = f_alpha0;
+                }
+
+                else
+                {
+                    f_alphaLast = f_alpha0;
+                    alphaLast = alpha0;
+                }
+                alpha0 = alphaNew;
+                f_alpha0 = f_alphaNew;
+                df_alpha0 = df_alphaNew;
+            }
+        }
+
+        // Failed to find a conforming step size
+        result.m_stepSize = 0.0;
+        result.m_functionValue = 0.0;
+        result.m_derivativeValue = 0.0;
+        result.m_outcome = LineSearchOutcome::FailureExceededIterations;
+        return result;
+    }
+
+    LineSearchResult LineSearchWolfe(const Function& f, const VectorVariable& x0, double f_x0,
+        const VectorVariable& searchDirection)
+    {
+        // uses the notation from Nocedal and Wright, where alpha represents the step size
+        // alpha0 and alpha1 are the lower and upper bounds of an interval which brackets the final value of alpha
+        // initial step size of 1 is recommended for quasi-Newton methods (Nocedal and Wright)
+        double alpha0 = 0.0;
+        double alpha1 = 1.0;
+
+        double f_alpha1 = FunctionValue(f, x0 + alpha1 * searchDirection);
+        double f_alpha0 = f_x0;
+        const double df_x0 = DirectionalDerivative(f, x0, searchDirection);
+        double df_alpha0 = df_x0;
+
+        for (AZ::u32 iteration = 0; iteration < lineSearchIterations; iteration++)
+        {
+            // if the value of f corresponding to alpha1 isn't sufficiently small compared to f at x0,
+            // then the interval [alpha0 ... alpha1] must bracket a suitable point.
+            if ((f_alpha1 > f_x0 + c1 * alpha1 * df_x0) || (iteration > 0 && f_alpha1 > f_alpha0))
+            {
+                return SelectStepSizeFromInterval(alpha0, alpha1, f_alpha0, f_alpha1, df_alpha0,
+                    f, x0, searchDirection, f_x0, df_x0, c1, c2);
+            }
+
+            // otherwise, if the derivative corresponding to alpha1 is large enough, alpha1 already
+            // satisfies the Wolfe conditions and so return alpha1.
+            double df_alpha1 = DirectionalDerivative(f, x0 + alpha1 * searchDirection, searchDirection);
+            if (fabs(df_alpha1) <= -c2 * df_x0)
+            {
+                LineSearchResult result;
+                result.m_outcome = LineSearchOutcome::Success;
+                result.m_stepSize = alpha1;
+                result.m_functionValue = f_alpha1;
+                result.m_derivativeValue = df_alpha1;
+                return result;
+            }
+
+            if (df_alpha1 >= 0.0)
+            {
+                return SelectStepSizeFromInterval(alpha1, alpha0, f_alpha1, f_alpha0, df_alpha1,
+                    f, x0, searchDirection, f_x0, df_x0, c1, c2);
+            }
+
+            // haven't found an interval which is guaranteed to bracket a suitable point,
+            // so expand the search region for the next iteration
+            alpha0 = alpha1;
+            f_alpha0 = f_alpha1;
+            alpha1 = 2.0 * alpha1;
+            f_alpha1 = FunctionValue(f, x0 + alpha1 * searchDirection);
+            df_alpha0 = df_alpha1;
+        }
+
+        LineSearchResult result;
+        result.m_outcome = LineSearchOutcome::BestEffort;
+        result.m_stepSize = alpha1;
+        result.m_functionValue = f_alpha1;
+        result.m_derivativeValue = DirectionalDerivative(f, x0 + alpha1 * searchDirection, searchDirection);
+        return result;
+    }
+} // namespace NumericalMethods::Optimization

Gems/PhysX/Code/NumericalMethods/Source/Optimization/LineSearch.h

@@ -0,0 +1,83 @@
+/*
+* 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 <LinearAlgebra.h>
+#include <NumericalMethods/Optimization.h>
+
+namespace NumericalMethods::Optimization
+{
+    //! Used to indicated if a line search was successful or give details of failure reasons.
+    enum class LineSearchOutcome
+    {
+        Success, //!< A result which completely satisfies the line search requirements.
+        BestEffort, //!< A result which is not optimal but should still be usable.
+        FailureExceededIterations //!< Failed because the iteration limit was reached and the value is not usable.
+    };
+
+    //! Struct to bundle together the numerical results of a line search and a qualitative indicator of search success.
+    struct LineSearchResult
+    {
+        double m_stepSize;
+        double m_functionValue;
+        double m_derivativeValue;
+        LineSearchOutcome m_outcome;
+    };
+
+    //! Helper function to check whether a LineSearchResult should be considered a failure.
+    bool IsFailure(const LineSearchResult& result);
+
+    //! Finds the value of x which minimizes the cubic polynomial which interpolates the provided points.
+    //! Finds the cubic polynomial P(x) which satisfies
+    //! P(a) = f_a
+    //! P'(a) = df_a
+    //! P(b) = f_b
+    //! P(c) = f_c
+    //! and returns the value of x which minimizes P.
+    ScalarVariable CubicMinimum(const double a, const double f_a, const double df_a, const double b, const double f_b,
+        const double c, const double f_c);
+
+    //! Finds the value of x which minimizes the quadratic which interpolates the provided points.
+    //! Finds the quadratic Q(x) which satisfies
+    //! Q(a) = f_a
+    //! Q'(a) = df_a
+    //! Q(b) = f_b
+    //! and returns the value of value which minimizes Q.
+    ScalarVariable QuadraticMinimum(const double a, const double f_a, const double df_a, const double b, const double f_b);
+
+    //! Checks that the result of an interpolation is satisfactory.
+    //! Checks that the result of an interpolation is valid, inside the expected interval, and sufficiently far from
+    //! interval boundaries.
+    //! @param alphaNew The new step size to be checked.
+    //! @param alpha0 One boundary of the current step size interval.
+    //! @param alpha1 The other boundary of the current interval.
+    //! @param edgeThreshold Defines how close to the edges of current interval the new step size is allowed to be.
+    bool ValidateStepSize(const ScalarVariable alphaNew, const double alpha0, const double alpha1,
+        const double edgeThreshold);
+
+    //! Used in LineSearchWolfe to narrow down a step size once a bracketing interval has been found.
+    //! This corresponds to the zoom function in Nocedal and Wright.
+    LineSearchResult SelectStepSizeFromInterval(double alpha0, double alpha1, double f_alpha0, double f_alpha1,
+        double df_alpha0, const Function& f, const VectorVariable& x0, const VectorVariable& searchDirection,
+        const double f_x0, const double df_x0, const double c1, const double c2);
+
+    //! Searches for a step size satisfying the Wolfe conditions for solution improvement.
+    //! Given a search direction, attempts to find a step size in that direction which satisfies the Wolfe conditions (
+    //! conditions for solution improvement which have nice properties for algorithms which rely on the line search).
+    //! The first wolfe condition requires that the function value at the new point is sufficiently improved relative to
+    //! the previous iteration.  The second condition requires that the directional derivative at the new point is
+    //! sufficient to indicate that significantly more progress could not have been made by choosing a larger step.
+    //! The search proceeds in two phases - first an interval containing a suitable point is found, then a point within
+    //! that interval is narrowed down using the SelectStepSizeFromInterval function.
+    LineSearchResult LineSearchWolfe(const Function& f, const VectorVariable& x0, double f_x0,
+        const VectorVariable& searchDirection);
+} // namespace NumericalMethods::Optimization

Gems/PhysX/Code/NumericalMethods/Source/Optimization/SolverBFGS.cpp

@@ -0,0 +1,87 @@
+/*
+* 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 <Optimization/SolverBFGS.h>
+#include <Optimization/LineSearch.h>
+#include <Optimization/Utilities.h>
+#include <Optimization/Constants.h>
+
+namespace NumericalMethods::Optimization
+{
+    SolverResult MinimizeBFGS(const Function& f, const AZStd::vector<double>& xInitial)
+    {
+        // using the notation from Nocedal and Wright
+        // H - an approximation to the inverse of the Hessian (matrix of second derivatives)
+        // s - the difference between the function value this iteration and the previous iteration
+        // y - the difference between the function gradient this iteration and the previous iteration
+
+        SolverResult result;
+        const AZ::u32 dimension = static_cast<AZ::u32>(xInitial.size());
+        VectorVariable searchDirection(dimension);
+
+        MatrixVariable H(dimension, dimension);
+        MatrixVariable I(dimension, dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            H.Element(i, i) = 1.0;
+            I.Element(i, i) = 1.0;
+        }
+
+        VectorVariable x = VectorVariable::CreateFromVector(xInitial);
+        double f_x = FunctionValue(f, x);
+        for (; result.m_iterations < solverIterations; ++result.m_iterations)
+        {
+            // stop if the gradient is small enough
+            VectorVariable gradient = Gradient(f, x);
+            if (gradient.Norm() < gradientTolerance)
+            {
+                result.m_outcome = SolverOutcome::Success;
+                result.m_xValues = x.GetValues();
+                return result;
+            }
+
+            // find a search direction based on the Hessian and gradient and then search for an appropriate step size in
+            // that direction
+            searchDirection = -(H * gradient);
+            LineSearchResult lineSearchResult = LineSearchWolfe(f, x, f_x, searchDirection);
+            if (IsFailure(lineSearchResult))
+            {
+                result.m_outcome = SolverOutcome::Incomplete;
+                result.m_xValues = x.GetValues();
+                return result;
+            }
+            VectorVariable s = lineSearchResult.m_stepSize * searchDirection;
+            x += s;
+            f_x = lineSearchResult.m_functionValue;
+            VectorVariable y = Gradient(f, x) - gradient;
+
+            // on the first iteration, use a heuristic to scale the Hessian
+            if (result.m_iterations == 0)
+            {
+                double scale = y.Dot(s) / y.Dot(y);
+                for (AZ::u32 i = 0; i < dimension; i++)
+                {
+                    H.Element(i, i) = scale;
+                }
+            }
+
+            // update the approximate inverse Hessian using the BFGS formula (see Nocedal and Wright)
+            double rho = 1.0 / y.Dot(s);
+            H = (I - rho * OuterProduct(s, y)) * H * (I - rho * OuterProduct(y, s)) + rho * OuterProduct(s, s);
+        }
+
+        result.m_outcome = SolverOutcome::MaxIterations;
+        result.m_xValues = x.GetValues();
+        return result;
+    }
+} // namespace NumericalMethods::Optimization

Gems/PhysX/Code/NumericalMethods/Source/Optimization/SolverBFGS.h

@@ -0,0 +1,23 @@
+/*
+* 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/Optimization.h>
+
+namespace NumericalMethods::Optimization
+{
+    //! Minimizes the supplied function using the Broyden-Fletcher-Goldfarb-Shanno algorithm (see Nocedal and Wright).
+    //! @param f Function to be minimized.
+    //! @param xInitial Initial guess for the independent variable.
+    //! @return Value of the independent variable which minimizes f.
+    SolverResult MinimizeBFGS(const Function& f, const AZStd::vector<double>& xInitial);
+} // namespace NumericalMethods::Optimization

Gems/PhysX/Code/NumericalMethods/Source/Optimization/Utilities.cpp

@@ -0,0 +1,45 @@
+/*
+* 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 <NumericalMethods/Optimization.h>
+#include <Optimization/Constants.h>
+#include <Optimization/Utilities.h>
+
+namespace NumericalMethods::Optimization
+{
+    double FunctionValue(const Function& function, const VectorVariable& point)
+    {
+        return function.Execute(point.GetValues()).GetValue();
+    }
+
+    double DirectionalDerivative(const Function& function, const VectorVariable& point, const VectorVariable& direction)
+    {
+        return Gradient(function, point).Dot(direction);
+    }
+
+    VectorVariable Gradient(const Function& function, const VectorVariable& point)
+    {
+        const AZ::u32 dimension = point.GetDimension();
+        VectorVariable gradient(dimension);
+        VectorVariable direction(dimension);
+        for (AZ::u32 i = 0; i < dimension; i++)
+        {
+            direction[i] = 1.0;
+            double f_plus = FunctionValue(function, point + epsilon * direction);
+            double f_minus = FunctionValue(function, point - epsilon * direction);
+            gradient[i] = (f_plus - f_minus) / (2.0 * epsilon);
+            direction[i] = 0.0;
+        }
+        return gradient;
+    }
+} // namespace NumericalMethods::Optimization

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

@@ -0,0 +1,30 @@
+/*
+* 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 <LinearAlgebra.h>
+
+namespace NumericalMethods::Optimization
+{
+    class Function;
+
+    //! Helper function for evaluating a function at a point.
+    double FunctionValue(const Function& function, const VectorVariable& point);
+
+    //! The 1-dimensional rate of change of a function with respect to changing the independent variables along the specified direction.
+    //! Note that some textbooks / authors define the directional derivative with respect to a normalized direction,
+    //! but that convention is not used here.
+    double DirectionalDerivative(const Function& function, const VectorVariable& point, const VectorVariable& direction);
+
+    //! Vector of derivatives with respect to each of the independent variables of a function, evaluated at the specified point.
+    VectorVariable Gradient(const Function& function, const VectorVariable& point);
+} // namespace NumericalMethods::Optimization