SmoothSegmentedFunction.h

Go to the documentation of this file.

#ifndef SMOOTHSEGMENTEDFUNCTION_H_
#define SMOOTHSEGMENTEDFUNCTION_H_
/* -------------------------------------------------------------------------- *
 *      OpenSim:  SmoothSegmentedFunction.h                                   *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Matthew Millard                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
/*
  Update:
  This is a port of the original code so that it will work with
  the multibody code RBDL written by Martin Felis.
  
  Author:
  Matthew Millard
  
  Date:
  Nov 2015
 
*/
 
#include "Function.h"
#include "SegmentedQuinticBezierToolkit.h"
#include <vector>
#include <rbdl/rbdl_math.h>
#include <rbdl/rbdl_config.h>
 
namespace RigidBodyDynamics {
  namespace Addons {
  namespace Geometry{
 
  class RBDL_ADDON_DLLAPI SmoothSegmentedFunction : public Function_<double>
  {
   
 
     public:
 
    SmoothSegmentedFunction();
 
    //SmoothSegmentedFunction();
     SmoothSegmentedFunction(
      const RigidBodyDynamics::Math::MatrixNd& mX, 
      const RigidBodyDynamics::Math::MatrixNd& mY, 
      double x0, double x1,
      double y0, double y1,
      double dydx0, double dydx1,
      const std::string& name); 
 
 
     void updSmoothSegmentedFunction(
      const RigidBodyDynamics::Math::MatrixNd& mX, 
      const RigidBodyDynamics::Math::MatrixNd& mY, 
      double x0, double x1,
      double y0, double y1,
      double dydx0, double dydx1,
      const std::string& name);
 
     void shift(double xShift, double yShift);
 
     void scale(double xScale, double yScale);
 
 
 
     double calcValue(double x) const;
 
     double calcInverseValue( double y,
                              double xGuess) const;
 
     double calcDerivative(double x, int order) const;   
 
     
 
     
     /*This will return the value of the integral of this objects curve 
     evaluated at x. 
     
     @param x the domain point of interest
 
     \b aborts \b
    -If the function does not have a pre-computed integral
     @return the value of the functions integral evaluated at x
 
     The integral is approximate, though its errors are small.
     The integral is computed by numerically integrating the function when
     the constructor for this class is called (if computeIntegral is true) and 
     then splining the result, thus the regions between the knot points may
     have some error in them. A very fine mesh of points is used to create the
     spline so the errors will be small
 
     <B>Computational Costs</B>
     \verbatim
      x in curve domain  : ~13 flops
      x in linear section: ~19 flops
     \endverbatim
 
     */
     //double calcIntegral(double x) const;
     
     /*
    Returns a bool that indicates if the integral curve has been computed.
 
    @return true if the integral of this function is available, false if
      it has not been computed.
     */
     //bool isIntegralAvailable() const;
 
     /*
     Returns a bool that indicates if the integral computed is compuated left
     to right, or right to left.
 
     @return true if the integral was computed left to right, and false if the
       integral was computed right to left. Note that the output of
       this function is only valid if isIntegralAvailable() returns
       true.
     */
     //bool isIntegralComputedLeftToRight() const;
 
     std::string getName() const;
 
     void setName(const std::string &name);
 
     RigidBodyDynamics::Math::VectorNd getCurveDomain() const;
 
     void printCurveToCSVFile(const std::string& path,
          const std::string& fileNameWithoutExtension,
            double domainMin,
            double domainMax) const;
     
 
     RigidBodyDynamics::Math::MatrixNd calcSampledCurve(int maxOrder,
                double domainMin,
                double domainMax) const;
 
 
     void getXControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const;
     void getYControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const;
 
    private:
     
 
    //Nov 2015: Not needed in the RBDL port.
    //SimTK::Array_<SimTK::Spline> _arraySplineUX;  
    //SimTK::Spline _splineYintX;
    
    std::vector<RigidBodyDynamics::Math::VectorNd> _mXVec; 
    std::vector<RigidBodyDynamics::Math::VectorNd> _mYVec; 
 
    int _numBezierSections;
 
    double _x0;
    double _x1;
    double _y0;
    double _y1;
    double _dydx0;
    double _dydx1;
    /*This is the users */
    //bool _computeIntegral;
 
    /*This variable, when true, indicates that the user wants the integral
    from left to right (x0 to x1). If it is false, the integral from right
    to left (x1 to x0) is computed*/
    //bool _intx0x1;
    std::string _name;
      
    friend class SmoothSegmentedFunctionFactory;
 
     
 
 
 
    void printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data,
          std::vector<std::string>& colnames,
          const std::string& path, 
          const std::string& filename) const;
 
    double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const; 
    /*virtual*/
 
     double calcDerivative(const std::vector<int>& derivComponents, 
           const RigidBodyDynamics::Math::VectorNd& x) const; 
           /*virtual*/ 
 
     int getArgumentSize() const; /*virtual*/ 
 
     /*virtual*/ 
     int getMaxDerivativeOrder() const;
 
     
  };
 
}
}
}
 
 
 
#endif