path.h

Go to the documentation of this file.

#ifndef COPP_PATH_H
#define COPP_PATH_H
 
/*
 * Generated by bindings/c/scripts/generate_headers.ps1.
 * Do not edit by hand; update src/ffi/c and regenerate.
 */

 
#include <stddef.h>
#include <math.h>
#include "copp/core.h"
 
#ifdef __cplusplus
extern "C" {
#endif


typedef struct CoppPath CoppPath;

typedef struct CoppJet3 {
    double v;
    double d1;
    double d2;
    double d3;
} CoppJet3;

typedef enum CoppPathOutOfRangeMode {
    COPP_PATH_OUT_OF_RANGE_MODE_ERROR = 0,
    COPP_PATH_OUT_OF_RANGE_MODE_CLAMP = 1,
} CoppPathOutOfRangeMode;

typedef enum CoppPathParametrization {
    COPP_PATH_PARAMETRIZATION_UNIFORM = 0,
} CoppPathParametrization;

typedef struct CoppPathOptions {
    size_t order;
    double s_min;
    double s_max;
    enum CoppPathOutOfRangeMode out_of_range_mode;
    enum CoppPathParametrization parametrization;
    struct CoppMatrixViewF64 start_state;
    struct CoppMatrixViewF64 end_state;
} CoppPathOptions;

typedef enum CoppStatus (*CoppPathParametricFn)(void *user_data,
                                                size_t dim,
                                                struct CoppJet3 s,
                                                struct CoppJet3 *q);

typedef enum CoppStatus (*CoppPathEvaluate2ndFn)(void *user_data,
                                                 size_t dim,
                                                 size_t n,
                                                 const double *s,
                                                 double *q,
                                                 double *dq,
                                                 double *ddq);

typedef enum CoppStatus (*CoppPathEvaluate3rdFn)(void *user_data,
                                                 size_t dim,
                                                 size_t n,
                                                 const double *s,
                                                 double *q,
                                                 double *dq,
                                                 double *ddq,
                                                 double *dddq);
 
#ifdef __cplusplus
}
#endif

static inline struct CoppJet3 copp_constant(double x)
{
    struct CoppJet3 out = {x, 0.0, 0.0, 0.0};
    return out;
}

static inline struct CoppJet3 copp_add(struct CoppJet3 a, struct CoppJet3 b)
{
    struct CoppJet3 out = {a.v + b.v, a.d1 + b.d1, a.d2 + b.d2, a.d3 + b.d3};
    return out;
}

static inline struct CoppJet3 copp_add_f64(struct CoppJet3 a, double b)
{
    struct CoppJet3 out = {a.v + b, a.d1, a.d2, a.d3};
    return out;
}

static inline struct CoppJet3 copp_sub(struct CoppJet3 a, struct CoppJet3 b)
{
    struct CoppJet3 out = {a.v - b.v, a.d1 - b.d1, a.d2 - b.d2, a.d3 - b.d3};
    return out;
}

static inline struct CoppJet3 copp_sub_f64(struct CoppJet3 a, double b)
{
    struct CoppJet3 out = {a.v - b, a.d1, a.d2, a.d3};
    return out;
}

static inline struct CoppJet3 copp_f64_sub(double a, struct CoppJet3 b)
{
    struct CoppJet3 out = {a - b.v, -b.d1, -b.d2, -b.d3};
    return out;
}

static inline struct CoppJet3 copp_neg(struct CoppJet3 x)
{
    struct CoppJet3 out = {-x.v, -x.d1, -x.d2, -x.d3};
    return out;
}

static inline struct CoppJet3 copp_mul(struct CoppJet3 a, struct CoppJet3 b)
{
    struct CoppJet3 out = {
        a.v * b.v,
        a.d1 * b.v + a.v * b.d1,
        a.d2 * b.v + 2.0 * a.d1 * b.d1 + a.v * b.d2,
        a.d3 * b.v + 3.0 * (a.d2 * b.d1 + a.d1 * b.d2) + a.v * b.d3};
    return out;
}

static inline struct CoppJet3 copp_mul_f64(struct CoppJet3 a, double b)
{
    struct CoppJet3 out = {a.v * b, a.d1 * b, a.d2 * b, a.d3 * b};
    return out;
}

static inline struct CoppJet3 copp_f64_div(double a, struct CoppJet3 b)
{
    const double inv = 1.0 / b.v;
    const double inv2 = inv * inv;
    const double inv3 = inv2 * inv;
    const double inv4 = inv3 * inv;
    const double d1sq = b.d1 * b.d1;
    struct CoppJet3 out = {
        a * inv,
        -a * inv2 * b.d1,
        a * (2.0 * inv3 * d1sq - inv2 * b.d2),
        a * (-6.0 * inv4 * d1sq * b.d1 + 6.0 * inv3 * b.d1 * b.d2 - inv2 * b.d3)};
    return out;
}

static inline struct CoppJet3 copp_div_f64(struct CoppJet3 a, double b)
{
    return copp_mul_f64(a, 1.0 / b);
}

static inline struct CoppJet3 copp_div(struct CoppJet3 a, struct CoppJet3 b)
{
    return copp_mul(a, copp_f64_div(1.0, b));
}

