mathlib.h

Go to the documentation of this file.

/********************************************************************
* Copyright (C) 2015 by Interstel Technologies, Inc.
*   and Hawaii Space Flight Laboratory.
*
* This file is part of the COSMOS/core that is the central
* module for COSMOS. For more information on COSMOS go to
* <http://cosmos-project.com>
*
* The COSMOS/core software is licenced under the
* GNU Lesser General Public License (LGPL) version 3 licence.
*
* You should have received a copy of the
* GNU Lesser General Public License
* If not, go to <http://www.gnu.org/licenses/>
*
* COSMOS/core is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
*
* COSMOS/core is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
* Lesser General Public License for more details.
*
* Refer to the "licences" folder for further information on the
* condititons and terms to use this software.
********************************************************************/

#ifndef _MATHLIB_H
#define _MATHLIB_H 1


#include "support/configCosmos.h"
#include "support/cosmos-errno.h"

// include all math modules
#include "constants.h"
#include "vector.h"
#include "matrix.h"
#include "rotation.h"

#include <cmath>
#include <iostream>

//#define true 1
//#define false 0



#define CRC16CCITT 0x1021
#define CRC16CCITTMSB 0x1021
#define CRC16CCITTR 0x8408
#define CRC16CCITTLSB 0x8408
#define CRC16CCITTRR 0x8810



// TODO: explain why this union is used for
typedef union
{
    double d;
    float f;
    int32_t i32;
    int16_t i16;
    uint32_t u32;
    uint16_t u16;
} utype;


// TODO: explain why this union is used for
typedef union
{
    quaternion q;
    qcomplex qc;
    qfirst qf;
    qlast ql;
    rvector r;
    cvector c;
    svector s;
    gvector g;
    matrix1d m1;
    avector a;
    double a4[4];
} uvector;


//typedef double*** matrix3d;


// TODO: create seperate class (and file) for gauss jackson functions
struct gj_kernel
{
    int32_t order;
    int32_t horder;
    double dvi;
    double dvi2;
    int32_t **binom;
    double **alpha;
    double **beta;
    double *c;
    double *gam;
    double *q;
    double *lam;
} ;


struct gj_step
{
    double vd0;
    double vd1;
    double vd2;
    double *a;
    double *b;
    double s;
    double ss;
    double sa;
    double sb;
} ;


struct gj_instance
{
    gj_kernel *kern;
    int32_t axes;
    gj_step **steps;
    double *vi;
    void (*calc_vd2)(double vi, double *vd0, double *vd2, int32_t axes);
} ;


struct gj_instance3d
{
    gj_kernel *kern;
    gj_step *stepx;
    gj_step *stepy;
    gj_step *stepz;
    double *vi;
    double (*calc_vd2)(double vi, double vdx0, double vdy0, double vdz0);
} ;


struct estimatorstruc
{
    double value[3];
    double error[3];
    vector<double> a;
    vector<double> x;
    vector<double> y;
};


struct estimatorhandle
{
    vector<estimatorstruc> r;
    int32_t index;
    uint32_t size;
    uint32_t degree;
    double xbase;
    double ybase;
} ;



double gaussian_random(double mean, double stdev);

double distance_rv(rvector p0, rvector p1, rvector p2);
//double distance_rv_1(rvector p0, rvector p1, rvector p2);
double area_rv(rvector p0, rvector p1, rvector p2);
double evaluate_poly(double x, rvector parms);
double evaluate_poly_slope(double x, rvector parms);
double evaluate_poly_accel(double x, rvector parms);
double evaluate_poly_jerk(double x, rvector parms);

gvector gv_evaluate_poly(double x, vector< vector<double> > parms);
gvector gv_evaluate_poly_slope(double x, vector< vector<double> > parms);
gvector gv_evaluate_poly_accel(double x, vector< vector<double> > parms);
gvector gv_evaluate_poly_jerk(double x, vector< vector<double> > parms);

rvector rv_evaluate_poly(double x, vector< vector<double> > parms);
rvector rv_evaluate_poly_slope(double x, vector< vector<double> > parms);
rvector rv_evaluate_poly_accel(double x, vector< vector<double> > parms);
rvector rv_evaluate_poly_jerk(double x, vector< vector<double> > parms);

// TODO: move these functions to quaternion ???
rvector rotate_q(quaternion q,rvector v);
cvector rotate_q(quaternion q, cvector v);
rvector transform_q(quaternion q,rvector v);

// Moving to new naming convention: drotate is Direct Rotation of vector, irotate is rotation of coordinate axes
rvector drotate(quaternion q,rvector v);
cvector drotate(quaternion q, cvector v);
rvector irotate(quaternion q,rvector v);
cvector irotate(quaternion q, cvector v);


