SegmentedQuinticBezierToolkit.h

Go to the documentation of this file.

#ifndef SEGMENTEDQUINTICBEZIERTOOLKIT_H_
#define SEGMENTEDQUINTICBEZIERTOOLKIT_H_
/* -------------------------------------------------------------------------- *
 *     OpenSim:  SegmentedQuinticBezierToolkit.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 <vector>
#include <rbdl/rbdl_math.h>
#include <rbdl/rbdl_config.h>
#include "Function.h"
 
namespace RigidBodyDynamics {
  namespace Addons {
  namespace Geometry{
 
class RBDL_ADDON_DLLAPI SegmentedQuinticBezierToolkit
{
 
 
  public:
 
  static double scaleCurviness(double curviness);
 
  static double calcU(
      double ax, 
      const RigidBodyDynamics::Math::VectorNd& bezierPtsX, 
      double tol, 
      int maxIter);
 
 
 
  static int calcIndex(
    double x, 
    const RigidBodyDynamics::Math::MatrixNd& bezierPtsX);
  
  static int calcIndex(
    double x, 
    const std::vector<RigidBodyDynamics::Math::VectorNd>& bezierPtsX);
 
 
  
  
 
  static double calcQuinticBezierCurveVal(
        double u, 
        const RigidBodyDynamics::Math::VectorNd& pts);
 
  static double calcQuinticBezierCurveDerivU(
        double u, 
        const RigidBodyDynamics::Math::VectorNd& pts,
        int order);
 
  static double  calcQuinticBezierCurveDerivDYDX(
      double u,
      const RigidBodyDynamics::Math::VectorNd& xpts, 
      const RigidBodyDynamics::Math::VectorNd& ypts, 
      int order);
 
  
  static RigidBodyDynamics::Math::MatrixNd 
  calcQuinticBezierCornerControlPoints(   double x0, double y0,
            double dydx0,
            double x1, double y1,
            double dydx1,
            double curviness);
 
  /*
  This function numerically integrates the Bezier curve y(x).
 
  @param vX   Values of x to evaluate the integral of y(x) 
  @param ic0  The initial value of the integral
  @param intAcc   Accuracy of the integrated solution
  @param uTol   Tolerance on the calculation of the intermediate u term
  @param uMaxIter Maximum number of iterations allowed for u to reach its
      desired tolerance.
  @param mX     The 6xn matrix of Bezier control points for x(u)
  @param mY     The 6xn matrix of Bezier control points for y(u)
 
  @param flag_intLeftToRight  Setting this flag to true will evaluate the
    integral from the left most point to the right most 
    point. Setting this flag to false will cause the 
    integral to be evaluated from right to left.
  @param name   Name of caller.
  @return RigidBodyDynamics::Math::MatrixNd Col 0: X vector, Col 1: int(y(x))
 
 
  This function numerically integrates the Bezier curve y(x), when really 
  both x and y are specified in terms of u. Evaluate the integral at the 
  locations specified in vX and return the result. 
 
  <B>Computational Costs</B>
 
  This the expense of this function depends on the number of points in
  vX, the points for which the integral y(x) must be calculated. The 
  integral is evaluated using a Runge Kutta 45 integrator, and so each
  point requires 6 function evaluations. 
  (http://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method)
 
  The cost of evaluating 1 Bezier curve y(x) scales with the number
  of points in xVal:
  \verbatim
    ~1740 flops per point
  \endverbatim
 
  The example below is quite involved, but just so it can show you an
  example of how to create the array of Spline objects that approximate 
  the function u(x). Although the example has been created for only 1
  Bezier curve set, simply changing the size and entries of the matricies
  _mX and _mY will allow multiple sets to be integrated.
 
 
  <B>Example:</B>
  @code
  //Integrator and u tolerance settings
  double INTTOL = 1e-12;
  double UTOL   = 1e-14;
  int  MAXITER= 10;
 
  //Make up a Bezier curve - these happen to be the control points
  //for a tendon curve
  RigidBodyDynamics::Math::MatrixNd _mX(6,1), _mY(6,1);
  _mX(0)= 1;
  _mX(1)= 1.01164;
  _mX(2)= 1.01164;
  _mX(3)= 1.02364;
  _mX(4)= 1.02364;
  _mX(5)= 1.04;
 
  _mY(0) = 0;
  _mY(1) = 3.10862e-16;
  _mY(2) = 3.10862e-16;
  _mY(3) = 0.3;
  _mY(4) = 0.3;
  _mY(5) = 1;
 
  _numBezierSections = 1;
  bool _intx0x1 = true; //Says we're integrating from x0 to x1
  //Generate the set of splines that approximate u(x)
  RigidBodyDynamics::Math::VectorNd u(NUM_SAMPLE_PTS); 
  //Used for the approximate inverse
  RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); 
  //Used for the approximate inverse
 
  //Used to generate the set of knot points of the integral of y(x)  
  RigidBodyDynamics::Math::VectorNd 
  xALL(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1));
  _arraySplineUX.resize(_numBezierSections);
  int xidx = 0;
 
  for(int s=0; s < _numBezierSections; s++){
    //Sample the local set for u and x
    for(int i=0;i<NUM_SAMPLE_PTS;i++){
    u(i) = ( (double)i )/( (double)(NUM_SAMPLE_PTS-1) );
    x(i) = SegmentedQuinticBezierToolkit::
      calcQuinticBezierCurveVal(u(i),_mX(s),_name);    
    if(_numBezierSections > 1){
       //Skip the last point of a set that has another set of points
       //after it. Why? The last point and the starting point of the
       //next set are identical in value.
      if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){
      xALL(xidx) = x(i);
      xidx++;
      }
    }else{
      xALL(xidx) = x(i);    
      xidx++;
    }
    }
    //Create the array of approximate inverses for u(x)  
    _arraySplineUX[s] = SimTK::SplineFitter<double>::
    fitForSmoothingParameter(3,x,u,0).getSpline();
  }
 
   
  //Compute the integral of y(x) and spline the result  
 
  RigidBodyDynamics::Math::VectorNd yInt =  SegmentedQuinticBezierToolkit::
    calcNumIntBezierYfcnX(xALL,0,INTTOL, UTOL, MAXITER,_mX, _mY,
    _arraySplineUX,_name);
  
  if(_intx0x1==false){
    yInt = yInt*-1;
    yInt = yInt - yInt(yInt.nelt()-1);
  }
 
  _splineYintX = SimTK::SplineFitter<double>::
    fitForSmoothingParameter(3,xALL,yInt,0).getSpline();
  
  @endcode
 
 
  */
 
