pathfillet.h

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */
#ifndef PATHFILLET_H
#define PATHFILLET_H

#include <ipathalgorithm.h>

class PATHPLANNER_EXPORT PathFillet : public IPathAlgorithm
{
    Q_OBJECT

public:
    explicit PathFillet(QObject *parent = nullptr);

    virtual bool verifyPath(FlightDataModel *model, QString &err);

    virtual bool processPath(FlightDataModel *model);

    virtual bool configure(QWidget *callingUi = nullptr);

private:
    double fillet_radius;

    FlightDataModel *new_model;

private:
    enum arc_center_results { CENTER_FOUND, COINCIDENT_POINTS, INSUFFICIENT_RADIUS };

    // Private functions

    void setNewWaypoint(int index, float *pos, float velocity, float curvature);

    int addNonCircleToSwitchingLoci(float position[3], float finalVelocity, float radius,
                                    int index);

    float VectorMagnitude(float *);

    double VectorMagnitude(double *);

    float circular_modulus_rad(float err);

    enum arc_center_results find_arc_center(float start_point[2], float end_point[2], float radius,
                                            float center[2], bool clockwise, bool minor);

    float measure_arc_rad(float oldPosition_NE[2], float newPosition_NE[2], float arcCenter_NE[2]);

    float angle_between_2d_vectors(float a[2], float b[2]);
};

#endif // PATHFILLET_H