static inline struct CoppJet3 copp_sin(struct CoppJet3 x)
{
    const double sv = sin(x.v);
    const double cv = cos(x.v);
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        sv,
        cv * x.d1,
        -sv * d1sq + cv * x.d2,
        -cv * d1sq * x.d1 - 3.0 * sv * x.d1 * x.d2 + cv * x.d3};
    return out;
}

static inline struct CoppJet3 copp_cos(struct CoppJet3 x)
{
    const double sv = sin(x.v);
    const double cv = cos(x.v);
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        cv,
        -sv * x.d1,
        -cv * d1sq - sv * x.d2,
        sv * d1sq * x.d1 - 3.0 * cv * x.d1 * x.d2 - sv * x.d3};
    return out;
}

static inline struct CoppJet3 copp_exp(struct CoppJet3 x)
{
    const double ev = exp(x.v);
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        ev,
        ev * x.d1,
        ev * (d1sq + x.d2),
        ev * (d1sq * x.d1 + 3.0 * x.d1 * x.d2 + x.d3)};
    return out;
}

static inline struct CoppJet3 copp_log(struct CoppJet3 x)
{
    const double inv = 1.0 / x.v;
    const double inv2 = inv * inv;
    const double inv3 = inv2 * inv;
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        log(x.v),
        inv * x.d1,
        -inv2 * d1sq + inv * x.d2,
        2.0 * inv3 * d1sq * x.d1 - 3.0 * inv2 * x.d1 * x.d2 + inv * x.d3};
    return out;
}

static inline struct CoppJet3 copp_sqrt(struct CoppJet3 x)
{
    const double sqrtv = sqrt(x.v);
    const double inv_sqrt = 1.0 / sqrtv;
    const double inv_v_sqrt = inv_sqrt / x.v;
    const double inv_v2_sqrt = inv_v_sqrt / x.v;
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        sqrtv,
        0.5 * inv_sqrt * x.d1,
        -0.25 * inv_v_sqrt * d1sq + 0.5 * inv_sqrt * x.d2,
        0.375 * inv_v2_sqrt * d1sq * x.d1 - 0.75 * inv_v_sqrt * x.d1 * x.d2 + 0.5 * inv_sqrt * x.d3};
    return out;
}

static inline struct CoppJet3 copp_powi(struct CoppJet3 x, int n)
{
    if (n == 0) {
        return copp_constant(1.0);
    }
 
    const double nf = (double)n;
    const double coeff2 = nf * (nf - 1.0);
    const double coeff3 = coeff2 * (nf - 2.0);
    const double dv = nf * pow(x.v, nf - 1.0);
    const double ddv = coeff2 == 0.0 ? 0.0 : coeff2 * pow(x.v, nf - 2.0);
    const double dddv = coeff3 == 0.0 ? 0.0 : coeff3 * pow(x.v, nf - 3.0);
    const double d1sq = x.d1 * x.d1;
    struct CoppJet3 out = {
        pow(x.v, (double)n),
        dv * x.d1,
        ddv * d1sq + dv * x.d2,
        dddv * d1sq * x.d1 + 3.0 * ddv * x.d1 * x.d2 + dv * x.d3};
    return out;
}
 
#ifdef __cplusplus
extern "C" {
#endif

enum CoppStatus copp_path_default_options(double s_min,
                                          double s_max,
                                          struct CoppPathOptions *out_options);

enum CoppStatus copp_path_from_waypoints(struct CoppMatrixViewF64 waypoints,
                                         struct CoppPathOptions options,
                                         struct CoppPath **out_path);

enum CoppStatus copp_path_from_parametric(size_t dim,
                                          double s_min,
                                          double s_max,
                                          CoppPathParametricFn evaluate,
                                          void *user_data,
                                          struct CoppPath **out_path);

enum CoppStatus copp_path_from_evaluator_2nd(size_t dim,
                                             double s_min,
                                             double s_max,
                                             CoppPathEvaluate2ndFn evaluate_2nd,
                                             void *user_data,
                                             struct CoppPath **out_path);

enum CoppStatus copp_path_from_evaluator_3rd(size_t dim,
                                             double s_min,
                                             double s_max,
                                             CoppPathEvaluate2ndFn evaluate_2nd,
                                             CoppPathEvaluate3rdFn evaluate_3rd,
                                             void *user_data,
                                             struct CoppPath **out_path);

enum CoppStatus copp_path_dim(const struct CoppPath *path, size_t *out_dim);

enum CoppStatus copp_path_s_range(const struct CoppPath *path,
                                  double *out_s_min,
                                  double *out_s_max);

enum CoppStatus copp_path_evaluate_up_to_2nd(const struct CoppPath *path,
                                             struct CoppSliceF64 s,
                                             struct CoppMatrixF64 *out_q,
                                             struct CoppMatrixF64 *out_dq,
                                             struct CoppMatrixF64 *out_ddq);

enum CoppStatus copp_path_evaluate_up_to_3rd(const struct CoppPath *path,
                                             struct CoppSliceF64 s,
                                             struct CoppMatrixF64 *out_q,
                                             struct CoppMatrixF64 *out_dq,
                                             struct CoppMatrixF64 *out_ddq,
                                             struct CoppMatrixF64 *out_dddq);

void copp_path_free(struct CoppPath *path);

 
#ifdef __cplusplus
}
#endif
 
#endif /* COPP_PATH_H */