Initial commit

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