//MM:   Can't port this over to RBDL as RBDL doesn't have an error
//  controlled integrator. I could add this if a dependency
//  like Boost was allowed.    
// 
//  static RigidBodyDynamics::Math::MatrixNd 
//  calcNumIntBezierYfcnX(
//    const RigidBodyDynamics::Math::VectorNd& vX, 
//    double ic0, 
//    double intAcc, 
//    double uTol, 
//    int uMaxIter,
//    const RigidBodyDynamics::Math::MatrixNd& mX,
//     const RigidBodyDynamics::Math::MatrixNd& mY,
//    const SimTK::Array_<SimTK::Spline>& aSplineUX, 
//    bool flag_intLeftToRight,const std::string& name);
 
 
   private:
  
 
  static void printMatrixToFile(
    const RigidBodyDynamics::Math::VectorNd& col0, 
    const RigidBodyDynamics::Math::MatrixNd& data, 
    std::string& filename);
 
  static void printBezierSplineFitCurves(
      const Function_<double>& curveFit,
      RigidBodyDynamics::Math::MatrixNd& ctrlPts, 
      RigidBodyDynamics::Math::VectorNd& xVal, 
      RigidBodyDynamics::Math::VectorNd& yVal, 
      std::string& filename);  
 
  static double clampU(double u);
 
  
 
class BezierData {
  public:
  RigidBodyDynamics::Math::MatrixNd _mX;
  RigidBodyDynamics::Math::MatrixNd _mY;
  //std::vector< std::vector<double> > _aArraySplineUX;
  double _initalValue;
  double _uTol;
  int _uMaxIter;
  //bool  _flag_intLeftToRight;
  //double _startValue; 
 
  std::string _name;
};
 
 
};
 
}
}
}
 
 
#endif