CL_Spring.h

Go to the documentation of this file.

/*
 * PROPRIETARY INFORMATION.  This software is proprietary to
 * Side Effects Software Inc., and is not to be reproduced,
 * transmitted, or disclosed in any way without written permission.
 *
 * NAME:        CL_Spring.h ( Clip Library, C++)
 *
 * COMMENTS:
 *    Applies a spring filter to input data
 */


#ifndef __CL_Spring__
#define __CL_Spring__

#include "CL_API.h"
#include <SYS/SYS_Math.h>
#include <SYS/SYS_Types.h>

#include <algorithm>

class UT_Interrupt;

class CL_Spring
{
public:

    /// Method used to compute the spring filter. Either position or force
    enum class Method
    {
        POSITION,
        FORCE
    };


    /// Solve for a spring filter given input data 
    /// data and result must both be of length size
    /// computes initial displacement and initial velocity
    /// automatically based on the input data
    template <class F>
    static void solve(const fpreal *data, fpreal *result, int size, 
                      fpreal spring_constant, fpreal mass,
                      fpreal damping_constant, fpreal inc, 
                      Method method, const F& interrupt);

    /// Solve for a spring filter given input data 
    /// data and result must both be of length size
    /// computes initial displacement and initial velocity
    /// automatically based on the input data
    /// Note: This method is an overload for the above, allowing the user to 
    /// avoid passing in a lambda for the interrupt method.
    /// With c++17 support, this could be more gracefully handled using
    /// inline variables:
    ///
    /// static inline auto defaultInterruptor = []() { return false; };

    /// template <typename F = decltype(defaultInterruptor)>
    /// static void foo(const F &interrupt = defaultInterruptor) {}
    static void solve(const fpreal *data, fpreal *result, int size, 
                      fpreal spring_constant, fpreal mass,
                      fpreal damping_constant, fpreal inc, 
                      Method method)
    {
        solve(data, result, size, spring_constant, mass, damping_constant, inc, 
              method, []() { return false; });
    }

    /// Solve for a spring filter given input data 
    /// data and result must both be of length size
    /// initial displacement and initial velocity are passed in manually
    template <class F>
    static void solve(const fpreal *data, fpreal *result, int size, 
                      fpreal spring_constant, fpreal mass,
                      fpreal damping_constant, fpreal inc,
                      fpreal initial_displacement, fpreal initial_velocity, 
                      Method method, const F& interrupt);

    static void solve(const fpreal *data, fpreal *result, int size, 
                      fpreal spring_constant, fpreal mass,
                      fpreal damping_constant, fpreal inc,
                      fpreal initial_displacement, fpreal initial_velocity, 
                      Method method)
    {
        solve(data, result, size, spring_constant, mass, damping_constant, inc,
              initial_displacement, initial_velocity, method, []() { return false; });
    }

    /// Spring solver for realtime data
    /// Must provide a lambda (get_data) which returns the data value 
    /// given the result index
    /// \return bool - whether or not the solution is steady
    /// \return d1, d2 - output parameters d1 and d2 are updated in place 
    template <class F>
    static bool solveRealtime(const F& get_data, fpreal *result, int size,
                              fpreal spring_constant, fpreal mass,
                              fpreal damping_constant, fpreal inc,
                              fpreal &d1, fpreal &d2,
                              Method method);

private:
    static fpreal doSolve(fpreal f, fpreal spring_constant, fpreal mass,
                          fpreal damping_constant, fpreal inc, Method method,
                          fpreal &d1, fpreal &d2)
    {
        if (method != Method::FORCE)
            f *= spring_constant;

        fpreal vel = (d1 - d2) / inc;

        fpreal mass_min = 0.001;
        mass = SYSmax(mass, mass_min);
        
        fpreal acc = (f - vel * damping_constant - d1 * spring_constant) / mass;
        vel += acc * inc;
        fpreal d = d1 + vel * inc; 

        d2 = d1;
        d1 = d;

        return d;
    }
};

template <class F>
inline void 
CL_Spring::solve(const fpreal *data, fpreal *result, int size, 
                 fpreal spring_constant, fpreal mass,
                 fpreal damping_constant, fpreal inc,
                 fpreal initial_displacement, fpreal initial_velocity,
                 Method method, const F& interrupt) 
{
    fpreal d1 = initial_displacement;
    fpreal d2 = d1 - initial_velocity * inc;

    for (exint j = 0; j < size; j++)
    {    
        if (interrupt())
            return;

        result[j] = doSolve(data[j], spring_constant, mass, damping_constant, inc, method, d1, d2);
    }
}


template <class F>
inline void
CL_Spring::solve(const fpreal *data, fpreal * result, int size, 
                 fpreal spring_constant, fpreal mass,
                 fpreal damping_constant, fpreal inc, 
                 Method method, const F& interrupt) 
{
    if (size < 2)
    {
        if (result != data)
            std::copy(data, data + size, result);
        return;
    }
    
    fpreal initial_displacement = data[0];
    fpreal initial_velocity = data[1] - initial_displacement;

    solve(data, result, size, spring_constant, mass, 
          damping_constant, inc, initial_displacement, initial_velocity, 
          method, interrupt);
}

template <class F>
inline bool 
CL_Spring::solveRealtime(const F& get_data, fpreal *result, int size,
                          fpreal spring_constant, fpreal mass,
                          fpreal damping_constant, fpreal inc,
                          fpreal &d1, fpreal &d2,
                          Method method)
{
    fpreal delta = 0.0;
    bool steady = true;
    for (exint j = 0; j < size; j++)
    {    
        fpreal f = get_data(j);
        fpreal d = doSolve(f, spring_constant, mass, damping_constant, inc, method, d1, d2);

        if (method == Method::FORCE)
            f /= spring_constant;

        delta = SYSabs(f - d);

        if (delta > 0.001)
            steady = false;

        result[j] = d;
    }

    return steady;
}

#endif