TimePath.h

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants
Licensee a limited, non-exclusive, non-transferable, royalty-free license
(without the right to sublicense) to use the API solely for the purpose of
Licensee's internal development efforts to develop applications for which
the API was provided.
 
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Design
File: TimePath
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/Buffer.h>
#include <NDEVR/Time.h>
#include <NDEVR/TimeSpan.h>
namespace NDEVR
{
    class Path
    {
    public:
        Path()
        {}

        Path(const Buffer<Time>& time, const Buffer<fltp04>& y, const Buffer<fltp04>& m)
            : mX(time)
            , mY(y)
            , mM(m)
        {}

        Path(const Path& path)
            : mX(path.mX)
            , mY(path.mY)
            , mM(path.mM)
        {}

        Path(const Path&& path)
            : mX(std::move(path.mX))
            , mY(std::move(path.mY))
            , mM(std::move(path.mM))
        {}

        fltp08 interpolateX(const Time& current_time)
        {
            // Handle the boundary cases.
            uint04 n = mX.size();
            if (IsInvalid(current_time) || n == 0) {
                return Constant<fltp08>::Invalid;
            }
            if (current_time <= mX[0]) {
                return mY[0];
            }
            if (current_time >= mX.last()) {
                return mY.last();
            }
            // Find the index 'i' of the last point with smaller X.
            // We know this will be within the spline due to the boundary tests.
            int i = 0;
            while (current_time >= mX[i + 1])
            {
                i += 1;
                if (current_time == mX[i])
                    return mY[i];
            }
            // Perform cubic Hermite spline interpolation.
            fltp08 h = (mX[i + 1] - mX[i]).elapsedSeconds();
            fltp08 t = (current_time - mX[i]).elapsedSeconds() / h;
            return (mY[i] * (1 + 2 * t) + h * mM[i] * t) * (1 - t) * (1 - t)
                + (mY[i + 1] * (3 - 2 * t) + h * mM[i + 1] * (t - 1)) * t * t;
        }

        static Path createMonotoneCubicPath(const Buffer<Time>& time, const Buffer<fltp04>& y)
        {
            if (time.size() != y.size() || time.size() < 2)
                throw Exception(_t("There must be at least two control points and the arrays must be of equal length."));
            uint04 n = time.size();
            fltp04 object = 0.0f;
            Buffer<fltp04> dx(n - 1, object); // could optimize this out
            Buffer<fltp04> mx(n, object);
            // Compute slopes of secant lines between successive points.
            for (uint04 i = 0; i < n - 1; i++)
            {
                fltp08 h = (time[i + 1] - time[i]).elapsedSeconds();
                if (h < 0.0)
                    throw Exception("The control points must all have strictly increasing X values:" + String(time[i]) + " " + String(time[i + 1]));
                dx[i] = (y[i + 1] - y[i]) / cast<fltp04>(h);
 
            }
            // Initialize the tangents as the average of the secants.
            mx[0] = dx[0];
            for (uint04 i = 1; i < n - 1; i++)
            {
                mx[i] = (dx[i - 1] + dx[i]) * 0.5f;
            }
            mx[n - 1] = dx[n - 2];
 
            // Update the tangents to preserve monotonicity.
            for (uint04 i = 0; i < n - 1; i++)
            {
                if (dx[i] == 0.0f)
                { // successive Y values are equal
                    mx[i] = 0.0f;
                    mx[i + 1] = 0.0f;
                }
                else
                {
                    float a = mx[i] / dx[i];
                    float b = mx[i + 1] / dx[i];
                    if (a < 0.0f || b < 0.0f) {
                        //throw new IllegalArgumentException("The control points must have "
                        //+ "monotonic Y values.");
                    }
                    float h = (float)hypot(a, b);
                    if (h > 9.0f) {
                        float t = 3.0f / h;
                        mx[i] = t * a * dx[i];
                        mx[i + 1] = t * b * dx[i];
                    }
                }
            }
            return Path(time, y, mx);
        }

        static Path createMonotoneCubicPathAzimuth(const Buffer<Time>& time, const Buffer<Angle<fltp08>>& y)
        {
            if (time.size() != y.size() || time.size() < 2)
            {
                throw new Exception(_t("There must be at least two control points and the arrays must be of equal length."));
            }
            Buffer<Time> temp_time(y.size() * 2);
            Buffer<fltp04> temp_y(y.size() * 2);
            for (uint04 i = 0; i < time.size() - 1; i++)
            {
                temp_y.add(cast<fltp04>(y[i].as<DEGREES>()));
                temp_time.add(time[i]);
                if((y[i].as<DEGREES>() - y[i + 1].as<DEGREES>()) > 180.0)
                {
                    temp_y.add( 179.99f);
                    temp_y.add(-179.99f);
                    temp_time.add(time[i] + (time[i + 1] - time[i]) / 2.0);
                    temp_time.add(time[i] + (time[i + 1] - time[i]) / 2.0);
                }
                if ((y[i].as<DEGREES>() - y[i + 1].as<DEGREES>()) < -180.0)
                {
                    temp_y.add(-179.99f);
                    temp_y.add( 179.99f);
                    temp_time.add(time[i] + (time[i + 1] - time[i]) / 2.0);
                    temp_time.add(time[i] + (time[i + 1] - time[i]) / 2.0);
                }
            }
            return createMonotoneCubicPath(temp_time, temp_y);
        }

        static Path createMonotoneCubicPathSorted(const Buffer<Time>& time, const Buffer<fltp04>& y)
        {
            Buffer<std::pair<Time, fltp04>> sorted_path(time.size());
            for (uint04 i = 0; i < time.size(); i++)
            {
                sorted_path.add(std::pair<Time, fltp04>(time[i], y[i]));
            }
            std::sort(sorted_path.begin(), sorted_path.end(), [](const std::pair<Time, fltp04>& p1, const std::pair<Time, fltp04>& p2)
            {
                return p1.first > p2.first;
            });
            Buffer<Time> corrected_time(time.size());
            Buffer<fltp04> corrected_value(time.size());
            for (uint04 i = 0; i < time.size(); i++)
            {
                corrected_value.add(sorted_path[i].second);
                corrected_time.add(sorted_path[i].first);
            }
            return createMonotoneCubicPath(corrected_time, corrected_value);
        }

        static Path createMonotoneAngleCubicPathSorted(const Buffer<Time>& time, const Buffer<Angle<fltp08>>& y)
        {
            Buffer<std::pair<Time, Angle<fltp08>>> sorted_path(time.size());
            for (uint04 i = 0; i < time.size(); i++)
            {
                sorted_path.add(std::pair<Time, Angle<fltp08>>(time[i], y[i]));
            }
            std::sort(sorted_path.begin(), sorted_path.end(), [](const std::pair<Time, Angle<fltp08>>& p1, const std::pair<Time, Angle<fltp08>>& p2)
            {
                return p1.first > p2.first;
            });
            Buffer<Time> corrected_time(time.size());
            Buffer<Angle<fltp08>> corrected_value(time.size());
            for (uint04 i = 0; i < time.size(); i++)
            {
                corrected_value.add(sorted_path[i].second);
                corrected_time.add(sorted_path[i].first);
            }
            return createMonotoneCubicPathAzimuth(corrected_time, corrected_value);
        }

        Path& operator=(const Path& path)
        {
            mX = path.mX;
            mY = path.mY;
            mM = path.mM;
            return *this;
        }
    protected:
        Buffer<Time> mX;    
        Buffer<fltp04> mY;  
        Buffer<fltp04> mM;  
    };
}