Line data    Source code

       1             : /*
       2             :  * spline5.cpp
       3             :  *
       4             :  * Created by William Geyer and Chris Olson modified for ardupilot
       5             :  * Original work by Ryan Muller
       6             :  * https://gist.github.com/ryanthejuggler/4132103
       7             :  * released under the Creative Commons CC0 License
       8             :  * http://creativecommons.org/publicdomain/zero/1.0/
       9             :  *
      10             :  * This file is free software: you can redistribute it and/or modify it
      11             :  * under the terms of the GNU General Public License as published by the
      12             :  * Free Software Foundation, either version 3 of the License, or
      13             :  * (at your option) any later version.
      14             :  *
      15             :  * This file is distributed in the hope that it will be useful, but
      16             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
      17             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      18             :  * See the GNU General Public License for more details.
      19             :  *
      20             :  * You should have received a copy of the GNU General Public License along
      21             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      22             :  */
      23             : 
      24             : #include <stdint.h>
      25             : #include "spline5.h"
      26             : 
      27      108681 : void splinterp5(const float x[5], float out[4][4])
      28             : {
      29             : 
      30             :     // number of spline points
      31      108681 :     const uint8_t n = 5;
      32             : 
      33             :     // working variables
      34      108681 :     float u[n] {};
      35             : 
      36             :     // second derivative
      37             :     // additional element in array necessary for back substitution loop.
      38      108681 :     float z[n+1] {};
      39             : 
      40             :     // set the second derivative to 0 at the ends
      41      108681 :     z[0] = u[0] = 0;
      42      108681 :     z[n-1] = 0;
      43             : 
      44             :     // decomposition loop
      45      434724 :     for (uint8_t i=1; i<n-1; i++) {
      46      326043 :         float p = 0.5f * z[i-1] + 2.0f;
      47             :         // keep p from ever becoming zero
      48      326043 :         if (p < 0.01f && p >= 0.0f) {
      49           0 :             p = 0.01f;
      50      326043 :         } else if (p > -0.01f && p < 0.0f) {
      51           0 :             p = -0.01f;
      52             :         }
      53      326043 :         const float p_inv = 1.0f / p;
      54      326043 :         z[i] = -0.5f * p_inv;
      55      326043 :         u[i] = x[i+1] + x[i-1] - 2.0f * x[i];
      56      326043 :         u[i] = (3.0f * u[i] - 0.5f * u[i-1]) * p_inv;
      57             :     }
      58             : 
      59             :     // back-substitution loop
      60      543405 :     for (uint8_t i=n-1; i>0; i--) {
      61      434724 :         z[i] = z[i] * z[i+1] + u[i];
      62             :     }
      63             : 
      64      543405 :     for (uint8_t i=0; i<n-1; i++) {
      65      434724 :         out[i][0] = x[i+1];
      66      434724 :         out[i][1] = x[i];
      67      434724 :         out[i][2] = z[i+1];
      68      434724 :         out[i][3] = z[i];
      69             :     }
      70             : 
      71      108681 : }