rvector rv_quaternion2axis(quaternion q);
uvector rv_fitpoly(uvector x, uvector y, uint32_t order);
vector<double> polyfit(vector<double> &x, vector<double> &y);
void multisolve(vector< vector<double> > x, vector<double> y, vector<double>& a);
void open_estimate(estimatorhandle *estimate, uint32_t size, uint32_t degree);
int16_t set_estimate(estimatorhandle *estimate, double independent, double dependent);
estimatorstruc get_estimate(estimatorhandle *estimate, double independent);



ByteOrder local_byte_order();

uint16_t uint16from(uint8_t *pointer, ByteOrder order);
int16_t int16from(uint8_t *pointer, ByteOrder order);
uint32_t uint32from(uint8_t *pointer, ByteOrder order);
int32_t int32from(uint8_t *pointer, ByteOrder order);
float floatfrom(uint8_t *pointer, ByteOrder order);
double doublefrom(uint8_t *pointer, ByteOrder order);
void uint32to(uint32_t value, uint8_t *pointer, ByteOrder order);
void int32to(int32_t value, uint8_t *pointer, ByteOrder order);
void uint16to(uint16_t value, uint8_t *pointer, ByteOrder order);
void int16to(int16_t value, uint8_t *pointer, ByteOrder order);
void floatto(float value, uint8_t *pointer, ByteOrder order);
void doubleto(double value, uint8_t *pointer, ByteOrder order);

// Gauss-Jackson routines
gj_kernel *gauss_jackson_kernel(int32_t order, double dvi);
void gauss_jackson_dekernel(gj_kernel *gjk);
gj_instance *gauss_jackson_instance(gj_kernel *gjk,int32_t axes,void (*calc_vd2)(double vi, double *vd0, double *vd2, int32_t axes));
//gj_instance3d *gauss_jackson_instance3d(gj_kernel *gjk,double (*calc_vd2)(double vi, double vdx0, double vdy0, double vdz0));
int gauss_jackson_setstep(gj_instance *gji, double vi, double *vd0, double *vd1, double *vd2, int32_t istep);
int gauss_jackson_getstep(gj_instance *gji, double *vi, double *vd0, double *vd1, double *vd2, int32_t istep);
gj_step *gauss_jackson_step(gj_kernel *kern);
void gauss_jackson_destep(gj_kernel *kern, gj_step *step);
void gauss_jackson_preset(gj_instance *gji);
void gauss_jackson_extrapolate(gj_instance *gji, double target);

double fixangle(double angle);
double actan(double y, double x);
double fixprecision(double number, double precision);
uint16_t calc_crc16ccitt(uint8_t *buf, int size, bool lsb=true);
uint16_t calc_crc16ccitt_lsb(vector<uint8_t>buf);
uint16_t calc_crc16ccitt_msb(vector<uint8_t>buf);
class LsFit
{
private:

    struct fitelement
    {
        // Independent variable
        double x;
        // Double, rvector or quaternion dependent variable
        uvector y{};
    } ;


    // Number of elements in fit
    uint16_t element_cnt;
    // Number of axes (double, rvector, quaternion)
    uint16_t depth;
    // Order of fit
    uint32_t order;
    // base level subtracted from independent variable before fitting
    double basex;
    // Of size element_cnt
    deque<fitelement> var;

    vector< vector<double> > parms;

    void fit();

public:
    double meanx;
    uvector meany{};
    double stdevx;
    uvector stdevy{};
    // Minimum reasonable step in dependent variable
    double resolution;

    // constructors
//    LsFit();
//    LsFit(uint16_t element_cnt);
//    LsFit(uint16_t element_cnt, uint16_t ord);
    LsFit(uint16_t cnt=10, uint16_t ord=2);

    void initialize(uint16_t cnt=10, uint16_t ord=2);
    void update(double x, double y);
    void update(double x, rvector y);
    void update(double x, gvector y);
    void update(double x, quaternion y);
    void update(fitelement cfit, uint16_t dep);
    double lastx();
    double firstx();
    size_t size();
    double eval(double x);
    rvector evalrvector(double x);
    gvector evalgvector(double x);
    quaternion evalquaternion(double x);
    double slope(double x);
    rvector slopervector(double x);
    gvector slopegvector(double x);
    quaternion slopequaternion(double x);
    double accel(double x);
    rvector accelrvector(double x);
    gvector accelgvector(double x);
    quaternion accelquaternion(double x);
    double jerk(double x);
    rvector jerkrvector(double x);
    gvector jerkgvector(double x);
    quaternion jerkquaternion(double x);
    vector < vector <double> > getparms();
    double getbasex();
};



#endif