├── trajectory-app
    ├── trajectory-app
    │   ├── include
    │   │   ├── file.h
    │   │   ├── sliders.h
    │   │   ├── optimization.h
    │   │   ├── tangent.hpp
    │   │   ├── spline.hpp
    │   │   ├── interpolation.h
    │   │   ├── usb.hpp
    │   │   ├── sharedmemory.hpp
    │   │   ├── file.cpp
    │   │   ├── tangent.cpp
    │   │   ├── spline.cpp
    │   │   ├── sharedmemory.cpp
    │   │   ├── sliders.cpp
    │   │   ├── Regresion.h
    │   │   ├── usb.cpp
    │   │   ├── optimization.cpp
    │   │   ├── interpolation.cpp
    │   │   ├── port.h
    │   │   ├── matrix.h
    │   │   ├── spline.h
    │   │   └── vvector.h
    │   ├── main.h
    │   ├── trajectory-app.pro
    │   └── main.cpp
    └── README.md
├── .gitignore
├── lidar-app
    ├── lidar-app
    │   ├── include
    │   │   ├── def.hpp
    │   │   ├── sharedmemory.hpp
    │   │   ├── process.hpp
    │   │   ├── urg.hpp
    │   │   ├── sharedmemory.cpp
    │   │   ├── process.cpp
    │   │   └── urg.cpp
    │   ├── lidar-app.pro
    │   └── main.cpp
    └── README.md
└── README.md


/trajectory-app/trajectory-app/include/file.h:
--------------------------------------------------------------------------------
 1 | #ifndef FILE_H
 2 | #define FILE_H
 3 | 
 4 | #include "main.h"
 5 | 
 6 | void points_to_mat(Mat &point_mat, string path);
 7 | void save_point_to_file(vector<Point> point_vector,String path);
 8 | 
 9 | 
10 | #endif // FILE_H
11 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Eclipse
 2 | .classpath
 3 | .settings/
 4 | .metadata/
 5 | .recommenders/
 6 | .jar
 7 | .save 
 8 | 
 9 | # Maven
10 | log/
11 | target/
12 | 
13 | # OpenCV
14 | build/
15 | build-d/
16 | build-p/
17 | build-r/
18 | *.user*
19 | *.pro*
20 | *.autosave*
21 | *.md~
22 | *.cpp~
23 | *.h~
24 | *.txt~
25 | 
26 | # STM32
27 | *.o
28 | *.elf
29 | *.bin
30 | *.hex
31 | *.map
32 | *.lst
33 | .*.un~
34 | .*.swp
35 | *.bak
36 | 
37 | 
38 | # Files containg private data
39 | **/remotedebug.xml
40 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/sliders.h:
--------------------------------------------------------------------------------
 1 | #include <opencv2/opencv.hpp>
 2 | #include "spline.hpp"
 3 | #include "main.h"
 4 | #include <vector>
 5 | 
 6 | extern int left_slider[5];
 7 | extern int right_slider[5];
 8 | extern int rect_slider[5];
 9 | 
10 | void left_trackbar( int, void* );
11 | void righ_trackbar( int, void* );
12 | void init_trackbars();
13 | 
14 | void init_rect_trackbars(String name);
15 | void rect_trackbars( int, void* );
16 | void points_preview(std::vector<Point>points,Mat& frame,const Scalar& col);
17 | 
18 | 
19 | 
20 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/optimization.h:
--------------------------------------------------------------------------------
 1 | #include "main.h"
 2 | #include <opencv2/opencv.hpp>
 3 | 
 4 | void points_to_mat(Mat& point_mat,vector<Point> points); //get from shared memory
 5 | void one_line_planner(spline_t r_line,int offset, spline_t& path_line);
 6 | void two_line_planner(spline_t y_line, spline_t w_line, int offset, spline_t& path_line);
 7 | void new_optimization(vector<Point> pts_vector,spline_t& spl,Mat &preview);
 8 | 
 9 | //lidar version
10 | void two_wall_planner(spline_t left_spline,spline_t right_spline, spline_t &path_line);
11 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/def.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | //#define DEBUG_MODE
 4 | //#define VERBOSE_MODE
 5 | //#define SHM_TEST
 6 | #define FPS_COUNT
 7 | 
 8 | #define FRAME_TIME 1
 9 | #define FPS_AMOUNT 250
10 | 
11 | #define WIDTH 1600
12 | #define HEIGHT 1800
13 | 
14 | #ifndef DEBUG_MODE
15 | #undef VERBOSE_MODE
16 | #endif
17 | 
18 | #include <opencv2/core/core.hpp>
19 | #include <opencv2/highgui/highgui.hpp>
20 | #include <opencv2/imgproc/imgproc.hpp>
21 | 
22 | struct LidarReading
23 | {
24 |     long *data;
25 |     std::vector<cv::Point> pos;
26 |     std::vector<double> angle;
27 | };
28 | 
29 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/tangent.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once 
 2 | #include <opencv2/opencv.hpp>
 3 | #include "spline.hpp"
 4 | #include "interpolation.h"
 5 | #include <cmath>
 6 | #include "main.h"
 7 | 
 8 | 
 9 | using namespace cv;
10 | 
11 | class tangent{
12 |   public:
13 |     double a;
14 |     double b;
15 |     double angle_deg;
16 |     int tangent_point;
17 |     spline_t tangent_spline;
18 | 
19 |     //poly3_interp poly3;
20 |     //exp_interp exp_curve;
21 | 
22 |     tangent();
23 |     void draw(Mat& frame,const Scalar& col);
24 |     void angle();
25 |     void calculate(spline_t spl, int x);
26 | 
27 |     void calculate_poly2( poly2_interp poly2, int x);
28 |     void calculate_exp(exp_interp exp_fit, int x);
29 | 
30 | 
31 | };
32 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/spline.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <stdio.h>
 4 | #include <cstdio>
 5 | #include <vector>
 6 | #include <iostream>
 7 | #include "spline.h"
 8 | #include <opencv2/opencv.hpp>
 9 | 
10 | 
11 | using namespace cv;
12 | using namespace tk;
13 | 
14 | class spline_t
15 | {
16 | public:
17 |     tk::spline spline;
18 |     std :: vector<double> X;
19 |     std :: vector<double> Y;
20 | 
21 |     spline_t();
22 |     void add_point();
23 |     void delete_point();
24 |     void fit_vector(int points_num);
25 |     void set_point_value(int num,double x, double y);
26 | 
27 |     void set_spline(std::vector<Point> vec, bool qubic); //wektor musi być posortowany po x
28 |     void draw(cv::Mat& frame,const cv::Scalar& col);
29 | };
30 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/main.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAIN_H
 2 | #define MAIN_H
 3 | 
 4 | 
 5 | #include "include/spline.hpp"
 6 | #include "include/sliders.h"
 7 | #include "include/tangent.hpp"
 8 | #include "include/sharedmemory.hpp"
 9 | #include "include/usb.hpp"
10 | #include "include/optimization.h"
11 | #include "include/file.h"
12 | 
13 | 
14 | #include <numeric>
15 | #include <algorithm>
16 | #include <fstream>
17 | #include <string>
18 | #include <iostream>
19 | #include <opencv2/opencv.hpp>
20 | #include <vector>
21 | 
22 | 
23 | 
24 | //#define RACE_MODE //without display
25 | #define DEBUG_MODE //data display
26 | //#define TEST_MODE //load data from file
27 | 
28 | #define FPS_COUNT
29 | #define FPS_AMOUNT 250
30 | 
31 | #define LIDAR_MAT_HEIGHT 900
32 | #define LIDAR_MAT_WIDTH 1600
33 | 
34 | extern int number_of_rec_cols;
35 | extern int number_of_rec_raws;
36 | 
37 | #endif // MAIN_H
38 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/sharedmemory.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <iostream>
 4 | #include <string.h>
 5 | #include <sys/types.h>
 6 | #include <sys/ipc.h>
 7 | #include <sys/shm.h>
 8 | 
 9 | #include <opencv2/highgui/highgui.hpp>
10 | #include <opencv2/imgproc/imgproc.hpp>
11 | 
12 | #include <include/def.hpp>
13 | 
14 | class SharedMemory
15 | {
16 |     int mem_id;
17 |     int mem_size;
18 |     key_t  key;
19 |     uint32_t *shared_variable;
20 |     uint32_t *end_of_variable;
21 | 
22 |     // Methods
23 | public:
24 |     SharedMemory(key_t  k = 50000, int m_size = 12000);
25 |     bool init();
26 |     void push_point_data(std::vector<cv::Point> &points);
27 |     void pull_points_data(cv::Mat &out, cv::Scalar color);
28 |     void push_additional_data(cv::Point lidar_pos, int scale, cv::Point gap_pos_left, cv::Point gap_pos_right);
29 |     void pull_additional_data();
30 | 
31 |     void close();
32 | };
33 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/process.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <iostream>
 4 | #include <math.h>
 5 | 
 6 | #include <opencv2/core/core.hpp>
 7 | #include <opencv2/highgui/highgui.hpp>
 8 | #include <opencv2/imgproc/imgproc.hpp>
 9 | 
10 | #include <include/def.hpp>
11 | 
12 | class Process
13 | {
14 | private:
15 |     std::vector<cv::Point> rejected_points;
16 |     std::vector<cv::Point> enemies_points;
17 |     std::vector<cv::Point> trash_points;
18 | 
19 | public:
20 |     int max_dist = 35;
21 |     int thresh_simplify = 7;
22 |     std::vector<cv::Point> left_points;
23 |     std::vector<cv::Point> right_points;
24 |     cv::Point gap_pos_left;
25 |     cv::Point gap_pos_right;
26 | 
27 | public:
28 |     Process();
29 |     void simplify_data(LidarReading &points);
30 |     void split_poins(LidarReading &points);
31 |     void split_poins_equally(LidarReading &points);
32 |     void search_gap();
33 |     void filter_enemies();
34 |     void draw_data(cv::Mat &out);
35 | };
36 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/interpolation.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <opencv2/opencv.hpp>
 4 | #include "include/vvector.h"
 5 | #include <vector>
 6 | #include <iostream>
 7 | 
 8 | 
 9 | using namespace std;
10 | using namespace cv;
11 | 
12 | class poly2_interp{
13 | 
14 | public:
15 |     double a;
16 |     double b;
17 |     double c;
18 | 
19 |     poly2_interp();
20 |     void calculate_coef(vector<Point> pts);
21 |     void draw(Mat& frame,const Scalar& col);
22 | 
23 | };
24 | 
25 | class poly3_interp{
26 | 
27 | public:
28 |     double a;
29 |     double b;
30 |     double c;
31 |     double d;
32 | 
33 |     poly3_interp();
34 |     void calculate_3coef(vector<Point> pts);
35 |     void draw(Mat& frame,const Scalar& col);
36 | 
37 | };
38 | 
39 | class exp_interp{
40 | 
41 | public:
42 |     double A;
43 |     double B;
44 | 
45 |     exp_interp();
46 |     void calculate_exp(vector<Point> pts,uint32_t len);
47 |     void draw(Mat& frame,const Scalar& col);
48 | 
49 | 
50 | };
51 | 
52 | 
53 | 
54 | 
55 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/usb.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <iostream>
 4 | #include <fcntl.h>
 5 | #include <unistd.h>
 6 | #include <termios.h>
 7 | #include <cstring>
 8 | #include <string>
 9 | #include <vector>
10 | 
11 | 
12 | struct data_container
13 | {
14 |     const uint8_t start = 0xFF;
15 |     const uint8_t code = 0x40;
16 |     uint8_t length = 14;
17 |     const uint8_t stop = 0xFE;
18 |     uint8_t data[14];
19 | };
20 | 
21 | class USB_STM
22 | {
23 |     char portname[13] = "/dev/ttyACM0";
24 |     unsigned char to_send_packed[22];
25 |     int fd; //file descriptor
26 | public:
27 |     USB_STM();
28 |     int init(int speed = B115200);
29 |     void uint32_to_char_tab(uint32_t input, unsigned char *output);
30 |     void char_tab_to_uint32(unsigned char input[], uint32_t *output);
31 |     void send_buf(data_container &to_send);
32 |     void read_buf(int buf_size,float& velocity, uint16_t &tf_mini,uint8_t &taranis_3_pos,uint8_t &taranis_reset_gear,uint8_t& stm_reset,uint8_t& lights);
33 |     void data_pack(uint16_t velo,uint16_t ang,data_container *container);
34 | };
35 | 
36 | 
37 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/sharedmemory.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <opencv2/highgui/highgui.hpp>
 4 | #include <opencv2/imgproc/imgproc.hpp>
 5 | 
 6 | #include <iostream>
 7 | #include <string.h>
 8 | #include <sys/types.h>
 9 | #include <sys/ipc.h>
10 | #include <sys/shm.h>
11 | 
12 | #define MEMSIZE 16384
13 | /*
14 | uint32_t velocity = 500;
15 | uint32_t servo_angle = 90000;
16 | unsigned char reset_STM = 0;
17 | unsigned char red_light_detected = 0;
18 | unsigned char green_light_detected = 0;
19 | unsigned char stop_line_detected = 0;
20 | uint32_t stop_distance = 0;
21 | */
22 | 
23 | class SharedMemory
24 | {
25 |     int mem_id;
26 |     int mem_size;
27 |     key_t  key;
28 |     uint32_t *shared_variable;
29 |     uint32_t *end_of_variable;
30 | 
31 | public:
32 |     SharedMemory(key_t  k = 50000, int m_size = 100000);
33 |     bool init();
34 |     bool get_access();
35 |     void push_data(uint8_t taranis_3_pos, uint8_t taranis_reset_gear,uint8_t stm_reset);
36 |     void pull_line_data(std::vector<cv::Point> &y_vector,std::vector<cv::Point> &w_vector,std::vector<cv::Point> &c_vector);
37 |     void pull_lidar_data(std::vector<cv::Point>&l_vector);
38 |     void pull_add_data(std::vector<uint32_t>&data);
39 |     void push_signal(uint32_t signal);
40 |     bool pull_signal();
41 | 
42 |     void close();
43 |     uint32_t get_lenght();
44 | };
45 | 
46 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/urg.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <iostream>
 4 | 
 5 | #include <opencv2/core/core.hpp>
 6 | #include <opencv2/highgui/highgui.hpp>
 7 | #include <opencv2/imgproc/imgproc.hpp>
 8 | 
 9 | #include <urg_c/urg_sensor.h>
10 | #include <urg_c/urg_utils.h>
11 | 
12 | #include <include/def.hpp>
13 | 
14 | class URG
15 | {
16 |     // Handler for lidar operations
17 |     urg_t urg;
18 | 
19 |     int n;
20 |     int data_size;
21 |     int angle_offset;
22 | 
23 | public:
24 |     int scale = 2;
25 |     int cut_left = 0;
26 |     int cut_right = 450;
27 |     int closest = 90;
28 |     int farthest = 2000;
29 |     int x_left = 0;
30 |     int x_right = WIDTH;
31 |     int y_up = 0;
32 |     int y_down = 875;
33 |     int thresh = 10;
34 | 
35 |     LidarReading raw_data;
36 |     LidarReading filtered_data[3];
37 | 
38 | public:
39 |     URG();
40 |     int init();
41 |     int read();
42 |     void polar_to_cartesian();
43 |     void filter_angle(LidarReading &input, LidarReading &output);
44 |     void filter_distance(LidarReading &input, LidarReading &output);
45 |     void filter_X(LidarReading &input, LidarReading &output);
46 |     void filter_Y(LidarReading &input, LidarReading &output);
47 |     void filter_single_points(LidarReading &input, LidarReading &output);
48 |     void draw_data_raw(cv::Mat &frame);
49 |     void draw_data_filtered(cv::Mat &frame, LidarReading &filtered);
50 |     void draw_boundaries(cv::Mat &frame, LidarReading &input);
51 | 
52 |     void close();
53 | };
54 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/file.cpp:
--------------------------------------------------------------------------------
 1 | #include "include/file.h"
 2 | 
 3 | void points_to_mat(Mat &point_mat, string path)
 4 | {
 5 |     vector<Point> test_set;
 6 |     string line1;
 7 |     string line2;
 8 | 
 9 |     ifstream myfile (path);
10 |     if (myfile.is_open())
11 |      {
12 |        while ( getline (myfile,line1) )
13 |        {
14 |            getline (myfile,line2);
15 |         test_set.push_back(Point(stoi(line1),stoi(line2)));
16 |        }
17 |        myfile.close();
18 |      }
19 | 
20 |      else
21 |         cout << "Unable to open file";
22 | 
23 | 
24 |     for (int i=0;i<(int)test_set.size();i++){
25 |              Point pom;
26 |              Vec3b  color;
27 |              pom.y = test_set[i].y;
28 |              pom.x = test_set[i].x;
29 | 
30 |              color = point_mat.at<Vec3b>(pom);
31 |              color.val[0] = 255;
32 |              color.val[1] = 255;
33 |              color.val[2] = 255;
34 |              point_mat.at<Vec3b>(pom) = color;
35 |       }
36 | }
37 | void save_point_to_file(vector<Point> point_vector,String path)
38 | {
39 |     ofstream plik ("/home/selfie/Desktop/cpp-opencv-app/trajectory-planning/trajectory-planning/krzywa_prosta1.txt");
40 | 
41 |     if(plik.good()==true)
42 |     {
43 |             for(int i=0;i<point_vector.size();i++)
44 |             {
45 |                 plik<<point_vector[i].x;
46 |                 plik<<endl;
47 |                 plik<<point_vector[i].y;
48 |                 plik<<endl;
49 |             }
50 |        plik.close();
51 |     }
52 | 
53 | }
54 | 
55 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/trajectory-app.pro:
--------------------------------------------------------------------------------
 1 | TEMPLATE = app
 2 | CONFIG += console c++11
 3 | CONFIG -= app_bundle
 4 | CONFIG -= qt
 5 | 
 6 | QMAKE_CXXFLAGS += -std=c++11
 7 | 
 8 | HEADERS += \
 9 |     include/tangent.hpp \
10 |     include/spline.hpp \
11 |     include/spline.h \
12 |     include/sliders.h \
13 |     include/sharedmemory.hpp \
14 |     include/vvector.h \
15 |     include/usb.hpp \
16 |     main.h \
17 |     include/matrix.h \
18 |     include/optimization.h \
19 |     include/file.h
20 | 
21 | 
22 | SOURCES += main.cpp \
23 |     include/tangent.cpp \
24 |     include/spline.cpp  \
25 |     include/sharedmemory.cpp \
26 |     include/usb.cpp \
27 |     include/sliders.cpp \
28 |     include/optimization.cpp \
29 |     include/file.cpp
30 | 
31 | # OpenCV
32 | INCLUDEPATH += /usr/local/include/opencv
33 | LIBS += -L/usr/local/lib -lopencv_stitching -lopencv_superres -lopencv_videostab -lopencv_xfeatures2d -lopencv_shape -lopencv_img_hash -lopencv_dpm -lopencv_optflow -lopencv_rgbd -lopencv_xphoto -lopencv_ximgproc -lopencv_line_descriptor -lopencv_fuzzy -lopencv_saliency -lopencv_stereo -lopencv_reg -lopencv_bgsegm -lopencv_freetype -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_bioinspired -lopencv_aruco -lopencv_ccalib -lopencv_calib3d -lopencv_face -lopencv_tracking -lopencv_datasets -lopencv_text -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_dnn -lopencv_video -lopencv_plot -lopencv_photo -lopencv_ml -lopencv_xobjdetect -lopencv_imgcodecs -lopencv_objdetect -lopencv_imgproc -lopencv_surface_matching -lopencv_flann -lopencv_core
34 | 
35 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/tangent.cpp:
--------------------------------------------------------------------------------
 1 | #include "tangent.hpp"
 2 | 
 3 | 
 4 | tangent::tangent()
 5 | {
 6 | 
 7 | 
 8 | }
 9 | 
10 | void tangent::calculate(spline_t spl, int x)
11 | {   tangent_spline=spl;
12 |     tangent_point=x;
13 |     a = spl.spline.deriv(1,x);
14 |     b =-a*x+spl.spline(x);
15 | 
16 | }
17 | 
18 | void tangent::draw(Mat& frame,const Scalar& col)
19 | {
20 | 
21 |     for (int i=0;i<frame.rows;i++){
22 |              Point pom;
23 |              Vec3b  color;
24 |              pom.y = i;
25 |              pom.x = a*i+b;
26 | 
27 |              if(pom.x>frame.cols)
28 |                  pom.x = frame.cols;
29 |              else if(pom.x<0)
30 |                  pom.x=0;
31 | 
32 |              color = frame.at<Vec3b>(pom);
33 |              color.val[0] = col[0];
34 |              color.val[1] = col[1];
35 |              color.val[2] = col[2];
36 |              frame.at<Vec3b>(pom) = color;
37 |       }
38 | }
39 | void tangent::angle()
40 | {
41 | 
42 |     //angle_deg = atan2( (a*LIDAR_MAT_HEIGHT+b) - a*tangent_point+b,LIDAR_MAT_HEIGHT - tangent_point);
43 |     angle_deg = atan(a);
44 | 
45 |     if (angle_deg<0)
46 |        angle_deg =  - angle_deg * 90 / M_PI;
47 |     else
48 |        angle_deg = (-angle_deg * 90 / M_PI);
49 | }
50 | 
51 | void tangent::calculate_poly2(poly2_interp poly2, int x){
52 | 
53 |     a = 2*poly2.a*x +poly2.b;
54 |     b =-a*x + poly2.a*x*x+poly2.b*x+poly2.c;
55 | 
56 | }
57 | 
58 | void tangent::calculate_exp(exp_interp exp_fit, int x){
59 | 
60 |     a = exp_fit.A*exp_fit.B*exp(exp_fit.B*x);
61 |     b =-a*x + exp_fit.A*exp(exp_fit.B*x);
62 | 
63 | }
64 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/lidar-app.pro:
--------------------------------------------------------------------------------
 1 | TEMPLATE = app
 2 | CONFIG += console c++11
 3 | CONFIG -= app_bundle
 4 | CONFIG -= qt
 5 | 
 6 | QMAKE_CXXFLAGS += -std=c++11
 7 | 
 8 | SOURCES += main.cpp \
 9 |     include/urg.cpp \
10 |     include/sharedmemory.cpp \
11 |     include/process.cpp
12 | 
13 | HEADERS += \
14 |     include/def.hpp \
15 |     include/urg.hpp \
16 |     include/sharedmemory.hpp \
17 |     include/process.hpp
18 | 
19 | # OpenCV
20 | INCLUDEPATH += /usr/local/include
21 | LIBS += -L/usr/local/lib -lopencv_stitching -lopencv_videostab -lopencv_superres -lopencv_hfs -lopencv_dnn_objdetect -lopencv_freetype -lopencv_img_hash -lopencv_face -lopencv_photo -lopencv_structured_light -lopencv_line_descriptor -lopencv_bioinspired -lopencv_tracking -lopencv_reg -lopencv_saliency -lopencv_rgbd -lopencv_optflow -lopencv_ximgproc -lopencv_aruco -lopencv_xfeatures2d -lopencv_shape -lopencv_ccalib -lopencv_dpm -lopencv_surface_matching -lopencv_stereo -lopencv_xphoto -lopencv_fuzzy -lopencv_bgsegm -lopencv_calib3d -lopencv_video -lopencv_datasets -lopencv_text -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_dnn -lopencv_ml -lopencv_flann -lopencv_plot -lopencv_xobjdetect -lopencv_imgcodecs -lopencv_objdetect -lopencv_phase_unwrapping -lopencv_imgproc -lopencv_core
22 | 
23 | # URG
24 | unix:!macx: LIBS += -L$$PWD/../../../../Libraries/URG-Network/urg_library-1.2.2/src/ -lurg_c
25 | INCLUDEPATH += $$PWD/../../../../Libraries/URG-Network/urg_library-1.2.2/include/c
26 | DEPENDPATH += $$PWD/../../../../Libraries/URG-Network/urg_library-1.2.2/include/c
27 | unix:!macx: PRE_TARGETDEPS += $$PWD/../../../../Libraries/URG-Network/urg_library-1.2.2/src/liburg_c.a
28 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/spline.cpp:
--------------------------------------------------------------------------------
 1 | #include "spline.hpp"
 2 | 
 3 | 
 4 | spline_t::spline_t()
 5 | {
 6 | 
 7 | 
 8 | }
 9 | 
10 | void spline_t::add_point()
11 | {
12 | 
13 |    X.push_back(1);
14 |    Y.push_back(1);
15 | }
16 | 
17 | void spline_t::delete_point()
18 | {
19 | 
20 |    X.pop_back();
21 |    Y.pop_back();
22 | }
23 | 
24 | void spline_t::fit_vector(int points_num)
25 | {   if(!X.empty()){
26 |         if(X.size()< points_num)
27 |         {
28 |             for(int i=0;i<points_num-X.size();i++)
29 |                 add_point();
30 | 
31 |         }
32 | 
33 |         else if(X.size()>points_num)
34 |         {
35 |             for(int i=0;i<X.size()-points_num;i++)
36 |                 delete_point();
37 |         }
38 |     }
39 |     else
40 |     {
41 |         for(int i=0;i<points_num;i++)
42 |             add_point();
43 |     }
44 | }
45 | 
46 | void spline_t::set_point_value(int num,double x, double y)
47 | {
48 |     X[num]=x;
49 |     Y[num]=y;
50 | }
51 | 
52 | void spline_t::set_spline(std::vector<cv::Point> vec, bool qubic)
53 | {
54 |     X.clear();
55 |     Y.clear();
56 | 
57 |     for(int i=vec.size()-1;i>-1;i--)
58 |     {   X.push_back(vec[i].y);
59 |         Y.push_back(vec[i].x);
60 |     }
61 | 
62 |     spline.set_points(X,Y,qubic);
63 | }
64 | 
65 | void spline_t::draw(Mat& frame,const Scalar& col)
66 | {
67 | 
68 |     for (int i=0;i<frame.rows;i++)
69 |     {
70 |         Point pom;
71 |         Vec3b  color;
72 |       pom.y = i;
73 |       pom.x=spline(i);//a*i*i+b*i+c+200;
74 | 
75 |       color = frame.at<Vec3b>(pom);
76 |       color.val[0] = col[0];
77 |       color.val[1] = col[1];
78 |       color.val[2] = col[2];
79 |       frame.at<Vec3b>(pom) = color;
80 |      }
81 | }
82 | 
83 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Selfie-autonomous-car
 2 | 
 3 | *Selfie* is the student project of autonomous cars. Vehicles based on 1:10 scale RC cars are customized to be able to operate autonomously in simulated road environments. They are equipped with camera, computer vision computing unit, controller and set of sensors like magnetic encoders, distance sensors and IMU.
 4 | 
 5 | ## Version 2.0 - *Selfie* 2.0
 6 | 
 7 | <img align="right" height="400" src="https://user-images.githubusercontent.com/16399071/44856111-e7c9af00-ac6c-11e8-8f9c-d272c861cc84.PNG">
 8 | 
 9 | Software released as version 2.0 was onboarded in the brand new vehicle built for International Autonomous Robot Racing Competition ([IARRC](https://www.robotcompetition.ca/iarrc2018/)) 2018 in Waterloo, Canada. *Selfie* won first prize outperforming in both Drag and Circut Races. The vehicle was able to reach the speed of 12 m/s in stable Drag Race. Picture presents the car in development (without bodywork).
10 | 
11 | The IARRC competiton consists of project presentation part (written report, video, oral presentation) and racing part. Racing events are running outdoors. Track is marked with white and yellow lines and cones. In *Drag Race* cars are competing on 60 meters of straight track. In *Curcuit Race* cars have to drive about 1000 meters (3 laps of about 330 meters).
12 | 
13 | The main challenges to overcome were start up light detection, high-speed line detection and trajectory corrections, collision avoidance and  varying lumininance conditions (areas covered with sun light or fully or partially shadowed).
14 | 
15 | 
16 | ### Description of content
17 | 
18 | #### cpp-opencv-app
19 | two apps for various tasks, placed respectively in /CIRCUIT/ and /DRAG/ directories. Used to run at platform equipped with Intel I3, mostly using opencv
20 | 
21 | #### stm32-atollic
22 | Vehicle controller application for FreeRTOS (Real-Time Operating System). Application designed to run on STM32F7 (AnyFC F7 board).
23 | 
24 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/sharedmemory.cpp:
--------------------------------------------------------------------------------
  1 | #include "sharedmemory.hpp"
  2 | 
  3 | SharedMemory::SharedMemory(key_t k, int m_size)
  4 | {
  5 |     key = k;
  6 |     mem_size = m_size;
  7 | 
  8 | }
  9 | 
 10 | bool SharedMemory::init()
 11 | {
 12 | 
 13 |     mem_id = shmget(key, mem_size, IPC_CREAT | 0666);
 14 |     if(mem_id < 0)
 15 |     {
 16 |         std::cout << "Error getting memmory access at given key" << std::endl;
 17 |         return 0;
 18 |     }
 19 | 
 20 | 
 21 |     shared_variable = (uint32_t*) shmat(mem_id, NULL, 0);
 22 | 
 23 |     if(shared_variable == (uint32_t*) -1)
 24 |     {
 25 |         std::cout << "Error trying to read memory for first time" << std::endl;
 26 |         return 0;
 27 |     }
 28 | 
 29 |     return 1;
 30 | }
 31 | 
 32 | bool SharedMemory::get_access()
 33 | {
 34 | 
 35 |     mem_id = shmget(key, mem_size, 0666);
 36 |     if(mem_id < 0)
 37 |     {
 38 |         std::cout << "Error getting memmory access at given key" << std::endl;
 39 |         return 0;
 40 |     }
 41 | 
 42 | 
 43 |     shared_variable = (uint32_t*) shmat(mem_id, NULL, 0);
 44 | 
 45 |     if(shared_variable == (uint32_t*) -1)
 46 |     {
 47 |         std::cout << "Error trying to read memory for first time" << std::endl;
 48 |         return 0;
 49 |     }
 50 | 
 51 |     return 1;
 52 | }
 53 | 
 54 | void SharedMemory::push_data(uint8_t taranis_3_pos, uint8_t taranis_reset_gear,uint8_t stm_reset)
 55 | {
 56 | 
 57 |     uint32_t tmp[7];
 58 | 
 59 |     tmp[0] = 6;
 60 |     tmp[1] = shared_variable[1];
 61 |     tmp[2] = shared_variable[2];
 62 |     tmp[3] = taranis_reset_gear;
 63 |     tmp[4] = stm_reset;
 64 |     tmp[5] = taranis_3_pos;
 65 |     tmp[6] = 1;
 66 | 
 67 |     // Copy data to shm
 68 |     memcpy(shared_variable, &tmp[0], 28);
 69 | }
 70 | 
 71 | void SharedMemory::pull_lidar_data(std::vector<cv::Point>&l_vector)
 72 | {
 73 |     uint32_t l_length = shared_variable[0];
 74 |     if(l_length>0)
 75 |     {
 76 |         for(int i=0;i<l_length;i++)
 77 |         {
 78 |             l_vector.push_back(cv::Point(shared_variable[2*i+1],shared_variable[2*i+2]));
 79 |         }
 80 |     }
 81 | }
 82 | 
 83 | void SharedMemory::pull_add_data(std::vector<uint32_t>&data)
 84 | {
 85 |     if(mem_id>0)
 86 |     {
 87 |         if(shared_variable[0] == 3)
 88 |         {
 89 |             for(int i=0;i<7;i++)
 90 |             {
 91 |                 data[i]=shared_variable[i+1];
 92 |             }
 93 |         }
 94 |     }
 95 | }
 96 | 
 97 | void SharedMemory::close()
 98 | {
 99 |     std::string command = "ipcrm -M ";
100 |     command += std::to_string(key);
101 |     system(command.c_str());
102 | }
103 | uint32_t SharedMemory::get_lenght()
104 | {
105 |     return shared_variable[0];
106 | }
107 | 
108 | void SharedMemory::push_signal(uint32_t signal)
109 | {
110 |     uint32_t tmp[1] = {signal};
111 | 
112 |     // Copy data to shm
113 |     memcpy(shared_variable, tmp, 4);
114 | }
115 | bool SharedMemory::pull_signal()
116 | {
117 |     if(shared_variable[0])
118 |         return true;
119 |     return false;
120 | }
121 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/sliders.cpp:
--------------------------------------------------------------------------------
  1 | #include "sliders.h"
  2 | using namespace std;
  3 | using namespace cv;
  4 | 
  5 | const int left_slider_max = 4000;
  6 | const int right_slider_max = 640;
  7 | 
  8 | int left_slider[5] = {2000,0,283,284,285};
  9 | int right_slider[5]= {360,360,360,360,360};
 10 | 
 11 | const int max_num = 100;
 12 | const int max_off = 200;
 13 | const int max_derive = LIDAR_MAT_HEIGHT;
 14 | const int max_pixel = 100;
 15 | 
 16 | int rect_slider[5]={72,39,20,LIDAR_MAT_HEIGHT,0};
 17 | 
 18 | 
 19 | void left_trackbar( int, void* )
 20 | {
 21 |     for(int i=0;i<5;i++)
 22 |    left_slider[i]=left_slider[i];
 23 | 
 24 | 
 25 | }
 26 | void right_trackbar(int, void*)
 27 | {
 28 |    for(int i=0;i<5;i++)
 29 |        right_slider[i]=right_slider[i];
 30 | }
 31 | 
 32 | void init_trackbars()
 33 | {
 34 |        namedWindow("Parametry", WINDOW_NORMAL);
 35 |        createTrackbar("predkosc","Parametry", &left_slider[0], left_slider_max, left_trackbar );
 36 |        createTrackbar( "kat","Parametry", &left_slider[1], left_slider_max, left_trackbar );
 37 | 
 38 |        //namedWindow("right border line", WINDOW_NORMAL);
 39 |       // for(int i=0;i<5;i++)
 40 |        //{
 41 | 
 42 |        //createTrackbar( std::to_string(i), "right border line", &right_slider[i], right_slider_max, right_trackbar );
 43 |        //}
 44 | }
 45 | 
 46 | void rect_trackbars( int, void* )
 47 | {
 48 |     for(int i=0;i<5;i++)
 49 |     rect_slider[i]=rect_slider[i];
 50 | 
 51 |     if(rect_slider[0]<8)
 52 |         rect_slider[0]=8;
 53 |     if(rect_slider[1]<8)
 54 |         rect_slider[1]=8;
 55 | }
 56 | 
 57 | void init_rect_trackbars(String name)
 58 | {
 59 | 
 60 | 
 61 |     createTrackbar("w", name, &rect_slider[0], max_num, rect_trackbars );
 62 |     createTrackbar("h", name, &rect_slider[1], max_num, rect_trackbars );
 63 |     createTrackbar("max pix", name, &rect_slider[2], max_pixel, rect_trackbars );
 64 |     createTrackbar("deriv point", name, &rect_slider[3], max_derive, rect_trackbars );
 65 |     createTrackbar("offset", name, &rect_slider[4], max_off, rect_trackbars );
 66 | 
 67 | 
 68 | }
 69 | void poly_fill(Mat& frame,const Scalar& col,spline_t left, spline_t right,Mat& result)
 70 | {
 71 | 
 72 |     Point fillp[1][2*480];
 73 |     const Point* ppt[1] = { fillp[0] };
 74 |     int npt[] = { 2*480 };
 75 |     Mat poly = Mat::zeros( 480, 640, CV_8UC3 );
 76 |     Mat dst;
 77 | 
 78 |     for(int i=0;i<480;i++){
 79 |         fillp[0][i].y=i;
 80 |         fillp[0][i].x=left.spline(i);
 81 |     }
 82 |     for(int i =0;i<480;i++){
 83 |         fillp[0][2*480-i-1].y=i;
 84 |         fillp[0][2*480-i-1].x=right.spline(i);
 85 |     }
 86 | 
 87 | 
 88 |     fillPoly( poly,ppt, npt, 1, Scalar( col[0], col[1],col[2]), 4 ); //fill poly
 89 |     addWeighted(frame, 0.6, poly, 0.4, 0.0, result);
 90 | }
 91 | 
 92 | void points_preview(vector<Point>points,Mat& frame,const Scalar& col){
 93 | 
 94 | 
 95 |     for (int i=0;i<(int)points.size();i++){
 96 |         Vec3b color;
 97 |         Point pom;
 98 |         for(int j=0;j<10;j++)
 99 | 	{
100 |             pom.y = points[i].y;
101 |             pom.x = points[i].x;
102 | 
103 | 
104 |             color = frame.at<Vec3b>(pom);
105 |             color.val[0] = col[0];
106 |             color.val[1] = col[1];
107 |             color.val[2] = col[2];
108 |             frame.at<Vec3b>(pom) = color;
109 |          }
110 |     }
111 | 
112 | }
113 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/sharedmemory.cpp:
--------------------------------------------------------------------------------
  1 | #include "sharedmemory.hpp"
  2 | 
  3 | SharedMemory::SharedMemory(key_t k, int m_size)
  4 | {
  5 |     key = k;
  6 |     mem_size = m_size;
  7 | }
  8 | 
  9 | bool SharedMemory::init()
 10 | {
 11 |     mem_id = shmget(key, mem_size, IPC_CREAT | 0666);
 12 |     if(mem_id < 0)
 13 |     {
 14 |         std::cout << "Error getting memmory access at key: " << key
 15 |                   << ", and size of: " << mem_size << "bytes" << std::endl;
 16 |         return 0;
 17 |     }
 18 | 
 19 |     shared_variable = (uint32_t*) shmat(mem_id, NULL, 0);
 20 | 
 21 |     if(shared_variable == (uint32_t*) -1)
 22 |     {
 23 |         std::cout << "Error trying to read from shared memory for the first time" << std::endl;
 24 |         return 0;
 25 |     }
 26 | 
 27 |     return 1;
 28 | }
 29 | 
 30 | void SharedMemory::push_point_data(std::vector<cv::Point> &points)
 31 | {
 32 |     uint32_t j = 0;
 33 |     std::vector<uint32_t> tmp;
 34 | 
 35 |     tmp.push_back(0);
 36 | 
 37 |     // Pack data
 38 |     if(points.size() > 0)
 39 |     {
 40 |         for(uint32_t i = 0; i < points.size(); i++)
 41 |         {
 42 |             tmp.push_back(points[i].x);
 43 |             tmp.push_back(points[i].y);
 44 |             j+=2;
 45 |         }
 46 | 
 47 |         tmp[0] = j;
 48 |      }
 49 | 
 50 |     // Copy data to shm
 51 |     memcpy(shared_variable, &tmp[0], 4*(j+1));
 52 | }
 53 | 
 54 | void SharedMemory::pull_points_data(cv::Mat &out, cv::Scalar color)
 55 | {
 56 |     cv::Point tmp;
 57 |     std::vector<cv::Point> vector_points;
 58 | 
 59 |     std::cout << "Length: " << shared_variable[0] << std::endl;
 60 | 
 61 |     if(shared_variable[0])
 62 |     {
 63 |         for(uint32_t i = 1; i <= shared_variable[0]; i+=2)
 64 |         {
 65 |             std::cout << "X: " << shared_variable[i] << ", Y: " << shared_variable[i+1] << std::endl;
 66 | 
 67 |             tmp.x = shared_variable[i];
 68 |             tmp.y = shared_variable[i+1];
 69 |             vector_points.push_back(tmp);
 70 |         }
 71 |     }
 72 | 
 73 |     for(uint32_t i = 0; i < vector_points.size(); i++)
 74 |     {
 75 |         cv::circle(out, vector_points[i], 2, color, CV_FILLED, cv::LINE_AA);
 76 |     }
 77 | }
 78 | 
 79 | void SharedMemory::push_additional_data(cv::Point lidar_pos, int scale, cv::Point gap_pos_left, cv::Point gap_pos_right)
 80 | {
 81 |     uint32_t tmp[8];
 82 | 
 83 |     tmp[0] = 3;
 84 |     tmp[1] = lidar_pos.x;
 85 |     tmp[2] = lidar_pos.y;
 86 |     tmp[3] = scale;
 87 |     tmp[4] = gap_pos_left.x;
 88 |     tmp[5] = gap_pos_left.y;
 89 |     tmp[6] = gap_pos_right.x;
 90 |     tmp[7] = gap_pos_right.y;
 91 | 
 92 |     // Copy data to shm
 93 |     memcpy(shared_variable, &tmp[0], 32);
 94 | }
 95 | 
 96 | void SharedMemory::pull_additional_data()
 97 | {
 98 |     std::cout << "Length:        " << shared_variable[0] << std::endl;
 99 |     std::cout << "Lidar pos:     " << "[" << shared_variable[1] << "," << shared_variable[2] << "]" << std::endl;
100 |     std::cout << "Scale:         " << shared_variable[3] << std::endl;
101 |     std::cout << "Gap pos left:  " << "[" << shared_variable[4] << "," << shared_variable[5] << "]" << std::endl;
102 |     std::cout << "Gap pos right: " << "[" << shared_variable[6] << "," << shared_variable[7] << "]" << std::endl;
103 | }
104 | 
105 | void SharedMemory::close()
106 | {
107 |     std::string command = "ipcrm -M ";
108 |     command += std::to_string(key);
109 |     system(command.c_str());
110 | }
111 | 


--------------------------------------------------------------------------------
/lidar-app/README.md:
--------------------------------------------------------------------------------
 1 | # Selfie-autonomous-car
 2 | 
 3 | *Selfie* is the student project of autonomous cars. Vehicles based on 1:10 scale RC cars are customized to be able to operate autonomously in simulated road environments. They are equipped with camera, computer vision computing unit, controller and set of sensors like magnetic encoders, distance sensors and IMU.
 4 | 
 5 | This file contains only basic informations, more details will hopefully occur as Wiki in finite time :)
 6 | 
 7 | ## Version 1.0 - *Selfie*
 8 | 
 9 | <img align="right" height="300" src="https://user-images.githubusercontent.com/16399071/37375786-96f3a8d6-2721-11e8-87de-6ebe179637dd.JPG">
10 | 
11 | Software released as [version 1.0](https://github.com/perciax/Selfie-autonomous-car/tree/v1.0) in first ready-to-go robot *Selfie*. It was prepared specifically for [Carolo-Cup 2018](https://wiki.ifr.ing.tu-bs.de/carolocup/) competition. It is fullfilling all the competition requirements, so basically it is able to perform:
12 | - road lane tracking, 
13 | - collision avoiding,
14 | - overtaking maneuver
15 | - parallel parking
16 | - intersection handling
17 | - proper light signalization
18 | 
19 | ### Description of content
20 | 
21 | #### cpp-opencv-app
22 | Computer Vision application designed to run on NVidia Jetson / Odroid platform.
23 | 
24 | #### stm32-atollic
25 | Vehicle controller application for FreeRTOS (Real-Time Operating System). Aplication is designed to run on STM32F7 (AnyFC F7 board).
26 | 
27 | ## Version 2.0 - *Selfie* 2.0
28 | 
29 | <img align="right" height="400" src="https://user-images.githubusercontent.com/16399071/44856111-e7c9af00-ac6c-11e8-8f9c-d272c861cc84.PNG">
30 | 
31 | Software released as [version 2.0](https://github.com/perciax/Selfie-autonomous-car/tree/v2.0) was onboarded in the brand new vehicle built for International Autonomous Robot Racing Competition ([IARRC](https://www.robotcompetition.ca/iarrc2018/)) 2018 in Waterloo, Canada. *Selfie* won first prize outperforming in both Drag and Circut Races. The vehicle was able to reach the speed of 12 m/s in stable Drag Race. Picture presents the car in development (without bodywork).
32 | 
33 | The IARRC competiton consists of project presentation part (written report, video, oral presentation) and racing part. Racing events are running outdoors. Track is marked with white and yellow lines and cones. In *Drag Race* cars are competing on 60 meters of straight track. In *Curcuit Race* cars have to drive about 1000 meters (3 laps of about 330 meters).
34 | 
35 | The main challenges to overcome were start up light detection, high-speed line detection and trajectory corrections, collision avoidance and  varying lumininance conditions (areas covered with sun light or fully or partially shadowed).
36 | 
37 | ## How to contribute
38 | 
39 | 1. Fork this repo to your account.
40 | 
41 | 2. Clone repo from your account to local folder on your device.
42 | 
43 | 	`git clone https://github.com/your-login/Selfie-autonomous-car`
44 | 
45 | 3. Add new remote repo as "main".
46 | 
47 | 	`git remote add main https://github.com/perciax/Selfie-autonomous-car`
48 | 
49 | Now you should have 2 remote repos configured:
50 | - "main" pointing this repo,
51 | - "origin" pointing copy on you account.
52 | This could be checked by command git remote -v.
53 | 
54 | 4. Update your local verison.
55 | 
56 | 	`git pull main master`
57 | 
58 | 5. Make your changes.
59 | 
60 | 6. Push them to repo on you account.
61 | 
62 | 	`git push origin master`
63 | 
64 | 7. Make Pull Request on Github.
65 | 
66 | 8. Repeat steps from 4 to 7 everytime you want to contribute to this "main" repo.	
67 | 


--------------------------------------------------------------------------------
/trajectory-app/README.md:
--------------------------------------------------------------------------------
 1 | # Selfie-autonomous-car
 2 | 
 3 | *Selfie* is the student project of autonomous cars. Vehicles based on 1:10 scale RC cars are customized to be able to operate autonomously in simulated road environments. They are equipped with camera, computer vision computing unit, controller and set of sensors like magnetic encoders, distance sensors and IMU.
 4 | 
 5 | This file contains only basic informations, more details will hopefully occur as Wiki in finite time :)
 6 | 
 7 | ## Version 1.0 - *Selfie*
 8 | 
 9 | <img align="right" height="300" src="https://user-images.githubusercontent.com/16399071/37375786-96f3a8d6-2721-11e8-87de-6ebe179637dd.JPG">
10 | 
11 | Software released as [version 1.0](https://github.com/perciax/Selfie-autonomous-car/tree/v1.0) in first ready-to-go robot *Selfie*. It was prepared specifically for [Carolo-Cup 2018](https://wiki.ifr.ing.tu-bs.de/carolocup/) competition. It is fullfilling all the competition requirements, so basically it is able to perform:
12 | - road lane tracking, 
13 | - collision avoiding,
14 | - overtaking maneuver
15 | - parallel parking
16 | - intersection handling
17 | - proper light signalization
18 | 
19 | ### Description of content
20 | 
21 | #### cpp-opencv-app
22 | Computer Vision application designed to run on NVidia Jetson / Odroid platform.
23 | 
24 | #### stm32-atollic
25 | Vehicle controller application for FreeRTOS (Real-Time Operating System). Aplication is designed to run on STM32F7 (AnyFC F7 board).
26 | 
27 | ## Version 2.0 - *Selfie* 2.0
28 | 
29 | <img align="right" height="400" src="https://user-images.githubusercontent.com/16399071/44856111-e7c9af00-ac6c-11e8-8f9c-d272c861cc84.PNG">
30 | 
31 | Software released as [version 2.0](https://github.com/perciax/Selfie-autonomous-car/tree/v2.0) was onboarded in the brand new vehicle built for International Autonomous Robot Racing Competition ([IARRC](https://www.robotcompetition.ca/iarrc2018/)) 2018 in Waterloo, Canada. *Selfie* won first prize outperforming in both Drag and Circut Races. The vehicle was able to reach the speed of 12 m/s in stable Drag Race. Picture presents the car in development (without bodywork).
32 | 
33 | The IARRC competiton consists of project presentation part (written report, video, oral presentation) and racing part. Racing events are running outdoors. Track is marked with white and yellow lines and cones. In *Drag Race* cars are competing on 60 meters of straight track. In *Curcuit Race* cars have to drive about 1000 meters (3 laps of about 330 meters).
34 | 
35 | The main challenges to overcome were start up light detection, high-speed line detection and trajectory corrections, collision avoidance and  varying lumininance conditions (areas covered with sun light or fully or partially shadowed).
36 | 
37 | ## How to contribute
38 | 
39 | 1. Fork this repo to your account.
40 | 
41 | 2. Clone repo from your account to local folder on your device.
42 | 
43 | 	`git clone https://github.com/your-login/Selfie-autonomous-car`
44 | 
45 | 3. Add new remote repo as "main".
46 | 
47 | 	`git remote add main https://github.com/perciax/Selfie-autonomous-car`
48 | 
49 | Now you should have 2 remote repos configured:
50 | - "main" pointing this repo,
51 | - "origin" pointing copy on you account.
52 | This could be checked by command git remote -v.
53 | 
54 | 4. Update your local verison.
55 | 
56 | 	`git pull main master`
57 | 
58 | 5. Make your changes.
59 | 
60 | 6. Push them to repo on you account.
61 | 
62 | 	`git push origin master`
63 | 
64 | 7. Make Pull Request on Github.
65 | 
66 | 8. Repeat steps from 4 to 7 everytime you want to contribute to this "main" repo.	
67 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/Regresion.h:
--------------------------------------------------------------------------------
  1 | #ifndef _POLYNOMIAL_REGRESSION_H
  2 | #define _POLYNOMIAL_REGRESSION_H  __POLYNOMIAL_REGRESSION_H
  3 | /**
  4 |  * PURPOSE:
  5 |  *
  6 |  *  Polynomial Regression aims to fit a non-linear relationship to a set of
  7 |  *  points. It approximates this by solving a series of linear equations using 
  8 |  *  a least-squares approach.
  9 |  *
 10 |  *  We can model the expected value y as an nth degree polynomial, yielding
 11 |  *  the general polynomial regression model:
 12 |  *
 13 |  *  y = a0 + a1 * x + a2 * x^2 + ... + an * x^n
 14 |  * @author Chris Engelsma
 15 |  */
 16 | #include<vector>
 17 | #include<stdlib.h>
 18 | 
 19 | template <class TYPE>
 20 | class PolynomialRegression {
 21 |   public:
 22 | 
 23 |     PolynomialRegression();
 24 |     virtual ~PolynomialRegression(){};
 25 | 
 26 |     bool fitIt(
 27 |       const std::vector<TYPE> & x,
 28 |       const std::vector<TYPE> & y,
 29 |       const int &             order,
 30 |       std::vector<TYPE> &     coeffs);
 31 | };
 32 | 
 33 | template <class TYPE>
 34 | PolynomialRegression<TYPE>::PolynomialRegression() {
 35 | 
 36 | }
 37 | 
 38 | template <class TYPE>
 39 | bool SpPolynomialRegression<TYPE>::fitIt( 
 40 |   const std::vector<TYPE> & x,
 41 |   const std::vector<TYPE> & y,
 42 |   const int &               order,
 43 |   std::vector<TYPE> &       coeffs)
 44 | {
 45 |   // The size of xValues and yValues should be same
 46 |   if (x.size() != y.size()) {
 47 |     throw std::runtime_error( "The size of x & y arrays are different" );
 48 |     return false;
 49 |   }
 50 |   // The size of xValues and yValues cannot be 0, should not happen
 51 |   if (x.size() == 0 || y.size() == 0) {
 52 |     throw std::runtime_error( "The size of x or y arrays is 0" );
 53 |     return false;
 54 |   }
 55 |   
 56 |   size_t N = x.size();
 57 |   int n = order;
 58 |   int np1 = n + 1;
 59 |   int np2 = n + 2;
 60 |   int tnp1 = 2 * n + 1;
 61 |   TYPE tmp;
 62 | 
 63 |   // X = vector that stores values of sigma(xi^2n)
 64 |   std::vector<TYPE> X(tnp1);
 65 |   for (int i = 0; i < tnp1; ++i) {
 66 |     X[i] = 0;
 67 |     for (int j = 0; j < N; ++j)
 68 |       X[i] += (TYPE)pow(x.get(j), i);
 69 |   }
 70 | 
 71 |   // a = vector to store final coefficients.
 72 |   std::vector<TYPE> a(np1);
 73 | 
 74 |   // B = normal augmented matrix that stores the equations.
 75 |   std::vector<std::vector<TYPE> > B(np1, std::vector<TYPE> (np2, 0));
 76 | 
 77 |   for (int i = 0; i <= n; ++i) 
 78 |     for (int j = 0; j <= n; ++j) 
 79 |       B[i][j] = X[i + j];
 80 | 
 81 |   // Y = vector to store values of sigma(xi^n * yi)
 82 |   std::vector<TYPE> Y(np1);
 83 |   for (int i = 0; i < np1; ++i) {
 84 |     Y[i] = (TYPE)0;
 85 |     for (int j = 0; j < N; ++j) {
 86 |       Y[i] += (TYPE)pow(x[j], i)*y[j];
 87 |     }
 88 |   }
 89 | 
 90 |   // Load values of Y as last column of B
 91 |   for (int i = 0; i <= n; ++i) 
 92 |     B[i][np1] = Y[i];
 93 | 
 94 |   n += 1;
 95 |   int nm1 = n-1;
 96 | 
 97 |   // Pivotisation of the B matrix.
 98 |   for (int i = 0; i < n; ++i) 
 99 |     for (int k = i+1; k < n; ++k) 
100 |       if (B[i][i] < B[k][i]) 
101 |         for (int j = 0; j <= n; ++j) {
102 |           tmp = B[i][j];
103 |           B[i][j] = B[k][j];
104 |           B[k][j] = tmp;
105 |         }
106 | 
107 |   // Performs the Gaussian elimination.
108 |   // (1) Make all elements below the pivot equals to zero
109 |   //     or eliminate the variable.
110 |   for (int i=0; i<nm1; ++i)
111 |     for (int k =i+1; k<n; ++k) {
112 |       TYPE t = B[k][i] / B[i][i];
113 |       for (int j=0; j<=n; ++j)
114 |         B[k][j] -= t*B[i][j];         // (1)
115 |     }
116 | 
117 |   // Back substitution.
118 |   // (1) Set the variable as the rhs of last equation
119 |   // (2) Subtract all lhs values except the target coefficient.
120 |   // (3) Divide rhs by coefficient of variable being calculated.
121 |   for (int i=nm1; i >= 0; --i) {
122 |     a[i] = B[i][n];                   // (1)
123 |     for (int j = 0; j<n; ++j)
124 |       if (j != i)
125 |         a[i] -= B[i][j] * a[j];       // (2)
126 |     a[i] /= B[i][i];                  // (3)
127 |   }
128 | 
129 |   coeffs.resize(a.size());
130 |   for (size_t i = 0; i < a.size(); ++i) 
131 |     coeffs.put(i, a[i]);
132 | 
133 |   return true;
134 | }
135 | #endif
136 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/usb.cpp:
--------------------------------------------------------------------------------
  1 | #include "usb.hpp"
  2 | 
  3 | 
  4 | USB_STM::USB_STM()
  5 | {
  6 | 
  7 |     std::string name = "/dev/ttyACM0";
  8 |     strcpy(&portname[0], name.c_str());
  9 | }
 10 | 
 11 | int USB_STM::init(int speed)
 12 | {
 13 |     // Try openning ports from ttyACM0 to ttyACM9
 14 | 
 15 |     for(int i = 48; i < 58; i++)
 16 |     {
 17 |         portname[11] = i;
 18 |         fd = open(&portname[0], O_RDWR | O_NOCTTY | O_SYNC);
 19 |         if (fd < 0)
 20 |             std::cout << "Could not open serial communication on port: " << portname << std::endl;
 21 |         else
 22 |         {
 23 |             std::cout << "Opened serial communication on port: " << portname << std::endl;
 24 |             std::cout << "File descriptor: " << fd << std::endl;
 25 |             break;
 26 |         }
 27 |     }
 28 | 
 29 |     if (fd < 0)
 30 |     {
 31 |         std::cout << "Could not open any USB port" << std::endl;
 32 |         return -1;
 33 |     }
 34 | 
 35 |     // Get attributes of transmission
 36 |     struct termios tty;
 37 |     if (tcgetattr(fd, &tty) < 0)
 38 |     {
 39 |         std::cout << "Error while getting attributes!" << std::endl;
 40 |         return -2;
 41 |     }
 42 | 
 43 |     // Set input and output speed
 44 |     cfsetospeed(&tty, (speed_t)speed);
 45 |     cfsetispeed(&tty, (speed_t)speed);
 46 | 
 47 |     tty.c_cflag |= (CLOCAL | CREAD);    // program will not become owner of port
 48 |     tty.c_cflag &= ~CSIZE;              // bit mask for data bits
 49 |     tty.c_cflag |= CS8;                 // 8 bit data lenght
 50 |     tty.c_cflag |= PARENB;              // enable parity
 51 |     tty.c_cflag &= ~PARODD;             // even parity
 52 |     tty.c_cflag &= ~CSTOPB;             // 1 stop bit
 53 |     tty.c_cflag &= ~CRTSCTS;            // no hardware flowcontrol
 54 | 
 55 |     // set to non blocking mode
 56 |     fcntl(fd, F_SETFL, FNDELAY);
 57 | 
 58 |     // non-canonical mode
 59 |     tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
 60 |     tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
 61 |     tty.c_oflag &= ~OPOST;
 62 | 
 63 |     // fetch bytes asap
 64 |     tty.c_cc[VMIN] = 1;
 65 |     tty.c_cc[VTIME] = 1;
 66 | 
 67 |     // Set new parameters of transmission
 68 |     if (tcsetattr(fd, TCSANOW, &tty) != 0)
 69 |     {
 70 |         std::cout << "Error while setting attributes!" << std::endl;
 71 |         return -3;
 72 |     }
 73 |     return 1;
 74 | }
 75 | 
 76 | void USB_STM::uint32_to_char_tab(uint32_t input, unsigned char *output)
 77 | {
 78 |     output[0] = (unsigned char) (input >> 24 &0xff);
 79 |     output[1] = (unsigned char) (input >> 16 &0xff);
 80 |     output[2] = (unsigned char) (input >> 8 &0xff);
 81 |     output[3] = (unsigned char) (input &0xff);
 82 | }
 83 | 
 84 | void USB_STM::char_tab_to_uint32(unsigned char input[], uint32_t *output)
 85 | {
 86 |     *output = uint32_t(input[0]<<24 | input[1]<<16 | input[2]<<8 |input [3]);
 87 | }
 88 | 
 89 | void USB_STM::send_buf(data_container &to_send)
 90 | {
 91 | 
 92 |     // Pack data
 93 |     to_send_packed[0] = to_send.start;
 94 |     to_send_packed[1] = to_send.code;
 95 |     to_send_packed[2] = to_send.length;
 96 | 
 97 |     for(int i=0;i<to_send.length;i++)
 98 |     to_send_packed[i+3] = to_send.data[i];
 99 | 
100 |     to_send_packed[to_send.length+5] = to_send.stop;
101 | 
102 | 
103 |     write(fd,&to_send_packed,18);
104 | 
105 | }
106 | 
107 | void USB_STM::read_buf(int buf_size,float& velocity, uint16_t &tf_mini,uint8_t &taranis_3_pos,uint8_t &taranis_reset_gear,uint8_t& stm_reset,uint8_t& lights)
108 | {
109 |     unsigned char buf[buf_size];
110 |     int read_state = read(fd, &buf, buf_size) ;
111 | 
112 | 
113 |     if(read_state>0)
114 |     {    std::cout << "Len: " << read_state<< std::endl;
115 |         //for(int i = 0; i < buf_size; i++)
116 |         //{
117 |             //std::cout << (int)buf[i]<<"\t";
118 |         //}
119 |     //4 byte --> float union
120 |     union
121 |     {
122 |         float f;
123 |         unsigned char b[4];
124 |     }u;
125 | 
126 |      u.b[0]=buf[3];
127 |      u.b[1]=buf[4];
128 |      u.b[2]=buf[5];
129 |      u.b[3]=buf[6];
130 | 
131 |      //car velocity
132 |      velocity = u.f;
133 | 
134 |      //tf mini distance
135 |      tf_mini = buf[7];
136 |      tf_mini = (tf_mini<<8) | buf[8];
137 | 
138 |      taranis_3_pos = buf[9];
139 |      taranis_reset_gear = buf[10];
140 |      stm_reset = buf[11];
141 |      lights = buf[12];
142 |     }
143 | }
144 | 
145 | void USB_STM::data_pack(uint16_t velo,uint16_t ang,data_container *container)
146 | {
147 | 
148 |     union u16to8
149 |     {
150 |         uint8_t u8[2];
151 |         uint16_t  u16;
152 |     };
153 | 
154 |     u16to8 unia;
155 | 
156 |     //fill timecode
157 |     for(int i=0;i<4;i++)
158 |     {
159 |         container->data[i] = 0;
160 |     }
161 | 
162 | 
163 |     unia.u16 = ang;
164 | 
165 |     container->data[4] = unia.u8[0] ;
166 |     container->data[5] = unia.u8[1] ;
167 | 
168 |     //fill dfi
169 |     container->data[6] = 0;
170 |     container->data[7] = 0;
171 | 
172 |     unia.u16 = velo;
173 | 
174 |     container->data[8] = unia.u8[0];
175 |     container->data[9] = unia.u8[1];
176 | 
177 |     //fill other
178 |     container->data[10] = 0;
179 |     container->data[11] = 0;
180 |     container->data[12] = 0;
181 |     container->data[13] = 0;
182 | 
183 | }
184 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | 
  3 | #include <include/urg.hpp>
  4 | #include <include/process.hpp>
  5 | #include <include/sharedmemory.hpp>
  6 | 
  7 | URG urg;
  8 | Process process;
  9 | SharedMemory shm_left(50001, 12000);
 10 | SharedMemory shm_right(50002, 12000);
 11 | SharedMemory shm_additional_data(50003, 32);
 12 | 
 13 | void clean_up();
 14 | 
 15 | int main()
 16 | {
 17 |     // USB communication with Lidar
 18 |     if(!urg.init())
 19 |     {
 20 |         std::cout << "Closing app!" << std::endl;
 21 |         clean_up();
 22 |         return -1;
 23 |     }
 24 | 
 25 |     // Shared Memory
 26 |     if(!shm_left.init() || !shm_right.init() || !shm_additional_data.init())
 27 |     {
 28 |         std::cout << "Closing app!" << std::endl;
 29 |         clean_up();
 30 |         return -1;
 31 |     }
 32 | 
 33 | #ifdef FPS_COUNT
 34 |     // FPS
 35 |     struct timespec start, end;
 36 |     unsigned int frame_count = 0;
 37 |     float seconds = 0;
 38 |     float fps = 0;
 39 | #endif
 40 | 
 41 | #ifdef DEBUG_MODE
 42 |     // OpenCV Mat's
 43 |     cv::Mat frame_raw(HEIGHT, WIDTH, CV_8UC3);
 44 |     cv::Mat frame_data(HEIGHT, WIDTH, CV_8UC3);
 45 |     cv::Mat frame_settings(1, 580, CV_8UC1);
 46 | #ifdef SHM_TEST
 47 |     cv::Mat frame_l(HEIGHT, WIDTH, CV_8UC3);
 48 |     cv::Mat frame_r(HEIGHT, WIDTH, CV_8UC3);
 49 | #endif
 50 | 
 51 |     // OpenCV UI
 52 |     char keypressed;
 53 | 
 54 |     // OpenCV windows
 55 |     cv::namedWindow("Data", cv::WINDOW_AUTOSIZE);
 56 |     cv::moveWindow("Data", 0, 0);
 57 |     cv::namedWindow("Settings", cv::WINDOW_AUTOSIZE);
 58 |     cv::moveWindow("Settings", 1500, 0);
 59 | #ifdef VERBOSE_MODE
 60 |     cv::namedWindow("Raw", cv::WINDOW_AUTOSIZE);
 61 |     cv::moveWindow("Raw", 100, 0);
 62 | #endif
 63 | 
 64 |     // OpenCV trackbars
 65 |     cv::createTrackbar("Scale", "Settings", &urg.scale, 10, NULL);
 66 |     cv::createTrackbar("C left", "Settings", &urg.cut_left, 270, NULL);
 67 |     cv::createTrackbar("C right", "Settings", &urg.cut_right, 450, NULL);
 68 |     cv::createTrackbar("Closest", "Settings", &urg.closest, 1000, NULL);
 69 |     cv::createTrackbar("Farthest", "Settings", &urg.farthest, 5000, NULL);
 70 |     cv::createTrackbar("X left", "Settings", &urg.x_left, WIDTH/2, NULL);
 71 |     cv::createTrackbar("X right", "Settings", &urg.x_right, WIDTH, NULL);
 72 |     cv::createTrackbar("Y up", "Settings", &urg.y_up, HEIGHT/2, NULL);
 73 |     cv::createTrackbar("Y down", "Settings", &urg.y_down, HEIGHT, NULL);
 74 |     cv::createTrackbar("Thresh", "Settings", &urg.thresh, 100, NULL);
 75 |     cv::createTrackbar("Simplify", "Settings", &process.thresh_simplify, 50, NULL);
 76 |     cv::createTrackbar("Max dist", "Settings", &process.max_dist, 200, NULL);
 77 | #endif
 78 | 
 79 |     // Main loop
 80 |     while(true)
 81 |     {
 82 | #ifdef FPS_COUNT
 83 |         // FPS
 84 |         if(frame_count == 0)
 85 |         {
 86 |             clock_gettime(CLOCK_MONOTONIC, &start);
 87 |         }
 88 | #endif
 89 |         // Read new data from Lidar
 90 |         if(urg.read() < 0)
 91 |         {
 92 |             std::cout << "Closing app!" << std::endl;
 93 |             clean_up();
 94 |             return -3;
 95 |         }
 96 | 
 97 |         // Preprocess data
 98 |         urg.polar_to_cartesian();
 99 |         urg.filter_distance(urg.raw_data, urg.filtered_data[0]);
100 |         urg.filter_Y(urg.filtered_data[0], urg.filtered_data[1]);
101 | 
102 |         // Process data
103 |         process.simplify_data(urg.filtered_data[1]);
104 |         process.split_poins_equally(urg.filtered_data[1]);
105 |         process.search_gap();
106 |         process.filter_enemies();
107 | 
108 |         // Send data to Shared Memory
109 |         shm_left.push_point_data(process.left_points);
110 |         shm_right.push_point_data(process.right_points);
111 |         shm_additional_data.push_additional_data(cv::Point(WIDTH/2, HEIGHT/2), urg.scale, process.gap_pos_left, process.gap_pos_right);
112 | 
113 | #ifdef DEBUG_MODE
114 |         // Draw lidar data
115 | #ifdef VERBOSE_MODE
116 |         urg.draw_data_raw(frame_raw);
117 | #endif
118 |         urg.draw_boundaries(frame_data, urg.filtered_data[1]);
119 |         process.draw_data(frame_data);
120 | 
121 | #ifdef SHM_TEST
122 |         // Read data from Shared Memory
123 |         shm_left.pull_points_data(frame_l, cv::Scalar(0,200,200));
124 |         shm_right.pull_points_data(frame_r, cv::Scalar(200,0,200));
125 |         shm_additional_data.pull_additional_data();
126 | #endif
127 |         // Update windows
128 |         cv::imshow("Data", frame_data);
129 |         cv::imshow("Settings", frame_settings);
130 | #ifdef SHM_TEST
131 |         cv::imshow("SHM_L", frame_l);
132 |         cv::imshow("SHM_R", frame_r);
133 | #endif
134 | 
135 | #ifdef VERBOSE_MODE
136 |         cv::imshow("Raw", frame_raw);
137 | #endif
138 | 
139 |         // Get input from user
140 |         keypressed = (char)cv::waitKey(FRAME_TIME);
141 | 
142 |         // Process given input
143 |         if( keypressed == 27 )
144 |             break;
145 | 
146 |         // Clear cv::Mat's for the next frame
147 |         frame_data = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
148 | #ifdef SHM_TEST
149 |         frame_l = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
150 |         frame_r = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
151 | #endif
152 | #endif
153 | 
154 | #ifdef FPS_COUNT
155 |         // FPS
156 |         if(frame_count > FPS_AMOUNT)
157 |         {
158 |             frame_count = 0;
159 |             clock_gettime(CLOCK_MONOTONIC, &end);
160 |             seconds = (end.tv_sec - start.tv_sec);
161 |             fps  =  1 / (seconds / FPS_AMOUNT);
162 | 
163 |             std::cout <<"FPS: " << fps << std::endl;
164 |         }
165 |         else
166 |         {
167 |             frame_count++;
168 |         }
169 | #endif
170 |     }
171 | 
172 |     clean_up();
173 |     return 0;
174 | }
175 | 
176 | void clean_up()
177 | {
178 |     urg.close();
179 |     shm_left.close();
180 |     shm_right.close();
181 |     shm_additional_data.close();
182 | }
183 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/optimization.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include "optimization.h"
  3 | 
  4 | using namespace std;
  5 | using namespace cv;
  6 | 
  7 | int param = 5;
  8 | 
  9 | void points_to_mat(Mat& point_mat,vector<Point> points)
 10 | {
 11 |     for (int i=0;i<(int)points.size();i++)
 12 |     {
 13 |              Point pom;
 14 |              Vec3b  color;
 15 |              pom.y = points[i].y;
 16 |              pom.x = points[i].x;
 17 | 
 18 |              color = point_mat.at<Vec3b>(pom);
 19 |              color.val[0] = 255;
 20 |              color.val[1] = 255;
 21 |              color.val[2] = 255;
 22 |              point_mat.at<Vec3b>(pom) = color;
 23 |       }
 24 | 
 25 | 
 26 | }
 27 | 
 28 | void one_line_planner(spline_t r_line,int offset, spline_t& path_line)
 29 | {
 30 |     //middle of camera view
 31 |     Point middle(LIDAR_MAT_WIDTH/2,LIDAR_MAT_HEIGHT-1);
 32 | 
 33 |     vector<Point>pom;
 34 |     pom.push_back(middle);
 35 | 
 36 |     pom.push_back(Point(int(r_line.spline(LIDAR_MAT_HEIGHT/2))+offset,LIDAR_MAT_HEIGHT/2));
 37 |     pom.push_back(Point(int(r_line.spline(LIDAR_MAT_HEIGHT/3))+offset,LIDAR_MAT_HEIGHT/3));
 38 | 
 39 |     pom.push_back(Point(r_line.spline(1)+offset,1));
 40 | 
 41 | 
 42 |     path_line.set_spline(pom,false);
 43 | 
 44 | }
 45 | 
 46 | void two_line_planner(spline_t y_line, spline_t w_line, int offset, spline_t& path_line)
 47 | {
 48 |     Point middle(LIDAR_MAT_WIDTH/2,LIDAR_MAT_HEIGHT-1);
 49 |     bool direction = false;
 50 | 
 51 |     //check if direction is correct
 52 |     if(y_line.spline(LIDAR_MAT_HEIGHT)<w_line.spline(LIDAR_MAT_HEIGHT) || y_line.spline(LIDAR_MAT_HEIGHT/2)<w_line.spline(LIDAR_MAT_HEIGHT/2))
 53 |     {
 54 |         direction = true;
 55 |     }
 56 | 
 57 |     if(direction)
 58 |     {
 59 |     vector<Point>pom;
 60 |     pom.push_back(middle);
 61 |     pom.push_back(Point((w_line.spline(LIDAR_MAT_HEIGHT/2) + y_line.spline(LIDAR_MAT_HEIGHT/2))/2 + offset, LIDAR_MAT_HEIGHT/2));
 62 |     pom.push_back(Point((w_line.spline(LIDAR_MAT_HEIGHT/3) + y_line.spline(LIDAR_MAT_HEIGHT/3))/2 + offset, LIDAR_MAT_HEIGHT/3));
 63 |     pom.push_back(Point((w_line.spline(1) + y_line.spline(1))/2 + offset, 1));
 64 |     path_line.set_spline(pom,true);
 65 | 
 66 | 
 67 | 
 68 |     }
 69 | }
 70 | 
 71 | void new_optimization(vector<Point> pts_vector,spline_t& spl,Mat &preview)
 72 | {
 73 |     int rec_height = LIDAR_MAT_HEIGHT/number_of_rec_raws;
 74 |     int rec_width = LIDAR_MAT_WIDTH/number_of_rec_cols;//szerokość prostokąta wykrywania
 75 |     int grid[number_of_rec_cols][number_of_rec_raws];
 76 | 
 77 |     vector<Point> spline_points;
 78 | 
 79 |     for(int c =0;c<number_of_rec_cols;c++)
 80 |     {
 81 |         for(int r =0;r<number_of_rec_raws;r++)
 82 |         {
 83 |             grid[c][r] = 0;
 84 |         }
 85 |     }
 86 | 
 87 |     for(int i = 0;i<pts_vector.size();i++)
 88 |     {
 89 |         int x = pts_vector[i].x/rec_width;
 90 |         int y = pts_vector[i].y/rec_height;
 91 | 
 92 |         grid[x][y] = grid[x][y]+1;
 93 |     }
 94 | 
 95 |     int max_col_index = 0;
 96 | 
 97 |     int find_flag = 0;
 98 | 
 99 |     for(int r=number_of_rec_raws-1;r>0;r--)
100 |     {
101 |         int max =0;
102 | 
103 |         if(find_flag ==0)
104 |         {
105 |             for(int c=0;c<number_of_rec_cols;c++)
106 |             {
107 |                 if(grid[c][r]>max)
108 |                 {
109 |                     max = grid[c][r];
110 |                     max_col_index = c;
111 |                 }
112 |             }
113 | 
114 |             if(max>0)
115 |             {
116 |                 find_flag = 1;
117 |                 spline_points.push_back(Point(max_col_index*rec_width+0.5*rec_width,r*rec_height+0.5*rec_height));
118 |                 rectangle(preview,Point(max_col_index*rec_width,r*rec_height),Point(max_col_index*rec_width+rec_width,r*rec_height+rec_height),CV_RGB(255,0,255));
119 | 
120 |             }
121 |         }
122 |         else
123 |         {
124 |             for(int c = max_col_index-param;c<max_col_index+param;c++)
125 |             {
126 |                 if(c>number_of_rec_cols-1 || c<0)
127 |                     continue;
128 | 
129 |                 if(grid[c][r]>max)
130 |                 {
131 |                     max = grid[c][r];
132 |                     max_col_index = c;
133 |                 }
134 |                 if(max == 0)
135 |                 {
136 |                     find_flag = 0;
137 |                     break;
138 |                 }
139 |             }
140 | 
141 |             spline_points.push_back(Point(max_col_index*rec_width+0.5*rec_width,r*rec_height+0.5*rec_height));
142 | 
143 |             rectangle(preview,Point(max_col_index*rec_width,r*rec_height),Point(max_col_index*rec_width+rec_width,r*rec_height+rec_height),CV_RGB(255,0,255));
144 |         }
145 |     }
146 | 
147 |     if(spline_points.size()>2)
148 |     {
149 |         vector<Point> spline_set;
150 | 
151 |         //minimalna liczba punktow
152 |         if(spline_points.size()<6)
153 |         {
154 | 
155 |         spline_set.push_back(spline_points[0]);//pierwszy punkt
156 |         spline_set.push_back(spline_points[1]);//drugi punkt
157 |         spline_set.push_back(spline_points.back());//ostatni punkt
158 | 
159 |         }
160 | 
161 |         //jezeli wiecej punktow to wez wiecej do spline
162 |         else
163 |         {
164 |             //spline_set.push_back(spline_points[0]);//pierwszy punkt
165 |             for(int i =0;i<spline_points.size();i++)
166 |                 spline_set.push_back(spline_points[i]);
167 |             //spline_set.push_back(spline_points.back());//ostatni punkt
168 |         }
169 | 
170 |         for(int i=0;i<spline_set.size()-1;i++)
171 |         {
172 |             if(spline_set[i+1].x==spline_set[i].x)
173 |                 spline_set[i].x=spline_set[i].x+1;
174 |         }
175 |         spl.set_spline(spline_set,false);
176 |     }
177 | 
178 |     else//nie wykryto
179 |     {
180 |     }
181 | 
182 | 
183 | }
184 | void two_wall_planner(spline_t left_spline,spline_t right_spline, spline_t &path_line)
185 | {
186 | 
187 | 
188 |     vector<Point>pom;
189 |     Point middle(LIDAR_MAT_WIDTH/2,LIDAR_MAT_HEIGHT-1); //midle point of view
190 |     pom.push_back(middle);
191 | 
192 |     pom.push_back(Point((left_spline.Y[left_spline.Y.size()/2] + right_spline.Y[right_spline.Y.size()/2])/2, (left_spline.X[left_spline.X.size()/2]+right_spline.X[right_spline.X.size()/2])/2));
193 | 
194 |     pom.push_back(Point((left_spline.Y[0] + right_spline.Y[0])/2, (left_spline.X[0]+right_spline.X[0])/2)); //midle of space between top detected points
195 | 
196 |     path_line.set_spline(pom,true);
197 | 
198 | }
199 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/process.cpp:
--------------------------------------------------------------------------------
  1 | #include "process.hpp"
  2 | 
  3 | Process::Process()
  4 | {
  5 |     gap_pos_left = cv::Point(100, 100);
  6 |     gap_pos_right = cv::Point(200, 200);
  7 | }
  8 | 
  9 | void Process::simplify_data(LidarReading &points)
 10 | {
 11 |     cv::Point tmp, last;
 12 |     std::vector<cv::Point> tmp_vec_pos;
 13 |     std::vector<double> tmp_vec_angle;
 14 | 
 15 |     tmp_vec_pos.push_back(points.pos[0]);
 16 |     tmp_vec_angle.push_back(points.angle[0]);
 17 |     last = points.pos[0];
 18 | 
 19 |     for(uint32_t i = 1; i < points.pos.size() - 1; i++)
 20 |     {
 21 |         tmp = points.pos[i] - last;
 22 |         if(sqrt(tmp.x*tmp.x + tmp.y*tmp.y) > thresh_simplify)
 23 |         {
 24 |             tmp_vec_pos.push_back(points.pos[i]);
 25 |             tmp_vec_angle.push_back(points.angle[i]);
 26 |             last = points.pos[i];
 27 |         }
 28 |     }
 29 | 
 30 |     points.pos.clear();
 31 |     points.angle.clear();
 32 |     points.pos = tmp_vec_pos;
 33 |     points.angle = tmp_vec_angle;
 34 | }
 35 | 
 36 | void Process::split_poins(LidarReading &points)
 37 | {
 38 |     left_points.clear();
 39 |     right_points.clear();
 40 |     rejected_points.clear();
 41 | 
 42 |     uint32_t i;
 43 |     uint32_t j;
 44 | 
 45 |     if(points.pos.size() >= 4)
 46 |     {
 47 |         left_points.push_back(points.pos[points.pos.size()-1]);
 48 | 
 49 |         for(i = points.pos.size()-2; i > 0; i--)
 50 |         {
 51 |             if(sqrt((points.pos[i].x - points.pos[i+1].x)*(points.pos[i].x - points.pos[i+1].x) + (points.pos[i].y - points.pos[i+1].y)*(points.pos[i].y - points.pos[i+1].y)) < max_dist)
 52 |                 left_points.push_back(points.pos[i]);
 53 |             else
 54 |                 break;
 55 |         }
 56 | 
 57 |         right_points.push_back(points.pos[0]);
 58 | 
 59 |         for(j = 1; j < points.pos.size(); j++)
 60 |         {
 61 |             if(sqrt((points.pos[j].x - points.pos[j-1].x)*(points.pos[j].x - points.pos[j-1].x) + (points.pos[j].y - points.pos[j-1].y)*(points.pos[j].y - points.pos[j-1].y)) < max_dist)
 62 |                 right_points.push_back(points.pos[j]);
 63 |             else
 64 |                 break;
 65 |         }
 66 | 
 67 |         int i_int = i;
 68 |         int j_int= j;
 69 | 
 70 |         if(i_int - j_int >= 0)
 71 |         {
 72 |             for(uint32_t k = j; k <= i; k++)
 73 |                 rejected_points.push_back(points.pos[k]);
 74 |         }
 75 |     }
 76 | 
 77 | }
 78 | 
 79 | void Process::split_poins_equally(LidarReading &points)
 80 | {
 81 |     left_points.clear();
 82 |     right_points.clear();
 83 |     rejected_points.clear();
 84 | 
 85 |     uint32_t i = 0;
 86 |     uint32_t j = points.pos.size();
 87 | 
 88 |     bool left_continous = true;
 89 |     bool right_continous = true;
 90 | 
 91 |     if(points.pos.size() >= 4)
 92 |     {
 93 |         if(points.pos[points.pos.size()-1].y > HEIGHT/2 - 50)
 94 |         {
 95 |             left_points.push_back(points.pos[points.pos.size()-1]);
 96 |         }
 97 |         else
 98 |         {
 99 |             left_continous = false;
100 |         }
101 | 
102 |         if(points.pos[0].y > HEIGHT/2 - 50)
103 |         {
104 |             right_points.push_back(points.pos[0]);
105 |         }
106 |         else
107 |         {
108 |             right_continous = false;
109 |         }
110 | 
111 |         std::cout << "Left:  " << points.angle[0] << std::endl;
112 |         std::cout << "Right: " << points.angle[points.angle.size()-1] << std::endl;
113 | 
114 |         for(i = 1; i < points.pos.size()-1; i++)
115 |         {
116 |             if(right_continous)
117 |             {
118 |                 if(sqrt((points.pos[i].x - points.pos[i-1].x)*(points.pos[i].x - points.pos[i-1].x) + (points.pos[i].y - points.pos[i-1].y)*(points.pos[i].y - points.pos[i-1].y)) < max_dist)
119 |                     right_points.push_back(points.pos[i]);
120 |                 else
121 |                     right_continous = false;
122 |             }
123 | 
124 |             if(left_continous)
125 |             {
126 |                 j = points.pos.size()-1-i;
127 |                 if(sqrt((points.pos[j].x - points.pos[j+1].x)*(points.pos[j].x - points.pos[j+1].x) + (points.pos[j].y - points.pos[j+1].y)*(points.pos[j].y - points.pos[j+1].y)) < max_dist)
128 |                     left_points.push_back(points.pos[j]);
129 |                 else
130 |                     left_continous = false;
131 |             }
132 | 
133 |             if(points.pos.size()-1-i < i || (!left_continous && !right_continous))
134 |                 break;
135 |         }
136 | 
137 |         int i_int = i;
138 |         int j_int= j;
139 | 
140 |         if(j_int - i_int >= 0)
141 |         {
142 |             for(uint32_t k = i; k <= j; k++)
143 |                 rejected_points.push_back(points.pos[k]);
144 |         }
145 |     }
146 | 
147 | }
148 | 
149 | void Process::search_gap()
150 | {
151 |     if(left_points.size() > 0)
152 |     {
153 |         gap_pos_left = left_points[left_points.size()-1];
154 |     }
155 |     else
156 |     {
157 |         gap_pos_left = cv::Point(WIDTH/2 - 100, HEIGHT/2 - 100);
158 |     }
159 |     if(right_points.size() > 0)
160 |     {
161 |         gap_pos_right = right_points[right_points.size()-1];
162 |     }
163 |     else
164 |     {
165 |         gap_pos_right = cv::Point(WIDTH/2 + 100, HEIGHT/2 - 100);
166 |     }
167 | }
168 | 
169 | void Process::filter_enemies()
170 | {
171 |     int Px, Py, alpha;
172 |     int Vx = gap_pos_left.x - gap_pos_right.x;
173 |     int Vy = gap_pos_left.y - gap_pos_right.y;
174 | 
175 |     enemies_points.clear();
176 |     trash_points.clear();
177 | 
178 |     for(uint32_t i = 0; i < rejected_points.size(); i++)
179 |     {
180 |         Px = rejected_points[i].x - gap_pos_right.x;
181 |         Py = rejected_points[i].y - gap_pos_right.y;
182 | 
183 |         alpha = Vx*Py - Vy*Px;
184 | 
185 |         if(alpha < 0)
186 |             enemies_points.push_back(rejected_points[i]);
187 |         else
188 |             trash_points.push_back(rejected_points[i]);
189 |     }
190 | }
191 | 
192 | void Process::draw_data(cv::Mat &out)
193 | {
194 |     for(uint32_t i = 0; i < left_points.size(); i++)
195 |         cv::circle(out, left_points[i], 2, cv::Scalar(255, 0, 0), 2, 8, 0);
196 | 
197 |     for(uint32_t i = 0; i < right_points.size(); i++)
198 |         cv::circle(out, right_points[i], 2, cv::Scalar(0, 255, 0), 2, 8, 0);
199 | 
200 |     for(uint32_t i = 0; i < enemies_points.size(); i++)
201 |         cv::circle(out, enemies_points[i], 2, cv::Scalar(0, 255, 255), 2, 8, 0);
202 | 
203 |     for(uint32_t i = 0; i < trash_points.size(); i++)
204 |         cv::circle(out, trash_points[i], 2, cv::Scalar(255, 0, 255), 2, 8, 0);
205 | 
206 |     cv::circle(out, gap_pos_left, 4, cv::Scalar(255, 255, 255), 2, 8, 0);
207 |     cv::circle(out, gap_pos_right, 4, cv::Scalar(255, 255, 255), 2, 8, 0);
208 |     cv::line(out, gap_pos_left, gap_pos_right, cv::Scalar(255,255,255), 2, cv::LINE_8, 0);
209 | }
210 | 


--------------------------------------------------------------------------------
/lidar-app/lidar-app/include/urg.cpp:
--------------------------------------------------------------------------------
  1 | #include "urg.hpp"
  2 | 
  3 | // Serial communication parameters
  4 | const char connect_device[] = "/dev/ttyACM0";
  5 | const long connect_baudrate = 115200;
  6 | 
  7 | URG::URG()
  8 | {
  9 | 
 10 | }
 11 | 
 12 | int URG::init()
 13 | {
 14 |     // Try to start communication
 15 |     if(urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate) < 0)
 16 |     {
 17 |         std::cout << "Can not open serial communication with sensor on port " << connect_device << std::endl;
 18 |         return -1;
 19 |     }
 20 |     else
 21 |     {
 22 |         std::cout << "Connected to lidar on port " << connect_device << std::endl;
 23 |     }
 24 | 
 25 |     // Check sensor
 26 |     if(!(urg_is_stable(&urg)))
 27 |     {
 28 |         std::cout << "Sensor is unstable and can not do measurement!" << std::endl;
 29 |         return -2;
 30 |     }
 31 |     else
 32 |     {
 33 |         std::cout << "Sensor is stable" << std::endl;
 34 |     }
 35 | 
 36 |     // Allocate memmory for data
 37 |     data_size = urg_max_data_size(&urg);
 38 |     raw_data.data = (long *)malloc(data_size * sizeof(long));
 39 | 
 40 |     // Calculate angle offset for OpenCV drawing
 41 |     urg_start_measurement(&urg, URG_DISTANCE, 1, 0);
 42 |     n = urg_get_distance(&urg, raw_data.data, NULL);
 43 |     angle_offset = 150 - ((data_size - n) * 0.36) / 4;
 44 | 
 45 |     return 1;
 46 | }
 47 | 
 48 | int URG::read()
 49 | {
 50 |     // Start sensor
 51 |     if(urg_start_measurement(&urg, URG_DISTANCE, 1, 0) < 0)
 52 |     {
 53 |         std::cout << "Error getting new data from sensor" << std::endl;
 54 |         return -1;
 55 |     }
 56 | 
 57 |     // Get new data from sensor
 58 |     n = urg_get_distance(&urg, raw_data.data, NULL);
 59 |     return n;
 60 | }
 61 | 
 62 | void URG::polar_to_cartesian()
 63 | {
 64 |     cv::Point new_data;
 65 |     raw_data.pos.clear();
 66 |     raw_data.angle.clear();
 67 | 
 68 |     for(int i = 0; i < n; i++)
 69 |     {
 70 |         raw_data.angle.push_back((i * 0.36 + angle_offset) * (3.14159/180));
 71 |         new_data.x = -cos(raw_data.angle[i]) * raw_data.data[i]/scale + WIDTH/2;
 72 |         new_data.y = sin(raw_data.angle[i]) * raw_data.data[i]/scale + HEIGHT/2;
 73 | 
 74 |         raw_data.pos.push_back(new_data);
 75 |     }
 76 | }
 77 | 
 78 | void URG::filter_angle(LidarReading &input, LidarReading &output)
 79 | {
 80 |     output.angle.clear();
 81 |     output.pos.clear();
 82 | 
 83 |     double right = cut_left*(3.14159/180);
 84 |     double left = cut_right*(3.14159/180);
 85 | 
 86 |     for(uint32_t i = 0; i < input.angle.size(); i++)
 87 |     {
 88 |         if(input.angle[i] > right && input.angle[i] < left)
 89 |         {
 90 |             output.pos.push_back(input.pos[i]);
 91 |             output.angle.push_back(input.angle[i]);
 92 |         }
 93 |     }
 94 | }
 95 | 
 96 | void URG::filter_distance(LidarReading &input, LidarReading &output)
 97 | {
 98 |     output.angle.clear();
 99 |     output.pos.clear();
100 | 
101 |     int distance;
102 | 
103 |     for(uint32_t i = 0; i < input.pos.size(); i++)
104 |     {
105 |         distance = scale * sqrt((input.pos[i].x - WIDTH/2)*(input.pos[i].x - WIDTH/2) + (input.pos[i].y - HEIGHT/2)*(input.pos[i].y - HEIGHT/2));
106 | 
107 |         if(distance > closest && distance < farthest)
108 |         {
109 |             output.pos.push_back(input.pos[i]);
110 |             output.angle.push_back(input.angle[i]);
111 |         }
112 |     }
113 | }
114 | 
115 | void URG::filter_X(LidarReading &input, LidarReading &output)
116 | {
117 |     output.angle.clear();
118 |     output.pos.clear();
119 | 
120 |     for(uint32_t i = 0; i < input.pos.size(); i++)
121 |     {
122 |         if(input.pos[i].x > x_left && input.pos[i].x < x_right)
123 |         {
124 |             output.pos.push_back(input.pos[i]);
125 |             output.angle.push_back(input.angle[i]);
126 |         }
127 |     }
128 | }
129 | 
130 | void URG::filter_Y(LidarReading &input, LidarReading &output)
131 | {
132 |     output.angle.clear();
133 |     output.pos.clear();
134 | 
135 |     for(uint32_t i = 0; i < input.pos.size(); i++)
136 |     {
137 |         if(input.pos[i].y < y_down && input.pos[i].y > y_up)
138 |         {
139 |             output.pos.push_back(input.pos[i]);
140 |             output.angle.push_back(input.angle[i]);
141 |         }
142 |     }
143 | }
144 | 
145 | void URG::filter_single_points(LidarReading &input, LidarReading &output)
146 | {
147 |     cv::Point tmp;
148 |     bool last = false;
149 | 
150 |     output.pos.clear();
151 | 
152 |     for(uint32_t i = 0; i < input.pos.size() - 1; i++)
153 |     {
154 |         tmp = input.pos[i] - input.pos[i+1];
155 |         if(sqrt(tmp.x*tmp.x + tmp.y*tmp.y) < thresh)
156 |         {
157 |             last = true;
158 |             output.pos.push_back(input.pos[i]);
159 |             output.angle.push_back(input.angle[i]);
160 |         }
161 |         else
162 |         {
163 |             if(last)
164 |                 output.pos.push_back(input.pos[i]);
165 | 
166 |             last = false;
167 |         }
168 |     }
169 | }
170 | 
171 | void URG::draw_data_raw(cv::Mat &frame)
172 | {
173 |     cv::circle(frame, cv::Point(WIDTH/2, HEIGHT/2), 20, cv::Scalar(0, 255, 255), 5, 8, 0);
174 | 
175 |     for(uint32_t i = 0; i < raw_data.pos.size(); i++)
176 |         cv::circle(frame, raw_data.pos[i], 2, cv::Scalar(255, 255, 255), 2, 8, 0);
177 | }
178 | 
179 | void URG::draw_data_filtered(cv::Mat &frame, LidarReading &filtered)
180 | {
181 |     cv::circle(frame, cv::Point(WIDTH/2, HEIGHT/2), 20, cv::Scalar(0, 255, 255), 5, 8, 0);
182 | 
183 |     for(uint32_t i = 0; i < filtered.pos.size(); i++)
184 |         cv::circle(frame, filtered.pos[i], 2, cv::Scalar(255, 255, 255), 2, 8, 0);
185 | }
186 | 
187 | void URG::draw_boundaries(cv::Mat &frame, LidarReading &input)
188 | {
189 |     if(input.pos.size() > 0)
190 |     {
191 |         cv::line(frame, cv::Point(WIDTH/2, HEIGHT/2), input.pos[0], cv::Scalar(255,0,0), 3, 8, 0);
192 |         cv::line(frame, cv::Point(WIDTH/2, HEIGHT/2), input.pos[input.pos.size()-1], cv::Scalar(0,255,0), 3, 8, 0);
193 |     }
194 | 
195 |     cv::line(frame, cv::Point(x_left,0), cv::Point(x_left, HEIGHT-1), cv::Scalar(0,0,255), 2, 8, 0);
196 |     cv::line(frame, cv::Point(x_right,0), cv::Point(x_right, HEIGHT-1), cv::Scalar(255,255,0), 2, 8, 0);
197 | 
198 |     cv::line(frame, cv::Point(0,y_up), cv::Point(WIDTH-1, y_up), cv::Scalar(255,0,255), 2, 8, 0);
199 |     cv::line(frame, cv::Point(0,y_down), cv::Point(WIDTH-1, y_down), cv::Scalar(0,255,255), 2, 8, 0);
200 | 
201 |     cv::circle(frame, cv::Point(WIDTH/2, HEIGHT/2), closest/scale, cv::Scalar(128, 128, 128), 5, 8, 0);
202 |     cv::circle(frame, cv::Point(WIDTH/2, HEIGHT/2), farthest/scale, cv::Scalar(128, 0, 128), 5, 8, 0);
203 | }
204 | 
205 | 
206 | void URG::close()
207 | {
208 |     // Stop sensor
209 |     urg_stop_measurement(&urg);
210 | 
211 |     // Disconnect from the sensor
212 |     urg_close(&urg);
213 | 
214 |     std::cout << "Disconnected from lidar" << std::endl;
215 | }
216 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/interpolation.cpp:
--------------------------------------------------------------------------------
  1 | #include "interpolation.h"
  2 | 
  3 | using namespace std;
  4 | 
  5 | poly2_interp::poly2_interp(){
  6 | 
  7 | }
  8 | 
  9 | poly3_interp::poly3_interp(){
 10 | 
 11 | }
 12 | 
 13 | exp_interp::exp_interp(){
 14 | 
 15 | }
 16 | 
 17 | 
 18 | void poly2_interp::calculate_coef(vector<Point> pts){
 19 | 
 20 |    double arr_x[3][3];
 21 |    double arr_y[3][1];
 22 | 
 23 |    float sum_x = 0;
 24 |    double sum_x2 = 0;
 25 |    double sum_x3 = 0;
 26 |    double sum_x4 = 0;
 27 | 
 28 |    double sum_y = 0;
 29 |    double sum_xy = 0;
 30 |    double sum_x2y= 0;
 31 | 
 32 | 
 33 |     for(int i=0;i<int(pts.size());i++){
 34 | 
 35 |        sum_x += pts[i].x;
 36 |        sum_x2 += pts[i].x*pts[i].x;
 37 |        sum_x3 += pts[i].x*pts[i].x*pts[i].x;
 38 |        sum_x4 += pts[i].x*pts[i].x*pts[i].x*pts[i].x;
 39 | 
 40 |        sum_y += pts[i].y;
 41 |        sum_xy +=pts[i].x*pts[i].y;
 42 |        sum_x2y +=pts[i].x*pts[i].x*pts[i].y;
 43 |     }
 44 | 
 45 | 
 46 |             //fill an arr_x
 47 |     for(int j=0;j<3;j++){
 48 |         for(int i=0;i<3;i++){
 49 |             if(i==0 && j==0)
 50 |                 arr_x[j][i] = double(pts.size());
 51 | 
 52 |             else if((i==0 && j==1) || (i==1 && j==0))
 53 |                 arr_x[j][i] = sum_x;
 54 |             else if((i==2 && j==0) || (i==1 && j==1) || (i==0 && j==2))
 55 |                 arr_x[j][i] = sum_x2;
 56 |             else if((i==2 && j ==1)|| (i==1 && j==2))
 57 |                 arr_x[j][i] =sum_x3;
 58 |             else if(i==2 && j==2)
 59 |                 arr_x[j][i] = sum_x4;
 60 |         }
 61 |     }
 62 | 
 63 |     arr_y[0][0] = sum_y;
 64 |     arr_y[1][0] = sum_xy;
 65 |     arr_y[2][0] = sum_x2y;
 66 | 
 67 |     double inv_arr_x[3][3];
 68 |     double det;
 69 | /*
 70 |     for(int i=0;i<3;i++){
 71 |         cout<<arr_x<<" ";
 72 |     }
 73 |     cout<<endl;
 74 | */
 75 |     INVERT_3X3(inv_arr_x,det,arr_x);
 76 | 
 77 |     double coef_arr[3][1];
 78 | 
 79 |     for(int j =0;j<3;j++ ){
 80 |         double tmp = 0;
 81 |         for(int i=0;i<3;i++){
 82 |             tmp += inv_arr_x[j][i]*arr_y[i][0];
 83 |         }
 84 |         coef_arr[j][0]=tmp;
 85 |     }
 86 |     a = coef_arr[2][0];
 87 |     b = coef_arr[1][0];
 88 |     c = coef_arr[0][0];
 89 | 
 90 | }
 91 | 
 92 | void poly2_interp::draw(Mat& frame,const Scalar& col)
 93 | {
 94 |     cout<<a<<endl<<b<<endl<<c<<endl;
 95 |     for (int i=0;i<360;i++){
 96 |         Point pom;
 97 |         Vec3b  color;
 98 |       pom.y = a*i*i+b*i+c;
 99 | 
100 | 
101 |       color = frame.at<Vec3b>(pom);
102 |       color.val[0] = col[0];
103 |       color.val[1] = col[1];
104 |       color.val[2] = col[2];
105 |       frame.at<Vec3b>(pom) = color;
106 |      }
107 | }
108 | 
109 | 
110 | 
111 | void poly3_interp::calculate_3coef(vector<Point> pts){
112 | 
113 | 
114 |    double arr_x[4][4];
115 |    double arr_y[4][1];
116 | 
117 |    double sum_x = 0;
118 |    double sum_x2 = 0;
119 |    double sum_x3 = 0;
120 |    double sum_x4 = 0;
121 |    double sum_x5 =0;
122 |    double sum_x6 = 0;
123 | 
124 |    double sum_y = 0;
125 |    double sum_xy = 0;
126 |    double sum_x2y= 0;
127 |    double sum_x3y=0;
128 | 
129 | 
130 |     for(int i=0;i<pts.size();i++){
131 | 
132 | 
133 |        sum_x += pts[i].x;
134 |        sum_x2 += pts[i].x*pts[i].x;
135 |        sum_x3 += pts[i].x*pts[i].x*pts[i].x;
136 |        sum_x4 += pts[i].x*pts[i].x*pts[i].x*pts[i].x;
137 |        sum_x5 += pts[i].x*pts[i].x*pts[i].x*pts[i].x*pts[i].x;
138 |        sum_x6 += pts[i].x*pts[i].x*pts[i].x*pts[i].x*pts[i].x*pts[i].x;
139 | 
140 |        sum_y += pts[i].y;
141 |        sum_xy +=pts[i].x*pts[i].y;
142 |        sum_x2y +=pts[i].x*pts[i].x*pts[i].y;
143 |        sum_x3y +=pts[i].x*pts[i].x*pts[i].x*pts[i].y;
144 | 
145 | 
146 |     }
147 | 
148 | 
149 |             //fill an arr_x
150 |     for(int j=0;j<4;j++){
151 |         for(int i=0;i<4;i++){
152 |             if(i==0 && j==0)
153 |                 arr_x[j][i] = int(pts.size());
154 |             else if((i==0 && j==1) || (i==1 && j==0))
155 |                 arr_x[j][i] = sum_x;
156 |             else if((i==2 && j==0) || (i==1 && j==1) || (i==0 && j==2))
157 |                 arr_x[j][i] = sum_x2;
158 |             else if((i==2 && j ==1)|| (i==1 && j==2) || (i==3 && j==0) || (i==0 && j ==3))
159 |                 arr_x[j][i] =sum_x3;
160 |             else if((i==3 && j==1 ) || (i==2 && j==2)  || (i==1 && j==3))
161 |                 arr_x[j][i] = sum_x4;
162 |             else if((i==3 && j==2)||(i==2 && j==3))
163 |                 arr_x[j][i]=sum_x5;
164 |             else if(i==3 && j==3)
165 |                 arr_x[j][i]=sum_x6;
166 |         }
167 |     }
168 | 
169 |     arr_y[0][0] = sum_y;
170 |     arr_y[1][0] = sum_xy;
171 |     arr_y[2][0] = sum_x2y;
172 |     arr_y[3][0] = sum_x3y;
173 | 
174 |     double inv_arr_x[4][4];
175 |     double det;
176 | 
177 |     INVERT_4X4(inv_arr_x,det,arr_x);
178 | 
179 |     float coef_arr[4][1];
180 | 
181 |     for(int j =0;j<4;j++ ){
182 |         float tmp = 0;
183 |         for(int i=0;i<4;i++){
184 |             tmp += inv_arr_x[j][i]*arr_y[i][0];
185 |         }
186 |         coef_arr[j][0]=tmp;
187 |     }
188 |     a = coef_arr[3][0];
189 |     b = coef_arr[2][0];
190 |     c = coef_arr[1][0];
191 |     d = coef_arr[0][0];
192 | 
193 | }
194 | 
195 | void poly3_interp::draw(Mat& frame,const Scalar& col)
196 | {
197 | 
198 |     for (int i=0;i<480;i++){
199 |         Point pom;
200 |         Vec3b  color;
201 |       pom.y = i;
202 |       pom.x=a*i*i*i + b*i*i + c*i +d;
203 | 
204 | 
205 |       color = frame.at<Vec3b>(pom);
206 |       color.val[0] = col[0];
207 |       color.val[1] = col[1];
208 |       color.val[2] = col[2];
209 |       frame.at<Vec3b>(pom) = color;
210 |      }
211 | }
212 | 
213 | 
214 | void exp_interp::calculate_exp(vector<Point> pts , uint32_t len){
215 | {
216 |         double sum_lny = 0;
217 |         double sum_x2 = 0;
218 |         double sum_x = 0;
219 |         double sum_xlny = 0;
220 | 
221 | 
222 |         for(int i=0;i<len;i++){
223 |             sum_lny +=log(pts[i].y);
224 |             sum_xlny +=pts[i].x*log(pts[i].y);
225 |             sum_x += pts[i].x;
226 |             sum_x2 += pts[i].x *pts[i].x;
227 |         }
228 | 
229 |         double a = (sum_lny*sum_x2 - sum_x*sum_xlny)/(pts.size()*sum_x2-sum_x*sum_x);
230 |         double b = (pts.size()*sum_xlny - sum_x*sum_lny)/(pts.size()*sum_x2 -sum_x*sum_x);
231 | 
232 |         A= exp(a);
233 |         B =b;
234 |     }
235 | 
236 | 
237 | }
238 | 
239 | void exp_interp::draw(Mat& frame,const Scalar& col)
240 | {
241 | 
242 |     for (int i=0;i<480;i++){
243 |         Point pom;
244 |         Vec3b  color;
245 |       pom.y = i;
246 |       pom.x=A*exp(B*i);
247 | 
248 |       if(pom.x<0)
249 |           pom.x=0;
250 |       //else if (pom.x>640)
251 |           //pom.x=640;
252 | 
253 |       color = frame.at<Vec3b>(pom);
254 |       color.val[0] = col[0];
255 |       color.val[1] = col[1];
256 |       color.val[2] = col[2];
257 |       frame.at<Vec3b>(pom) = color;
258 |      }
259 | }
260 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/port.h:
--------------------------------------------------------------------------------
  1 | 
  2 | /*
  3 |  * port.h 
  4 |  *
  5 |  * FUNCTION:
  6 |  * This file contains defines for porting the tubing toolkit from GL to
  7 |  * OpenGL to some callback scheme.
  8 |  *
  9 |  * HISTORY:
 10 |  * Created by Linas Vepstas --  February 1993
 11 |  * Added auto texture coord generation hacks, Linas April 1994 
 12 |  */
 13 | 
 14 | #ifndef __GLE_PORT_H__
 15 | #define __GLE_PORT_H__
 16 | 
 17 | 
 18 | #ifndef TRUE
 19 | #define TRUE 1
 20 | #endif
 21 | 
 22 | #ifndef FALSE
 23 | #define FALSE 0
 24 | #endif
 25 | 
 26 | #ifndef M_PI
 27 | #define M_PI 3.14159265358979323846
 28 | #endif
 29 | 
 30 | /* ====================================================== */
 31 | /* Some compilers can't handle multiply-subscripted array's */
 32 | 
 33 | #ifdef FUNKY_C
 34 | typedef gleDouble gleVector;
 35 | #define AVAL(arr,n,i,j)  arr(6*n+3*i+j)
 36 | #define VVAL(arr,n,i)  arr(3*n+i)
 37 | 
 38 | #else  /* FUNKY_C */
 39 | typedef double gleVector[3];
 40 | typedef double glePoint[2];
 41 | #define AVAL(arr,n,i,j)  arr[n][i][j]
 42 | #define VVAL(arr,n,i)  arr[n][i];
 43 | 
 44 | #endif /* FUNKY_C */
 45 | 
 46 | /* ====================================================== */
 47 | /* These are used to convey info about topography to the 
 48 |  * texture mapping routines */
 49 | 
 50 | #define FRONT 		1
 51 | #define BACK 		2
 52 | #define FRONT_CAP 	3
 53 | #define BACK_CAP	4
 54 | #define FILLET		5
 55 | 
 56 | /* ====================================================== */
 57 | 
 58 | #define __GLE_DOUBLE
 59 | 
 60 | /* ====================================================== */
 61 | 
 62 | #ifdef __GLE_DOUBLE
 63 | #define gleDouble 		double
 64 | #define urot_axis(a,b,c) 	urot_axis_d(a,b,c)
 65 | #define uview_direction(a,b,c) 	uview_direction_d(a,b,c)
 66 | #define uviewpoint(a,b,c,d) 	uviewpoint_d(a,b,c,d)
 67 | #define MULTMATRIX(m)		MULTMATRIX_D(m)
 68 | #define LOADMATRIX(m)		LOADMATRIX_D(m)
 69 | #define V3F(x,j,id)		V3F_D(x,j,id)
 70 | #define N3F(x)			N3F_D(x)
 71 | #define T2F(x,y)		T2F_D(x,y)
 72 | #else 
 73 | #define gleDouble 		float
 74 | #define urot_axis(a,b,c) 	urot_axis_f(a,b,c)
 75 | #define uview_direction(a,b,c) 	uview_direction_f(a,b,c)
 76 | #define uviewpoint(a,b,c,d) 	uviewpoint_f(a,b,c,d)
 77 | #define MULTMATRIX(m)		MULTMATRIX_F(m)
 78 | #define LOADMATRIX(m)		LOADMATRIX_F(m)
 79 | #define V3F(x,j,id)		V3F_F(x,j,id)
 80 | #define N3F(x)			N3F_F(x)
 81 | #define T2F(x,y)		T2F_F(x,y)
 82 | #endif 
 83 | 
 84 | /* ====================================================== */
 85 | 
 86 | #if (defined DEBUG_GL_32 || DEBUG_OPENGL_10)
 87 | #undef GL_32
 88 | #undef OPENGL_10
 89 | 
 90 | #define BGNTMESH(i,len)	printf ("bgntmesh() \n");
 91 | #define ENDTMESH()      printf ("endtmesh() \n");
 92 | #define BGNPOLYGON()	printf ("bgnpolygon() \n");
 93 | #define ENDPOLYGON()	printf ("endpolygon() \n");
 94 | #define V3F_F(x,j,id)	printf ("v3f(x)		%f %f %f \n", x[0], x[1], x[2]);
 95 | #define V3F_D(x,j,id)	printf ("v3d(x) 	%f %f %f \n", x[0], x[1], x[2]);
 96 | #define	N3F_F(x)	printf ("n3f(x) 	%f %f %f \n", x[0], x[1], x[2]);
 97 | #define	N3F_D(x)	printf ("n3d(x)		%f %f %f \n", x[0], x[1], x[2]);
 98 | #define	C3F(x)		printf ("c3f(x) 	%f %f %f \n", x[0], x[1], x[2]);
 99 | 
100 | #define	POPMATRIX()	printf ("popmatrix () \n");
101 | #define	PUSHMATRIX()	printf ("pushmatrix() \n");
102 | #define	MULTMATRIX_F(x)	MULTMATRIX_D(x)
103 | #define	LOADMATRIX_F(x)	LOADMATRIX_D(x)
104 | 
105 | 
106 | #define LOADMATRIX_D(x) {				\
107 |    int i, j; 						\
108 |    printf ("loadmatrix (x) \n");			\
109 |    for (i=0; i<4; i++) {				\
110 |       for (j=0; j<4; j++) {				\
111 |          printf ( "%f ", x[i][j]);			\
112 |       }							\
113 |       printf (" \n");					\
114 |    }							\
115 | }
116 | 
117 | #define MULTMATRIX_D(x) {				\
118 |    int i, j; 						\
119 |    printf ("multmatrix (x) \n");			\
120 |    for (i=0; i<4; i++) {				\
121 |       for (j=0; j<4; j++) {				\
122 |          printf ( "%f ", x[i][j]);			\
123 |       }							\
124 |       printf (" \n");					\
125 |    }							\
126 | }
127 | 
128 | #define __IS_LIGHTING_ON  (1)
129 | 
130 | #endif
131 | 
132 | /* ====================================================== */
133 | 
134 | #ifdef GL_32
135 | 
136 | #include <gl/gl.h>
137 | 
138 | #define BGNTMESH(i,len)	bgntmesh()
139 | #define ENDTMESH()	endtmesh()
140 | #define BGNPOLYGON()	bgnpolygon()
141 | #define ENDPOLYGON()	endpolygon()
142 | #define V3F_F(x,j,id)	v3f(x)
143 | #define V3F_D(x,j,id)	v3d(x)
144 | #define	N3F_F(x)	n3f(x)
145 | #define	T2F_F(x,y)
146 | #define	T2F_D(x,y)
147 | #define	C3F(x)		c3f(x)
148 | 
149 | #define	POPMATRIX()	popmatrix ()
150 | #define	PUSHMATRIX()	pushmatrix()
151 | #define	MULTMATRIX_F(x)	multmatrix (x)
152 | #define	LOADMATRIX_F(x)	loadmatrix (x)
153 | 
154 | #define	N3F_D(x) {					\
155 |    float nnn[3];					\
156 |    nnn[0] = (float) x[0]; 				\
157 |    nnn[1] = (float) x[1]; 				\
158 |    nnn[2] = (float) x[2]; 				\
159 |    n3f (nnn);						\
160 | }
161 | 
162 | #define LOADMATRIX_D(x) {				\
163 |    int i, j; 						\
164 |    float mmm[4][4];					\
165 |    for (i=0; i<4; i++) {				\
166 |       for (j=0; j<4; j++) {				\
167 |          mmm[i][j] = (float) x[i][j];			\
168 |       }							\
169 |    }							\
170 |    loadmatrix(mmm);					\
171 | }
172 | 
173 | #define MULTMATRIX_D(x) {				\
174 |    int i, j; 						\
175 |    float mmm[4][4];					\
176 |    for (i=0; i<4; i++) {				\
177 |       for (j=0; j<4; j++) {				\
178 |          mmm[i][j] = (float) x[i][j];			\
179 |       }							\
180 |    }							\
181 |    multmatrix(mmm);					\
182 | }
183 | 
184 | /* #define __IS_LIGHTING_ON  (MSINGLE == getmmode()) */
185 | #define __IS_LIGHTING_ON  (extrusion_join_style & TUBE_LIGHTING_ON)
186 | 
187 | #endif /* GL_32 */
188 | 
189 | /* ====================================================== */
190 | #ifdef OPENGL_10
191 | 
192 | #if defined(_WIN32)
193 | #include <windows.h>
194 | #pragma warning (disable:4244)          /* disable bogus conversion warnings */
195 | #endif
196 | #if defined(__APPLE__) && defined(__MACH__)
197 | /* Mac OS X places GL headers within OpenGL framework. */
198 | #include <OpenGL/gl.h>
199 | #include <OpenGL/glu.h>
200 | #else
201 | /* All other platforms put GL headers in the standard place. */
202 | #include <GL/gl.h>
203 | #include <GL/glu.h>
204 | #endif
205 | 
206 | /*
207 | #define	N3F_F(x) {					\
208 |    float nnn[3];					\
209 |    nnn[0] = - (float) x[0]; 				\
210 |    nnn[1] = - (float) x[1]; 				\
211 |    nnn[2] = - (float) x[2]; 				\
212 |    glNormal3fv (nnn);						\
213 | }
214 | #define	N3F_D(x) {					\
215 |    float nnn[3];					\
216 |    nnn[0] = - (float) x[0]; 				\
217 |    nnn[1] = - (float) x[1]; 				\
218 |    nnn[2] = - (float) x[2]; 				\
219 |    glNormal3fv (nnn);						\
220 | }
221 | */
222 | 
223 | #define	C3F(x)		glColor3fv(x)
224 | #define	T2F_F(x,y)	glTexCoord2f(x,y)
225 | #define	T2F_D(x,y)	glTexCoord2d(x,y)
226 | 
227 | #define	POPMATRIX()	glPopMatrix()
228 | #define	PUSHMATRIX()	glPushMatrix()
229 | 
230 | #define	MULTMATRIX_F(x)	glMultMatrixf ((const GLfloat *)x)
231 | #define	LOADMATRIX_F(x)	glLoadMatrixf ((const GLfloat *)x)
232 | 
233 | #define	MULTMATRIX_D(x)	glMultMatrixd ((const GLdouble *)x)
234 | #define	LOADMATRIX_D(x)	glLoadMatrixd ((const GLdouble *)x)
235 | 
236 | #define __IS_LIGHTING_ON  (glIsEnabled(GL_LIGHTING))
237 | 
238 | /* ====================================================== */
239 | #ifdef AUTO_TEXTURE
240 | 
241 | #define BGNTMESH(i,len) { 					\
242 | 	if(_gle_gc -> bgn_gen_texture) (*(_gle_gc -> bgn_gen_texture))(i,len);\
243 | 	glBegin (GL_TRIANGLE_STRIP); 			\
244 | }
245 | 
246 | #define BGNPOLYGON() { 					\
247 | 	if(_gle_gc -> bgn_gen_texture) (*(_gle_gc -> bgn_gen_texture))();\
248 | 	glBegin (GL_POLYGON);				\
249 | }
250 | 
251 | #define N3F_F(x) { 					\
252 | 	if(_gle_gc -> n3f_gen_texture) (*(_gle_gc -> n3f_gen_texture))(x); \
253 | 	glNormal3fv(x); 				\
254 | }
255 | 
256 | #define N3F_D(x) { 					\
257 | 	if(_gle_gc -> n3d_gen_texture) (*(_gle_gc -> n3d_gen_texture))(x); \
258 | 	glNormal3dv(x); 				\
259 | }
260 | 
261 | #define V3F_F(x,j,id) { 					\
262 | 	if(_gle_gc -> v3f_gen_texture) (*(_gle_gc -> v3f_gen_texture))(x,j,id);\
263 | 	glVertex3fv(x); 				\
264 | }
265 | 
266 | #define V3F_D(x,j,id) { 					\
267 | 	if(_gle_gc -> v3d_gen_texture) (*(_gle_gc -> v3d_gen_texture))(x,j,id); \
268 | 	glVertex3dv(x); 				\
269 | }
270 | 
271 | #define ENDTMESH() {					\
272 | 	if(_gle_gc -> end_gen_texture) (*(_gle_gc -> end_gen_texture))(); \
273 | 	glEnd ();					\
274 | }
275 | 
276 | #define ENDPOLYGON() {					\
277 | 	if(_gle_gc -> end_gen_texture) (*(_gle_gc -> end_gen_texture))(); \
278 | 	glEnd ();					\
279 | }
280 | 
281 | /* ====================================================== */
282 | #else /* AUTO_TEXTURE */
283 | 
284 | #define BGNTMESH(i,len)	glBegin (GL_TRIANGLE_STRIP);
285 | #define BGNPOLYGON() 	glBegin (GL_POLYGON);
286 | 
287 | #define	N3F_F(x)	glNormal3fv(x)
288 | #define	N3F_D(x)	glNormal3dv(x)
289 | #define V3F_F(x,j,id)	glVertex3fv(x);
290 | #define V3F_D(x,j,id)	glVertex3dv(x);
291 | 
292 | #define ENDTMESH()	glEnd ()
293 | #define ENDPOLYGON()	glEnd()
294 | 
295 | #endif /* AUTO_TEXTURE */
296 | 
297 | #endif /* OPENGL_10 */
298 | 
299 | /* ====================================================== */
300 | 
301 | 
302 | #endif /* __GLE_PORT_H__ */
303 | /* ================== END OF FILE ======================= */
304 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/matrix.h:
--------------------------------------------------------------------------------
  1 | 
  2 | /*
  3 |  * port.h
  4 |  *
  5 |  * FUNCTION:
  6 |  * This file contains defines for porting the tubing toolkit from GL to
  7 |  * OpenGL to some callback scheme.
  8 |  *
  9 |  * HISTORY:
 10 |  * Created by Linas Vepstas --  February 1993
 11 |  * Added auto texture coord generation hacks, Linas April 1994
 12 |  */
 13 | 
 14 | #ifndef __GLE_PORT_H__
 15 | #define __GLE_PORT_H__
 16 | 
 17 | 
 18 | #ifndef TRUE
 19 | #define TRUE 1
 20 | #endif
 21 | 
 22 | #ifndef FALSE
 23 | #define FALSE 0
 24 | #endif
 25 | 
 26 | #ifndef M_PI
 27 | #define M_PI 3.14159265358979323846
 28 | #endif
 29 | 
 30 | /* ====================================================== */
 31 | /* Some compilers can't handle multiply-subscripted array's */
 32 | 
 33 | #ifdef FUNKY_C
 34 | typedef gleDouble gleVector;
 35 | #define AVAL(arr,n,i,j)  arr(6*n+3*i+j)
 36 | #define VVAL(arr,n,i)  arr(3*n+i)
 37 | 
 38 | #else  /* FUNKY_C */
 39 | typedef double gleVector[3];
 40 | typedef double glePoint[2];
 41 | #define AVAL(arr,n,i,j)  arr[n][i][j]
 42 | #define VVAL(arr,n,i)  arr[n][i];
 43 | 
 44 | #endif /* FUNKY_C */
 45 | 
 46 | /* ====================================================== */
 47 | /* These are used to convey info about topography to the
 48 |  * texture mapping routines */
 49 | 
 50 | #define FRONT 		1
 51 | #define BACK 		2
 52 | #define FRONT_CAP 	3
 53 | #define BACK_CAP	4
 54 | #define FILLET		5
 55 | 
 56 | /* ====================================================== */
 57 | 
 58 | #define __GLE_DOUBLE
 59 | 
 60 | /* ====================================================== */
 61 | 
 62 | #ifdef __GLE_DOUBLE
 63 | #define gleDouble 		double
 64 | #define urot_axis(a,b,c) 	urot_axis_d(a,b,c)
 65 | #define uview_direction(a,b,c) 	uview_direction_d(a,b,c)
 66 | #define uviewpoint(a,b,c,d) 	uviewpoint_d(a,b,c,d)
 67 | #define MULTMATRIX(m)		MULTMATRIX_D(m)
 68 | #define LOADMATRIX(m)		LOADMATRIX_D(m)
 69 | #define V3F(x,j,id)		V3F_D(x,j,id)
 70 | #define N3F(x)			N3F_D(x)
 71 | #define T2F(x,y)		T2F_D(x,y)
 72 | #else
 73 | #define gleDouble 		float
 74 | #define urot_axis(a,b,c) 	urot_axis_f(a,b,c)
 75 | #define uview_direction(a,b,c) 	uview_direction_f(a,b,c)
 76 | #define uviewpoint(a,b,c,d) 	uviewpoint_f(a,b,c,d)
 77 | #define MULTMATRIX(m)		MULTMATRIX_F(m)
 78 | #define LOADMATRIX(m)		LOADMATRIX_F(m)
 79 | #define V3F(x,j,id)		V3F_F(x,j,id)
 80 | #define N3F(x)			N3F_F(x)
 81 | #define T2F(x,y)		T2F_F(x,y)
 82 | #endif
 83 | 
 84 | /* ====================================================== */
 85 | 
 86 | #if (defined DEBUG_GL_32 || DEBUG_OPENGL_10)
 87 | #undef GL_32
 88 | #undef OPENGL_10
 89 | 
 90 | #define BGNTMESH(i,len)	printf ("bgntmesh() \n");
 91 | #define ENDTMESH()      printf ("endtmesh() \n");
 92 | #define BGNPOLYGON()	printf ("bgnpolygon() \n");
 93 | #define ENDPOLYGON()	printf ("endpolygon() \n");
 94 | #define V3F_F(x,j,id)	printf ("v3f(x)		%f %f %f \n", x[0], x[1], x[2]);
 95 | #define V3F_D(x,j,id)	printf ("v3d(x) 	%f %f %f \n", x[0], x[1], x[2]);
 96 | #define	N3F_F(x)	printf ("n3f(x) 	%f %f %f \n", x[0], x[1], x[2]);
 97 | #define	N3F_D(x)	printf ("n3d(x)		%f %f %f \n", x[0], x[1], x[2]);
 98 | #define	C3F(x)		printf ("c3f(x) 	%f %f %f \n", x[0], x[1], x[2]);
 99 | 
100 | #define	POPMATRIX()	printf ("popmatrix () \n");
101 | #define	PUSHMATRIX()	printf ("pushmatrix() \n");
102 | #define	MULTMATRIX_F(x)	MULTMATRIX_D(x)
103 | #define	LOADMATRIX_F(x)	LOADMATRIX_D(x)
104 | 
105 | 
106 | #define LOADMATRIX_D(x) {				\
107 |    int i, j; 						\
108 |    printf ("loadmatrix (x) \n");			\
109 |    for (i=0; i<4; i++) {				\
110 |       for (j=0; j<4; j++) {				\
111 |          printf ( "%f ", x[i][j]);			\
112 |       }							\
113 |       printf (" \n");					\
114 |    }							\
115 | }
116 | 
117 | #define MULTMATRIX_D(x) {				\
118 |    int i, j; 						\
119 |    printf ("multmatrix (x) \n");			\
120 |    for (i=0; i<4; i++) {				\
121 |       for (j=0; j<4; j++) {				\
122 |          printf ( "%f ", x[i][j]);			\
123 |       }							\
124 |       printf (" \n");					\
125 |    }							\
126 | }
127 | 
128 | #define __IS_LIGHTING_ON  (1)
129 | 
130 | #endif
131 | 
132 | /* ====================================================== */
133 | 
134 | #ifdef GL_32
135 | 
136 | #include <gl/gl.h>
137 | 
138 | #define BGNTMESH(i,len)	bgntmesh()
139 | #define ENDTMESH()	endtmesh()
140 | #define BGNPOLYGON()	bgnpolygon()
141 | #define ENDPOLYGON()	endpolygon()
142 | #define V3F_F(x,j,id)	v3f(x)
143 | #define V3F_D(x,j,id)	v3d(x)
144 | #define	N3F_F(x)	n3f(x)
145 | #define	T2F_F(x,y)
146 | #define	T2F_D(x,y)
147 | #define	C3F(x)		c3f(x)
148 | 
149 | #define	POPMATRIX()	popmatrix ()
150 | #define	PUSHMATRIX()	pushmatrix()
151 | #define	MULTMATRIX_F(x)	multmatrix (x)
152 | #define	LOADMATRIX_F(x)	loadmatrix (x)
153 | 
154 | #define	N3F_D(x) {					\
155 |    float nnn[3];					\
156 |    nnn[0] = (float) x[0]; 				\
157 |    nnn[1] = (float) x[1]; 				\
158 |    nnn[2] = (float) x[2]; 				\
159 |    n3f (nnn);						\
160 | }
161 | 
162 | #define LOADMATRIX_D(x) {				\
163 |    int i, j; 						\
164 |    float mmm[4][4];					\
165 |    for (i=0; i<4; i++) {				\
166 |       for (j=0; j<4; j++) {				\
167 |          mmm[i][j] = (float) x[i][j];			\
168 |       }							\
169 |    }							\
170 |    loadmatrix(mmm);					\
171 | }
172 | 
173 | #define MULTMATRIX_D(x) {				\
174 |    int i, j; 						\
175 |    float mmm[4][4];					\
176 |    for (i=0; i<4; i++) {				\
177 |       for (j=0; j<4; j++) {				\
178 |          mmm[i][j] = (float) x[i][j];			\
179 |       }							\
180 |    }							\
181 |    multmatrix(mmm);					\
182 | }
183 | 
184 | /* #define __IS_LIGHTING_ON  (MSINGLE == getmmode()) */
185 | #define __IS_LIGHTING_ON  (extrusion_join_style & TUBE_LIGHTING_ON)
186 | 
187 | #endif /* GL_32 */
188 | 
189 | /* ====================================================== */
190 | #ifdef OPENGL_10
191 | 
192 | #if defined(_WIN32)
193 | #include <windows.h>
194 | #pragma warning (disable:4244)          /* disable bogus conversion warnings */
195 | #endif
196 | #if defined(__APPLE__) && defined(__MACH__)
197 | /* Mac OS X places GL headers within OpenGL framework. */
198 | #include <OpenGL/gl.h>
199 | #include <OpenGL/glu.h>
200 | #else
201 | /* All other platforms put GL headers in the standard place. */
202 | #include <GL/gl.h>
203 | #include <GL/glu.h>
204 | #endif
205 | 
206 | /*
207 | #define	N3F_F(x) {					\
208 |    float nnn[3];					\
209 |    nnn[0] = - (float) x[0]; 				\
210 |    nnn[1] = - (float) x[1]; 				\
211 |    nnn[2] = - (float) x[2]; 				\
212 |    glNormal3fv (nnn);						\
213 | }
214 | #define	N3F_D(x) {					\
215 |    float nnn[3];					\
216 |    nnn[0] = - (float) x[0]; 				\
217 |    nnn[1] = - (float) x[1]; 				\
218 |    nnn[2] = - (float) x[2]; 				\
219 |    glNormal3fv (nnn);						\
220 | }
221 | */
222 | 
223 | #define	C3F(x)		glColor3fv(x)
224 | #define	T2F_F(x,y)	glTexCoord2f(x,y)
225 | #define	T2F_D(x,y)	glTexCoord2d(x,y)
226 | 
227 | #define	POPMATRIX()	glPopMatrix()
228 | #define	PUSHMATRIX()	glPushMatrix()
229 | 
230 | #define	MULTMATRIX_F(x)	glMultMatrixf ((const GLfloat *)x)
231 | #define	LOADMATRIX_F(x)	glLoadMatrixf ((const GLfloat *)x)
232 | 
233 | #define	MULTMATRIX_D(x)	glMultMatrixd ((const GLdouble *)x)
234 | #define	LOADMATRIX_D(x)	glLoadMatrixd ((const GLdouble *)x)
235 | 
236 | #define __IS_LIGHTING_ON  (glIsEnabled(GL_LIGHTING))
237 | 
238 | /* ====================================================== */
239 | #ifdef AUTO_TEXTURE
240 | 
241 | #define BGNTMESH(i,len) { 					\
242 |     if(_gle_gc -> bgn_gen_texture) (*(_gle_gc -> bgn_gen_texture))(i,len);\
243 |     glBegin (GL_TRIANGLE_STRIP); 			\
244 | }
245 | 
246 | #define BGNPOLYGON() { 					\
247 |     if(_gle_gc -> bgn_gen_texture) (*(_gle_gc -> bgn_gen_texture))();\
248 |     glBegin (GL_POLYGON);				\
249 | }
250 | 
251 | #define N3F_F(x) { 					\
252 |     if(_gle_gc -> n3f_gen_texture) (*(_gle_gc -> n3f_gen_texture))(x); \
253 |     glNormal3fv(x); 				\
254 | }
255 | 
256 | #define N3F_D(x) { 					\
257 |     if(_gle_gc -> n3d_gen_texture) (*(_gle_gc -> n3d_gen_texture))(x); \
258 |     glNormal3dv(x); 				\
259 | }
260 | 
261 | #define V3F_F(x,j,id) { 					\
262 |     if(_gle_gc -> v3f_gen_texture) (*(_gle_gc -> v3f_gen_texture))(x,j,id);\
263 |     glVertex3fv(x); 				\
264 | }
265 | 
266 | #define V3F_D(x,j,id) { 					\
267 |     if(_gle_gc -> v3d_gen_texture) (*(_gle_gc -> v3d_gen_texture))(x,j,id); \
268 |     glVertex3dv(x); 				\
269 | }
270 | 
271 | #define ENDTMESH() {					\
272 |     if(_gle_gc -> end_gen_texture) (*(_gle_gc -> end_gen_texture))(); \
273 |     glEnd ();					\
274 | }
275 | 
276 | #define ENDPOLYGON() {					\
277 |     if(_gle_gc -> end_gen_texture) (*(_gle_gc -> end_gen_texture))(); \
278 |     glEnd ();					\
279 | }
280 | 
281 | /* ====================================================== */
282 | #else /* AUTO_TEXTURE */
283 | 
284 | #define BGNTMESH(i,len)	glBegin (GL_TRIANGLE_STRIP);
285 | #define BGNPOLYGON() 	glBegin (GL_POLYGON);
286 | 
287 | #define	N3F_F(x)	glNormal3fv(x)
288 | #define	N3F_D(x)	glNormal3dv(x)
289 | #define V3F_F(x,j,id)	glVertex3fv(x);
290 | #define V3F_D(x,j,id)	glVertex3dv(x);
291 | 
292 | #define ENDTMESH()	glEnd ()
293 | #define ENDPOLYGON()	glEnd()
294 | 
295 | #endif /* AUTO_TEXTURE */
296 | 
297 | #endif /* OPENGL_10 */
298 | 
299 | /* ====================================================== */
300 | 
301 | 
302 | #endif /* __GLE_PORT_H__ */
303 | /* ================== END OF FILE ======================= */
304 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/main.cpp:
--------------------------------------------------------------------------------
  1 | #include "main.h"
  2 | 
  3 | #define AVG_ANGLE_COUNT 1
  4 | 
  5 | using namespace std;
  6 | using namespace cv;
  7 | 
  8 | 
  9 | //rectangle algorithm params
 10 | int number_of_rec_cols = 40;
 11 | int number_of_rec_raws = 30; //liczba pasów detekcji //s
 12 | 
 13 | //mats
 14 | Mat test_mat = Mat::zeros(LIDAR_MAT_HEIGHT, LIDAR_MAT_WIDTH, CV_8UC3 );
 15 | Mat output_mat = Mat::zeros(LIDAR_MAT_HEIGHT, LIDAR_MAT_WIDTH, CV_8UC3 );
 16 | 
 17 | Mat  lidar_left_mat = Mat::zeros(LIDAR_MAT_HEIGHT,LIDAR_MAT_WIDTH,CV_8UC3);
 18 | Mat  lidar_right_mat = Mat::zeros(LIDAR_MAT_HEIGHT,LIDAR_MAT_WIDTH,CV_8UC3);
 19 | 
 20 | 
 21 | int main(int argc, char** argv)
 22 | {
 23 |     #if defined(TEST_MODE) || defined (DEBUG_MODE)
 24 |     //init_trackbars();
 25 |     namedWindow("rect_trackbars");
 26 |     init_rect_trackbars("rect_trackbars");//trackbars for rect opt
 27 |     namedWindow("tryb DEBUG",WINDOW_NORMAL);
 28 |     resizeWindow("tryb DEBUG",1000,600);
 29 |     #endif
 30 | 
 31 |     //splines
 32 |     spline_t left_lidar_spline;
 33 |     spline_t right_lidar_spline;
 34 |     spline_t trajectory_path_spline;
 35 | 
 36 |     //set default spline = go straigth
 37 |     vector<Point> straight_vector;
 38 |     straight_vector.push_back(Point(LIDAR_MAT_WIDTH/2,LIDAR_MAT_HEIGHT));
 39 |     straight_vector.push_back(Point(LIDAR_MAT_WIDTH/2,LIDAR_MAT_HEIGHT/2));
 40 |     straight_vector.push_back(Point(LIDAR_MAT_WIDTH/2,0));
 41 | 
 42 |     left_lidar_spline.set_spline(straight_vector,false);
 43 |     right_lidar_spline.set_spline(straight_vector,false);
 44 |     trajectory_path_spline.set_spline(straight_vector,true);
 45 | 
 46 |     //tangents
 47 |     tangent middle_tangent;
 48 |     tangent trajectory_tangent;
 49 | 
 50 |     //sharedmemory
 51 |     SharedMemory left_lidar_points_shm(50001, 12000);
 52 |     vector<Point>left_lidar_vec;
 53 |     left_lidar_points_shm.init(); //init
 54 | 
 55 |     SharedMemory right_lidar_points_shm(50002, 12000);
 56 |     vector<Point>right_lidar_vec;
 57 |     right_lidar_points_shm.init(); //init
 58 | 
 59 |     SharedMemory additional_data_shm(50003,32);
 60 |     vector<uint32_t> add_data_container(7);
 61 |     additional_data_shm.init();
 62 | 
 63 |     //usb communication
 64 |     USB_STM USB_COM;
 65 |     data_container to_send;
 66 |     uint16_t velocity_to_send;
 67 |     uint16_t angle;
 68 | 
 69 |     //USB_COM.init();//init
 70 | 
 71 |     init_trackbars();
 72 | 
 73 |     bool left_line_detect = 0;
 74 |     bool right_line_detect = 0;
 75 | 
 76 | 
 77 | 
 78 |     uint16_t angle_to_send;
 79 |     uint16_t velocity;
 80 | 
 81 |     //offets and wagis
 82 |     float servo_weigth;
 83 |     float ang_div;
 84 |     float far_tg_fac;
 85 | 
 86 |     #ifdef FPS_COUNT
 87 |     // FPS
 88 |     struct timespec start, end;
 89 |     unsigned int frame_count = 0;
 90 |     float seconds = 0;
 91 |     float fps = 0;
 92 |     #endif
 93 | 
 94 |     left_lidar_vec.push_back(Point(300,800));
 95 |     left_lidar_vec.push_back(Point(350,700));
 96 |     left_lidar_vec.push_back(Point(200,600));
 97 | 
 98 |     right_lidar_vec.push_back(Point(500,800));
 99 |     right_lidar_vec.push_back(Point(450,700));
100 |     right_lidar_vec.push_back(Point(480,600));
101 | 
102 | ////////////////////////////////////////////WHILE///////////////////////////////////////////////////
103 | while(1)
104 | {
105 |     #ifdef FPS_COUNT
106 |         // FPS
107 |         if(frame_count == 0)
108 |         {
109 |             clock_gettime(CLOCK_MONOTONIC, &start);
110 |         }
111 |     #endif
112 | 
113 |     #if defined(TEST_MODE) || defined(DEBUG_MODE)
114 |         number_of_rec_cols = rect_slider[0];
115 |         number_of_rec_raws = rect_slider[1];
116 |     #endif
117 | 
118 | 
119 |     //clear buffer which could have another size
120 |     left_lidar_vec.clear();
121 |     right_lidar_vec.clear();
122 | 
123 |     #ifdef DEBUG_MODE
124 | 
125 |     test_mat = Mat::zeros(LIDAR_MAT_HEIGHT, LIDAR_MAT_WIDTH, CV_8UC3 );
126 |     output_mat = Mat::zeros(LIDAR_MAT_HEIGHT, LIDAR_MAT_WIDTH, CV_8UC3 );
127 | 
128 |     lidar_left_mat = Mat::zeros(LIDAR_MAT_HEIGHT,LIDAR_MAT_WIDTH,CV_8UC3);
129 |     lidar_right_mat = Mat::zeros(LIDAR_MAT_HEIGHT,LIDAR_MAT_WIDTH,CV_8UC3);
130 | 
131 | 
132 |     //get new set of points
133 |     left_lidar_points_shm.pull_lidar_data(left_lidar_vec);
134 |     right_lidar_points_shm.pull_lidar_data(right_lidar_vec);
135 | 
136 | 
137 |     //get additional data
138 |     additional_data_shm.pull_add_data(add_data_container);
139 | 
140 |     //preview of received points
141 |     points_preview(right_lidar_vec,lidar_right_mat,CV_RGB(255,255,255));
142 |     points_preview(left_lidar_vec,lidar_left_mat,CV_RGB(255,255,0));
143 | 
144 |     add(lidar_right_mat,lidar_left_mat,lidar_left_mat);
145 |     #endif
146 | 
147 |     //detection flags
148 |     left_line_detect = 0;
149 |     right_line_detect = 0;
150 | 
151 |     //begin y condition
152 |     if(right_lidar_vec.size()>2)
153 |     {
154 |         new_optimization(right_lidar_vec,right_lidar_spline,lidar_right_mat);
155 |         right_line_detect = 1;
156 |     }
157 |     if(left_lidar_vec.size()>2)
158 |     {
159 |         new_optimization(left_lidar_vec,left_lidar_spline,lidar_left_mat);
160 |         left_line_detect = 1;
161 |     }
162 | 
163 | 
164 |     //set path according to lines
165 |     if(left_line_detect == 1 && right_line_detect == 1)
166 |     {
167 |        two_wall_planner(left_lidar_spline,right_lidar_spline,trajectory_path_spline);
168 |        trajectory_tangent.calculate(trajectory_path_spline,rect_slider[3]);
169 |        trajectory_tangent.angle();
170 | 
171 |        middle_tangent.calculate(trajectory_path_spline,200);
172 |        middle_tangent.angle();
173 |     }
174 |     else if(right_line_detect)
175 |     {
176 |        one_line_planner(right_lidar_spline,0,trajectory_path_spline);
177 |        trajectory_tangent.calculate(trajectory_path_spline,rect_slider[3]);
178 |        trajectory_tangent.angle();
179 | 
180 |        middle_tangent.calculate(trajectory_path_spline,200);
181 |        middle_tangent.angle();
182 |     }
183 |     else if(left_line_detect)
184 |     {
185 |        one_line_planner(left_lidar_spline,0,trajectory_path_spline);
186 |        trajectory_tangent.calculate(trajectory_path_spline,rect_slider[3]);
187 |        trajectory_tangent.angle();
188 | 
189 |        middle_tangent.calculate(trajectory_path_spline,200);
190 |        middle_tangent.angle();
191 |     }
192 | 
193 | ///////////////////////////////////////////////////////////////////////////////////////////
194 |      #if defined(RACE_MODE) || defined(DEBUG_MODE)
195 | 
196 |     static uint32_t previous_velocity = 2500;
197 |     servo_weigth = 0.9;
198 |     ang_div = abs(middle_tangent.angle_deg - trajectory_tangent.angle_deg);
199 |     far_tg_fac = 0.8;
200 | 
201 |     //first condition
202 |     if(ang_div>10)
203 |     {
204 |         velocity = 10/ang_div*2500; //*velocity
205 |         servo_weigth = 1;
206 |         far_tg_fac = 0.9;
207 |     }
208 |     else
209 |     {
210 |         uint32_t acceleration = 20;
211 |         velocity = previous_velocity + acceleration;
212 | 
213 |         if(velocity>5000)
214 |             velocity = 5000;
215 |     }
216 |     previous_velocity = velocity;
217 |     angle = 300 + servo_weigth*(far_tg_fac*middle_tangent.angle_deg + (1.0-far_tg_fac)*trajectory_tangent.angle_deg)*10;
218 | 
219 |     angle_to_send = angle;
220 |         //pack data
221 |     //USB_COM.data_pack(velocity_to_send,angle_to_send,&to_send);
222 | 
223 |     //send data to stm
224 |     //USB_COM.send_buf(to_send);
225 |     #endif
226 | 
227 | ////////////////////////////////////////////////////////////////////////////////////////////////////////
228 |      //display section
229 |     string label;
230 |     #ifdef DEBUG_MODE
231 | 
232 |         add(lidar_right_mat,lidar_left_mat,output_mat); // to see rectangles
233 | 
234 |         //draw detected spline
235 |         if(right_line_detect)
236 |             right_lidar_spline.draw(output_mat,CV_RGB(255,255,0));
237 |         if(left_line_detect)
238 |             left_lidar_spline.draw(output_mat,CV_RGB(255,255,255));
239 | 
240 |         trajectory_path_spline.draw(output_mat,CV_RGB(0,255,0));
241 |         trajectory_tangent.draw(output_mat,CV_RGB(100,100,100));
242 |         middle_tangent.draw(output_mat,CV_RGB(255,0,255));
243 | 
244 |         //display send angle
245 |         label  = "send_angle: "+ std::to_string(far_tg_fac*middle_tangent.angle_deg + (1.0-far_tg_fac)*trajectory_tangent.angle_deg);
246 |         putText(output_mat, label, Point(450, 40), FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(0,255,0), 1.0);
247 | 
248 |         //display send speed
249 |         label = "send speed:" + std::to_string(velocity);
250 |         putText(output_mat, label, Point(450, 80), FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(0,255,0), 1.0);
251 | 
252 |         //show white line mat
253 |         imshow("tryb DEBUG",output_mat);
254 | 
255 |         //clean matrixes
256 | 
257 |         if(waitKey(30)=='q')
258 |             break;
259 |     #endif
260 | 
261 |     #ifdef FPS_COUNT
262 |         // FPS
263 |         if(frame_count > FPS_AMOUNT)
264 |         {
265 |             frame_count = 0;
266 |             clock_gettime(CLOCK_MONOTONIC, &end);
267 |             seconds = (end.tv_sec - start.tv_sec);
268 |             fps  =  1 / (seconds / FPS_AMOUNT);
269 | 
270 |             std::cout <<"FPS: " << fps << std::endl;
271 |         }
272 |         else
273 |         {
274 |             frame_count++;
275 |         }
276 |     #endif
277 | }//end of while(1)
278 | 
279 |    //left_lidar_points_shm.close();
280 |    //right_lidar_points_shm.close();
281 |    //additional_data_shm.close();
282 | 
283 |     return 0;
284 | 
285 | }
286 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/spline.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * spline.h
  3 |  *
  4 |  * simple cubic spline interpolation library without external
  5 |  * dependencies
  6 |  *
  7 |  * ---------------------------------------------------------------------
  8 |  * Copyright (C) 2011, 2014 Tino Kluge (ttk448 at gmail.com)
  9 |  *
 10 |  *  This program is free software; you can redistribute it and/or
 11 |  *  modify it under the terms of the GNU General Public License
 12 |  *  as published by the Free Software Foundation; either version 2
 13 |  *  of the License, or (at your option) any later version.
 14 |  *
 15 |  *  This program is distributed in the hope that it will be useful,
 16 |  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 17 |  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 18 |  *  GNU General Public License for more details.
 19 |  *
 20 |  *  You should have received a copy of the GNU General Public License
 21 |  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 22 |  * ---------------------------------------------------------------------
 23 |  *
 24 |  */
 25 | 
 26 | 
 27 | #ifndef TK_SPLINE_H
 28 | #define TK_SPLINE_H
 29 | 
 30 | #include <cstdio>
 31 | #include <cassert>
 32 | #include <vector>
 33 | #include <algorithm>
 34 | 
 35 | 
 36 | // unnamed namespace only because the implementation is in this
 37 | // header file and we don't want to export symbols to the obj files
 38 | namespace
 39 | {
 40 | 
 41 | namespace tk
 42 | {
 43 | 
 44 | // band matrix solver
 45 | class band_matrix
 46 | {
 47 | private:
 48 |     std::vector< std::vector<double> > m_upper;  // upper band
 49 |     std::vector< std::vector<double> > m_lower;  // lower band
 50 | public:
 51 |     band_matrix() {};                             // constructor
 52 |     band_matrix(int dim, int n_u, int n_l);       // constructor
 53 |     ~band_matrix() {};                            // destructor
 54 |     void resize(int dim, int n_u, int n_l);      // init with dim,n_u,n_l
 55 |     int dim() const;                             // matrix dimension
 56 |     int num_upper() const
 57 |     {
 58 |         return m_upper.size()-1;
 59 |     }
 60 |     int num_lower() const
 61 |     {
 62 |         return m_lower.size()-1;
 63 |     }
 64 |     // access operator
 65 |     double & operator () (int i, int j);            // write
 66 |     double   operator () (int i, int j) const;      // read
 67 |     // we can store an additional diogonal (in m_lower)
 68 |     double& saved_diag(int i);
 69 |     double  saved_diag(int i) const;
 70 |     void lu_decompose();
 71 |     std::vector<double> r_solve(const std::vector<double>& b) const;
 72 |     std::vector<double> l_solve(const std::vector<double>& b) const;
 73 |     std::vector<double> lu_solve(const std::vector<double>& b,
 74 |                                  bool is_lu_decomposed=false);
 75 | 
 76 | };
 77 | 
 78 | 
 79 | // spline interpolation
 80 | class spline
 81 | {
 82 | public:
 83 |     enum bd_type {
 84 |         first_deriv = 1,
 85 |         second_deriv = 2
 86 |     };
 87 | 
 88 | 
 89 |     std::vector<double> m_x,m_y;            // x,y coordinates of points
 90 |     // interpolation parameters
 91 |     // f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
 92 |     std::vector<double> m_a,m_b,m_c;        // spline coefficients
 93 |     double  m_b0, m_c0;                     // for left extrapol
 94 |     bd_type m_left, m_right;
 95 |     double  m_left_value, m_right_value;
 96 |     bool    m_force_linear_extrapolation;
 97 | 
 98 | public:
 99 |     // set default boundary condition to be zero curvature at both ends
100 |     spline(): m_left(second_deriv), m_right(second_deriv),
101 |         m_left_value(0.0), m_right_value(0.0),
102 |         m_force_linear_extrapolation(false)
103 |     {
104 |         ;
105 |     }
106 | 
107 |     // optional, but if called it has to come be before set_points()
108 |     void set_boundary(bd_type left, double left_value,
109 |                       bd_type right, double right_value,
110 |                       bool force_linear_extrapolation=false);
111 |     void set_points(const std::vector<double>& x,
112 |                     const std::vector<double>& y, bool cubic_spline=true);
113 |     double operator() (double x) const;
114 |     double deriv(int order, double x) const;
115 | };
116 | 
117 | 
118 | 
119 | // ---------------------------------------------------------------------
120 | // implementation part, which could be separated into a cpp file
121 | // ---------------------------------------------------------------------
122 | 
123 | 
124 | // band_matrix implementation
125 | // -------------------------
126 | 
127 | band_matrix::band_matrix(int dim, int n_u, int n_l)
128 | {
129 |     resize(dim, n_u, n_l);
130 | }
131 | void band_matrix::resize(int dim, int n_u, int n_l)
132 | {
133 |     assert(dim>0);
134 |     assert(n_u>=0);
135 |     assert(n_l>=0);
136 |     m_upper.resize(n_u+1);
137 |     m_lower.resize(n_l+1);
138 |     for(size_t i=0; i<m_upper.size(); i++) {
139 |         m_upper[i].resize(dim);
140 |     }
141 |     for(size_t i=0; i<m_lower.size(); i++) {
142 |         m_lower[i].resize(dim);
143 |     }
144 | }
145 | int band_matrix::dim() const
146 | {
147 |     if(m_upper.size()>0) {
148 |         return m_upper[0].size();
149 |     } else {
150 |         return 0;
151 |     }
152 | }
153 | 
154 | 
155 | // defines the new operator (), so that we can access the elements
156 | // by A(i,j), index going from i=0,...,dim()-1
157 | double & band_matrix::operator () (int i, int j)
158 | {
159 |     int k=j-i;       // what band is the entry
160 |     assert( (i>=0) && (i<dim()) && (j>=0) && (j<dim()) );
161 |     assert( (-num_lower()<=k) && (k<=num_upper()) );
162 |     // k=0 -> diogonal, k<0 lower left part, k>0 upper right part
163 |     if(k>=0)   return m_upper[k][i];
164 |     else	    return m_lower[-k][i];
165 | }
166 | double band_matrix::operator () (int i, int j) const
167 | {
168 |     int k=j-i;       // what band is the entry
169 |     assert( (i>=0) && (i<dim()) && (j>=0) && (j<dim()) );
170 |     assert( (-num_lower()<=k) && (k<=num_upper()) );
171 |     // k=0 -> diogonal, k<0 lower left part, k>0 upper right part
172 |     if(k>=0)   return m_upper[k][i];
173 |     else	    return m_lower[-k][i];
174 | }
175 | // second diag (used in LU decomposition), saved in m_lower
176 | double band_matrix::saved_diag(int i) const
177 | {
178 |     assert( (i>=0) && (i<dim()) );
179 |     return m_lower[0][i];
180 | }
181 | double & band_matrix::saved_diag(int i)
182 | {
183 |     assert( (i>=0) && (i<dim()) );
184 |     return m_lower[0][i];
185 | }
186 | 
187 | // LR-Decomposition of a band matrix
188 | void band_matrix::lu_decompose()
189 | {
190 |     int  i_max,j_max;
191 |     int  j_min;
192 |     double x;
193 | 
194 |     // preconditioning
195 |     // normalize column i so that a_ii=1
196 |     for(int i=0; i<this->dim(); i++) {
197 |         assert(this->operator()(i,i)!=0.0);
198 |         this->saved_diag(i)=1.0/this->operator()(i,i);
199 |         j_min=std::max(0,i-this->num_lower());
200 |         j_max=std::min(this->dim()-1,i+this->num_upper());
201 |         for(int j=j_min; j<=j_max; j++) {
202 |             this->operator()(i,j) *= this->saved_diag(i);
203 |         }
204 |         this->operator()(i,i)=1.0;          // prevents rounding errors
205 |     }
206 | 
207 |     // Gauss LR-Decomposition
208 |     for(int k=0; k<this->dim(); k++) {
209 |         i_max=std::min(this->dim()-1,k+this->num_lower());  // num_lower not a mistake!
210 |         for(int i=k+1; i<=i_max; i++) {
211 |             assert(this->operator()(k,k)!=0.0);
212 |             x=-this->operator()(i,k)/this->operator()(k,k);
213 |             this->operator()(i,k)=-x;                         // assembly part of L
214 |             j_max=std::min(this->dim()-1,k+this->num_upper());
215 |             for(int j=k+1; j<=j_max; j++) {
216 |                 // assembly part of R
217 |                 this->operator()(i,j)=this->operator()(i,j)+x*this->operator()(k,j);
218 |             }
219 |         }
220 |     }
221 | }
222 | // solves Ly=b
223 | std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const
224 | {
225 |     assert( this->dim()==(int)b.size() );
226 |     std::vector<double> x(this->dim());
227 |     int j_start;
228 |     double sum;
229 |     for(int i=0; i<this->dim(); i++) {
230 |         sum=0;
231 |         j_start=std::max(0,i-this->num_lower());
232 |         for(int j=j_start; j<i; j++) sum += this->operator()(i,j)*x[j];
233 |         x[i]=(b[i]*this->saved_diag(i)) - sum;
234 |     }
235 |     return x;
236 | }
237 | // solves Rx=y
238 | std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const
239 | {
240 |     assert( this->dim()==(int)b.size() );
241 |     std::vector<double> x(this->dim());
242 |     int j_stop;
243 |     double sum;
244 |     for(int i=this->dim()-1; i>=0; i--) {
245 |         sum=0;
246 |         j_stop=std::min(this->dim()-1,i+this->num_upper());
247 |         for(int j=i+1; j<=j_stop; j++) sum += this->operator()(i,j)*x[j];
248 |         x[i]=( b[i] - sum ) / this->operator()(i,i);
249 |     }
250 |     return x;
251 | }
252 | 
253 | std::vector<double> band_matrix::lu_solve(const std::vector<double>& b,
254 |         bool is_lu_decomposed)
255 | {
256 |     assert( this->dim()==(int)b.size() );
257 |     std::vector<double>  x,y;
258 |     if(is_lu_decomposed==false) {
259 |         this->lu_decompose();
260 |     }
261 |     y=this->l_solve(b);
262 |     x=this->r_solve(y);
263 |     return x;
264 | }
265 | 
266 | 
267 | 
268 | 
269 | // spline implementation
270 | // -----------------------
271 | 
272 | void spline::set_boundary(spline::bd_type left, double left_value,
273 |                           spline::bd_type right, double right_value,
274 |                           bool force_linear_extrapolation)
275 | {
276 |     assert(m_x.size()==0);          // set_points() must not have happened yet
277 |     m_left=left;
278 |     m_right=right;
279 |     m_left_value=left_value;
280 |     m_right_value=right_value;
281 |     m_force_linear_extrapolation=force_linear_extrapolation;
282 | }
283 | 
284 | 
285 | void spline::set_points(const std::vector<double>& x,
286 |                         const std::vector<double>& y, bool cubic_spline)
287 | {
288 |     assert(x.size()==y.size());
289 |     assert(x.size()>2);
290 |     m_x=x;
291 |     m_y=y;
292 |     int   n=x.size();
293 |     // TODO: maybe sort x and y, rather than returning an error
294 |     for(int i=0; i<n-1; i++) {
295 | 
296 |         //assert(m_x[i]<m_x[i+1]);
297 | 
298 |         //assert(m_x[i]<m_x[i+1]);
299 | 
300 |     }
301 | 
302 |     if(cubic_spline==true) { // cubic spline interpolation
303 |         // setting up the matrix and right hand side of the equation system
304 |         // for the parameters b[]
305 |         band_matrix A(n,1,1);
306 |         std::vector<double>  rhs(n);
307 |         for(int i=1; i<n-1; i++) {
308 |             A(i,i-1)=1.0/3.0*(x[i]-x[i-1]);
309 |             A(i,i)=2.0/3.0*(x[i+1]-x[i-1]);
310 |             A(i,i+1)=1.0/3.0*(x[i+1]-x[i]);
311 |             rhs[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
312 |         }
313 |         // boundary conditions
314 |         if(m_left == spline::second_deriv) {
315 |             // 2*b[0] = f''
316 |             A(0,0)=2.0;
317 |             A(0,1)=0.0;
318 |             rhs[0]=m_left_value;
319 |         } else if(m_left == spline::first_deriv) {
320 |             // c[0] = f', needs to be re-expressed in terms of b:
321 |             // (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
322 |             A(0,0)=2.0*(x[1]-x[0]);
323 |             A(0,1)=1.0*(x[1]-x[0]);
324 |             rhs[0]=3.0*((y[1]-y[0])/(x[1]-x[0])-m_left_value);
325 |         } else {
326 |             assert(false);
327 |         }
328 |         if(m_right == spline::second_deriv) {
329 |             // 2*b[n-1] = f''
330 |             A(n-1,n-1)=2.0;
331 |             A(n-1,n-2)=0.0;
332 |             rhs[n-1]=m_right_value;
333 |         } else if(m_right == spline::first_deriv) {
334 |             // c[n-1] = f', needs to be re-expressed in terms of b:
335 |             // (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
336 |             // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
337 |             A(n-1,n-1)=2.0*(x[n-1]-x[n-2]);
338 |             A(n-1,n-2)=1.0*(x[n-1]-x[n-2]);
339 |             rhs[n-1]=3.0*(m_right_value-(y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
340 |         } else {
341 |             assert(false);
342 |         }
343 | 
344 |         // solve the equation system to obtain the parameters b[]
345 |         m_b=A.lu_solve(rhs);
346 | 
347 |         // calculate parameters a[] and c[] based on b[]
348 |         m_a.resize(n);
349 |         m_c.resize(n);
350 |         for(int i=0; i<n-1; i++) {
351 |             m_a[i]=1.0/3.0*(m_b[i+1]-m_b[i])/(x[i+1]-x[i]);
352 |             m_c[i]=(y[i+1]-y[i])/(x[i+1]-x[i])
353 |                    - 1.0/3.0*(2.0*m_b[i]+m_b[i+1])*(x[i+1]-x[i]);
354 |         }
355 |     } else { // linear interpolation
356 |         m_a.resize(n);
357 |         m_b.resize(n);
358 |         m_c.resize(n);
359 |         for(int i=0; i<n-1; i++) {
360 |             m_a[i]=0.0;
361 |             m_b[i]=0.0;
362 |             m_c[i]=(m_y[i+1]-m_y[i])/(m_x[i+1]-m_x[i]);
363 |         }
364 |     }
365 | 
366 |     // for left extrapolation coefficients
367 |     m_b0 = (m_force_linear_extrapolation==false) ? m_b[0] : 0.0;
368 |     m_c0 = m_c[0];
369 | 
370 |     // for the right extrapolation coefficients
371 |     // f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
372 |     double h=x[n-1]-x[n-2];
373 |     // m_b[n-1] is determined by the boundary condition
374 |     m_a[n-1]=0.0;
375 |     m_c[n-1]=3.0*m_a[n-2]*h*h+2.0*m_b[n-2]*h+m_c[n-2];   // = f'_{n-2}(x_{n-1})
376 |     if(m_force_linear_extrapolation==true)
377 |         m_b[n-1]=0.0;
378 | }
379 | 
380 | double spline::operator() (double x) const
381 | {
382 |     size_t n=m_x.size();
383 |     // find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
384 |     std::vector<double>::const_iterator it;
385 |     it=std::lower_bound(m_x.begin(),m_x.end(),x);
386 |     int idx=std::max( int(it-m_x.begin())-1, 0);
387 | 
388 |     double h=x-m_x[idx];
389 |     double interpol;
390 |     if(x<m_x[0]) {
391 |         // extrapolation to the left
392 |         interpol=(m_b0*h + m_c0)*h + m_y[0];
393 |     } else if(x>m_x[n-1]) {
394 |         // extrapolation to the right
395 |         interpol=(m_b[n-1]*h + m_c[n-1])*h + m_y[n-1];
396 |     } else {
397 |         // interpolation
398 |         interpol=((m_a[idx]*h + m_b[idx])*h + m_c[idx])*h + m_y[idx];
399 |     }
400 |     return interpol;
401 | }
402 | 
403 | double spline::deriv(int order, double x) const
404 | {
405 |     assert(order>0);
406 | 
407 |     size_t n=m_x.size();
408 |     // find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
409 |     std::vector<double>::const_iterator it;
410 |     it=std::lower_bound(m_x.begin(),m_x.end(),x);
411 |     int idx=std::max( int(it-m_x.begin())-1, 0);
412 | 
413 |     double h=x-m_x[idx];
414 |     double interpol;
415 |     if(x<m_x[0]) {
416 |         // extrapolation to the left
417 |         switch(order) {
418 |         case 1:
419 |             interpol=2.0*m_b0*h + m_c0;
420 |             break;
421 |         case 2:
422 |             interpol=2.0*m_b0*h;
423 |             break;
424 |         default:
425 |             interpol=0.0;
426 |             break;
427 |         }
428 |     } else if(x>m_x[n-1]) {
429 |         // extrapolation to the right
430 |         switch(order) {
431 |         case 1:
432 |             interpol=2.0*m_b[n-1]*h + m_c[n-1];
433 |             break;
434 |         case 2:
435 |             interpol=2.0*m_b[n-1];
436 |             break;
437 |         default:
438 |             interpol=0.0;
439 |             break;
440 |         }
441 |     } else {
442 |         // interpolation
443 |         switch(order) {
444 |         case 1:
445 |             interpol=(3.0*m_a[idx]*h + 2.0*m_b[idx])*h + m_c[idx];
446 |             break;
447 |         case 2:
448 |             interpol=6.0*m_a[idx]*h + 2.0*m_b[idx];
449 |             break;
450 |         case 3:
451 |             interpol=6.0*m_a[idx];
452 |             break;
453 |         default:
454 |             interpol=0.0;
455 |             break;
456 |         }
457 |     }
458 |     return interpol;
459 | }
460 | 
461 | 
462 | 
463 | } // namespace tk
464 | 
465 | 
466 | } // namespace
467 | 
468 | #endif /* TK_SPLINE_H */
469 | 


--------------------------------------------------------------------------------
/trajectory-app/trajectory-app/include/vvector.h:
--------------------------------------------------------------------------------
   1 | 
   2 | /*
   3 |  * vvector.h
   4 |  *
   5 |  * FUNCTION:
   6 |  * This file contains a number of utilities useful for handling
   7 |  * 3D vectors
   8 |  * 
   9 |  * HISTORY:
  10 |  * Written by Linas Vepstas, August 1991
  11 |  * Added 2D code, March 1993
  12 |  * Added Outer products, C++ proofed, Linas Vepstas October 1993
  13 |  */
  14 | 
  15 | #ifndef __GUTIL_VECTOR_H__
  16 | #define __GUTIL_VECTOR_H__
  17 | 
  18 | #if defined(__cplusplus) || defined(c_plusplus)
  19 | extern "C" {
  20 | #endif
  21 | 
  22 | 
  23 | #include <math.h>
  24 | #include "matrix.h"
  25 | 
  26 | /* ========================================================== */
  27 | /* Zero out a 2D vector */
  28 | 
  29 | #define VEC_ZERO_2(a)				\
  30 | {						\
  31 |    (a)[0] = (a)[1] = 0.0;			\
  32 | }
  33 | 
  34 | /* ========================================================== */
  35 | /* Zero out a 3D vector */
  36 | 
  37 | #define VEC_ZERO(a)				\
  38 | {						\
  39 |    (a)[0] = (a)[1] = (a)[2] = 0.0;		\
  40 | }
  41 | 
  42 | /* ========================================================== */
  43 | /* Zero out a 4D vector */
  44 | 
  45 | #define VEC_ZERO_4(a)				\
  46 | {						\
  47 |    (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0;	\
  48 | }
  49 | 
  50 | /* ========================================================== */
  51 | /* Vector copy */
  52 | 
  53 | #define VEC_COPY_2(b,a)				\
  54 | {						\
  55 |    (b)[0] = (a)[0];				\
  56 |    (b)[1] = (a)[1];				\
  57 | }
  58 | 
  59 | /* ========================================================== */
  60 | /* Copy 3D vector */
  61 | 
  62 | #define VEC_COPY(b,a)				\
  63 | {						\
  64 |    (b)[0] = (a)[0];				\
  65 |    (b)[1] = (a)[1];				\
  66 |    (b)[2] = (a)[2];				\
  67 | }
  68 | 
  69 | /* ========================================================== */
  70 | /* Copy 4D vector */
  71 | 
  72 | #define VEC_COPY_4(b,a)				\
  73 | {						\
  74 |    (b)[0] = (a)[0];				\
  75 |    (b)[1] = (a)[1];				\
  76 |    (b)[2] = (a)[2];				\
  77 |    (b)[3] = (a)[3];				\
  78 | }
  79 | 
  80 | /* ========================================================== */
  81 | /* Vector difference */
  82 | 
  83 | #define VEC_DIFF_2(v21,v2,v1)			\
  84 | {						\
  85 |    (v21)[0] = (v2)[0] - (v1)[0];		\
  86 |    (v21)[1] = (v2)[1] - (v1)[1];		\
  87 | }
  88 | 
  89 | /* ========================================================== */
  90 | /* Vector difference */
  91 | 
  92 | #define VEC_DIFF(v21,v2,v1)			\
  93 | {						\
  94 |    (v21)[0] = (v2)[0] - (v1)[0];		\
  95 |    (v21)[1] = (v2)[1] - (v1)[1];		\
  96 |    (v21)[2] = (v2)[2] - (v1)[2];		\
  97 | }
  98 | 
  99 | /* ========================================================== */
 100 | /* Vector difference */
 101 | 
 102 | #define VEC_DIFF_4(v21,v2,v1)			\
 103 | {						\
 104 |    (v21)[0] = (v2)[0] - (v1)[0];		\
 105 |    (v21)[1] = (v2)[1] - (v1)[1];		\
 106 |    (v21)[2] = (v2)[2] - (v1)[2];		\
 107 |    (v21)[3] = (v2)[3] - (v1)[3];		\
 108 | }
 109 | 
 110 | /* ========================================================== */
 111 | /* Vector sum */
 112 | 
 113 | #define VEC_SUM_2(v21,v2,v1)			\
 114 | {						\
 115 |    (v21)[0] = (v2)[0] + (v1)[0];		\
 116 |    (v21)[1] = (v2)[1] + (v1)[1];		\
 117 | }
 118 | 
 119 | /* ========================================================== */
 120 | /* Vector sum */
 121 | 
 122 | #define VEC_SUM(v21,v2,v1)			\
 123 | {						\
 124 |    (v21)[0] = (v2)[0] + (v1)[0];		\
 125 |    (v21)[1] = (v2)[1] + (v1)[1];		\
 126 |    (v21)[2] = (v2)[2] + (v1)[2];		\
 127 | }
 128 | 
 129 | /* ========================================================== */
 130 | /* Vector sum */
 131 | 
 132 | #define VEC_SUM_4(v21,v2,v1)			\
 133 | {						\
 134 |    (v21)[0] = (v2)[0] + (v1)[0];		\
 135 |    (v21)[1] = (v2)[1] + (v1)[1];		\
 136 |    (v21)[2] = (v2)[2] + (v1)[2];		\
 137 |    (v21)[3] = (v2)[3] + (v1)[3];		\
 138 | }
 139 | 
 140 | /* ========================================================== */
 141 | /* scalar times vector */
 142 | 
 143 | #define VEC_SCALE_2(c,a,b)			\
 144 | {						\
 145 |    (c)[0] = (a)*(b)[0];				\
 146 |    (c)[1] = (a)*(b)[1];				\
 147 | }
 148 | 
 149 | /* ========================================================== */
 150 | /* scalar times vector */
 151 | 
 152 | #define VEC_SCALE(c,a,b)			\
 153 | {						\
 154 |    (c)[0] = (a)*(b)[0];				\
 155 |    (c)[1] = (a)*(b)[1];				\
 156 |    (c)[2] = (a)*(b)[2];				\
 157 | }
 158 | 
 159 | /* ========================================================== */
 160 | /* scalar times vector */
 161 | 
 162 | #define VEC_SCALE_4(c,a,b)			\
 163 | {						\
 164 |    (c)[0] = (a)*(b)[0];				\
 165 |    (c)[1] = (a)*(b)[1];				\
 166 |    (c)[2] = (a)*(b)[2];				\
 167 |    (c)[3] = (a)*(b)[3];				\
 168 | }
 169 | 
 170 | /* ========================================================== */
 171 | /* accumulate scaled vector */
 172 | 
 173 | #define VEC_ACCUM_2(c,a,b)			\
 174 | {						\
 175 |    (c)[0] += (a)*(b)[0];			\
 176 |    (c)[1] += (a)*(b)[1];			\
 177 | }
 178 | 
 179 | /* ========================================================== */
 180 | /* accumulate scaled vector */
 181 | 
 182 | #define VEC_ACCUM(c,a,b)			\
 183 | {						\
 184 |    (c)[0] += (a)*(b)[0];			\
 185 |    (c)[1] += (a)*(b)[1];			\
 186 |    (c)[2] += (a)*(b)[2];			\
 187 | }
 188 | 
 189 | /* ========================================================== */
 190 | /* accumulate scaled vector */
 191 | 
 192 | #define VEC_ACCUM_4(c,a,b)			\
 193 | {						\
 194 |    (c)[0] += (a)*(b)[0];			\
 195 |    (c)[1] += (a)*(b)[1];			\
 196 |    (c)[2] += (a)*(b)[2];			\
 197 |    (c)[3] += (a)*(b)[3];			\
 198 | }
 199 | 
 200 | /* ========================================================== */
 201 | /* Vector dot product */
 202 | 
 203 | #define VEC_DOT_PRODUCT_2(c,a,b)			\
 204 | {							\
 205 |    c = (a)[0]*(b)[0] + (a)[1]*(b)[1];			\
 206 | }
 207 | 
 208 | /* ========================================================== */
 209 | /* Vector dot product */
 210 | 
 211 | #define VEC_DOT_PRODUCT(c,a,b)				\
 212 | {							\
 213 |    c = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2];	\
 214 | }
 215 | 
 216 | /* ========================================================== */
 217 | /* Vector dot product */
 218 | 
 219 | #define VEC_DOT_PRODUCT_4(c,a,b)			\
 220 | {							\
 221 |    c = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3] ;	\
 222 | }
 223 | 
 224 | /* ========================================================== */
 225 | /* vector impact parameter (squared) */
 226 | 
 227 | #define VEC_IMPACT_SQ(bsq,direction,position)		\
 228 | {							\
 229 |    gleDouble len, llel;					\
 230 |    VEC_DOT_PRODUCT (len, position, position);		\
 231 |    VEC_DOT_PRODUCT (llel, direction, position);		\
 232 |    bsq = len - llel*llel;				\
 233 | }
 234 | 
 235 | /* ========================================================== */
 236 | /* vector impact parameter */
 237 | 
 238 | #define VEC_IMPACT(bsq,direction,position)		\
 239 | {							\
 240 |    VEC_IMPACT_SQ(bsq,direction,position);		\
 241 |    bsq = sqrt (bsq);					\
 242 | }
 243 | 
 244 | /* ========================================================== */
 245 | /* Vector length */
 246 | 
 247 | #define VEC_LENGTH_2(len,a)			\
 248 | {						\
 249 |    len = a[0]*a[0] + a[1]*a[1];			\
 250 |    len = sqrt (len);				\
 251 | }
 252 | 
 253 | /* ========================================================== */
 254 | /* Vector length */
 255 | 
 256 | #define VEC_LENGTH(len,a)			\
 257 | {						\
 258 |    len = (a)[0]*(a)[0] + (a)[1]*(a)[1];		\
 259 |    len += (a)[2]*(a)[2];			\
 260 |    len = sqrt (len);				\
 261 | }
 262 | 
 263 | /* ========================================================== */
 264 | /* Vector length */
 265 | 
 266 | #define VEC_LENGTH_4(len,a)			\
 267 | {						\
 268 |    len = (a)[0]*(a)[0] + (a)[1]*(a)[1];		\
 269 |    len += (a)[2]*(a)[2];			\
 270 |    len += (a)[3] * (a)[3];			\
 271 |    len = sqrt (len);				\
 272 | }
 273 | 
 274 | /* ========================================================== */
 275 | /* distance between two points */
 276 | 
 277 | #define VEC_DISTANCE(len,va,vb)			\
 278 | {						\
 279 |     gleDouble tmp[4];				\
 280 |     VEC_DIFF (tmp, vb, va);			\
 281 |     VEC_LENGTH (len, tmp);			\
 282 | }
 283 | 
 284 | /* ========================================================== */
 285 | /* Vector length */
 286 | 
 287 | #define VEC_CONJUGATE_LENGTH(len,a)		\
 288 | {						\
 289 |    len = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
 290 |    len = sqrt (len);				\
 291 | }
 292 | 
 293 | /* ========================================================== */
 294 | /* Vector length */
 295 | 
 296 | #define VEC_NORMALIZE(a)			\
 297 | {						\
 298 |    double len;					\
 299 |    VEC_LENGTH (len,a);				\
 300 |    if (len != 0.0) {				\
 301 |       len = 1.0 / len;				\
 302 |       a[0] *= len;				\
 303 |       a[1] *= len;				\
 304 |       a[2] *= len;				\
 305 |    }						\
 306 | }
 307 | 
 308 | /* ========================================================== */
 309 | /* Vector length */
 310 | 
 311 | #define VEC_RENORMALIZE(a,newlen)		\
 312 | {						\
 313 |    double len;					\
 314 |    VEC_LENGTH (len,a);				\
 315 |    if (len != 0.0) {				\
 316 |       len = newlen / len;				\
 317 |       a[0] *= len;				\
 318 |       a[1] *= len;				\
 319 |       a[2] *= len;				\
 320 |    }						\
 321 | }
 322 | 
 323 | /* ========================================================== */
 324 | /* 3D Vector cross product yeilding vector */
 325 | 
 326 | #define VEC_CROSS_PRODUCT(c,a,b)		\
 327 | {						\
 328 |    c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];	\
 329 |    c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];	\
 330 |    c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];	\
 331 | }
 332 | 
 333 | /* ========================================================== */
 334 | /* Vector perp -- assumes that n is of unit length 
 335 |  * accepts vector v, subtracts out any component parallel to n */
 336 | 
 337 | #define VEC_PERP(vp,v,n)			\
 338 | {						\
 339 |    double dot;					\
 340 | 						\
 341 |    VEC_DOT_PRODUCT (dot, v, n);			\
 342 |    vp[0] = (v)[0] - (dot) * (n)[0];		\
 343 |    vp[1] = (v)[1] - (dot) * (n)[1];		\
 344 |    vp[2] = (v)[2] - (dot) * (n)[2];		\
 345 | }
 346 | 
 347 | /* ========================================================== */
 348 | /* Vector parallel -- assumes that n is of unit length 
 349 |  * accepts vector v, subtracts out any component perpendicular to n */
 350 | 
 351 | #define VEC_PARALLEL(vp,v,n)			\
 352 | {						\
 353 |    double dot;					\
 354 | 						\
 355 |    VEC_DOT_PRODUCT (dot, v, n);			\
 356 |    vp[0] = (dot) * (n)[0];			\
 357 |    vp[1] = (dot) * (n)[1];			\
 358 |    vp[2] = (dot) * (n)[2];			\
 359 | }
 360 | 
 361 | /* ========================================================== */
 362 | /* Vector reflection -- assumes n is of unit length */
 363 | /* Takes vector v, reflects it against reflector n, and returns vr */
 364 | 
 365 | #define VEC_REFLECT(vr,v,n)			\
 366 | {						\
 367 |    double dot;					\
 368 | 						\
 369 |    VEC_DOT_PRODUCT (dot, v, n);			\
 370 |    vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];	\
 371 |    vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];	\
 372 |    vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];	\
 373 | }
 374 | 
 375 | /* ========================================================== */
 376 | /* Vector blending */
 377 | /* Takes two vectors a, b, blends them together */ 
 378 | 
 379 | #define VEC_BLEND(vr,sa,a,sb,b)			\
 380 | {						\
 381 | 						\
 382 |    vr[0] = (sa) * (a)[0] + (sb) * (b)[0];	\
 383 |    vr[1] = (sa) * (a)[1] + (sb) * (b)[1];	\
 384 |    vr[2] = (sa) * (a)[2] + (sb) * (b)[2];	\
 385 | }
 386 | 
 387 | /* ========================================================== */
 388 | /* Vector print */
 389 | 
 390 | #define VEC_PRINT_2(a)					\
 391 | {							\
 392 |    double len;						\
 393 |    VEC_LENGTH_2 (len, a);					\
 394 |    printf (" a is %f %f length of a is %f \n", a[0], a[1], len); \
 395 | }
 396 | 
 397 | /* ========================================================== */
 398 | /* Vector print */
 399 | 
 400 | #define VEC_PRINT(a)					\
 401 | {							\
 402 |    double len;						\
 403 |    VEC_LENGTH (len, (a));				\
 404 |    printf (" a is %f %f %f length of a is %f \n", (a)[0], (a)[1], (a)[2], len); \
 405 | }
 406 | 
 407 | /* ========================================================== */
 408 | /* Vector print */
 409 | 
 410 | #define VEC_PRINT_4(a)					\
 411 | {							\
 412 |    double len;						\
 413 |    VEC_LENGTH_4 (len, (a));				\
 414 |    printf (" a is %f %f %f %f length of a is %f \n",	\
 415 |        (a)[0], (a)[1], (a)[2], (a)[3], len);		\
 416 | }
 417 | 
 418 | /* ========================================================== */
 419 | /* print matrix */
 420 | 
 421 | #define MAT_PRINT_4X4(mmm) {				\
 422 |    int i,j;						\
 423 |    printf ("matrix mmm is \n");				\
 424 |    if (mmm == NULL) {					\
 425 |       printf (" Null \n");				\
 426 |    } else {						\
 427 |       for (i=0; i<4; i++) {				\
 428 |          for (j=0; j<4; j++) {				\
 429 |             printf ("%f ", mmm[i][j]);			\
 430 |          }						\
 431 |          printf (" \n");				\
 432 |       }							\
 433 |    }							\
 434 | }
 435 | 
 436 | /* ========================================================== */
 437 | /* print matrix */
 438 | 
 439 | #define MAT_PRINT_3X3(mmm) {				\
 440 |    int i,j;						\
 441 |    printf ("matrix mmm is \n");				\
 442 |    if (mmm == NULL) {					\
 443 |       printf (" Null \n");				\
 444 |    } else {						\
 445 |       for (i=0; i<3; i++) {				\
 446 |          for (j=0; j<3; j++) {				\
 447 |             printf ("%f ", mmm[i][j]);			\
 448 |          }						\
 449 |          printf (" \n");				\
 450 |       }							\
 451 |    }							\
 452 | }
 453 | 
 454 | /* ========================================================== */
 455 | /* print matrix */
 456 | 
 457 | #define MAT_PRINT_2X3(mmm) {				\
 458 |    int i,j;						\
 459 |    printf ("matrix mmm is \n");				\
 460 |    if (mmm == NULL) {					\
 461 |       printf (" Null \n");				\
 462 |    } else {						\
 463 |       for (i=0; i<2; i++) {				\
 464 |          for (j=0; j<3; j++) {				\
 465 |             printf ("%f ", mmm[i][j]);			\
 466 |          }						\
 467 |          printf (" \n");				\
 468 |       }							\
 469 |    }							\
 470 | }
 471 | 
 472 | /* ========================================================== */
 473 | /* initialize matrix */
 474 | 
 475 | #define IDENTIFY_MATRIX_3X3(m)			\
 476 | {						\
 477 |    m[0][0] = 1.0;				\
 478 |    m[0][1] = 0.0;				\
 479 |    m[0][2] = 0.0;				\
 480 | 						\
 481 |    m[1][0] = 0.0;				\
 482 |    m[1][1] = 1.0;				\
 483 |    m[1][2] = 0.0;				\
 484 | 						\
 485 |    m[2][0] = 0.0;				\
 486 |    m[2][1] = 0.0;				\
 487 |    m[2][2] = 1.0;				\
 488 | }
 489 | 
 490 | /* ========================================================== */
 491 | /* initialize matrix */
 492 | 
 493 | #define IDENTIFY_MATRIX_4X4(m)			\
 494 | {						\
 495 |    m[0][0] = 1.0;				\
 496 |    m[0][1] = 0.0;				\
 497 |    m[0][2] = 0.0;				\
 498 |    m[0][3] = 0.0;				\
 499 | 						\
 500 |    m[1][0] = 0.0;				\
 501 |    m[1][1] = 1.0;				\
 502 |    m[1][2] = 0.0;				\
 503 |    m[1][3] = 0.0;				\
 504 | 						\
 505 |    m[2][0] = 0.0;				\
 506 |    m[2][1] = 0.0;				\
 507 |    m[2][2] = 1.0;				\
 508 |    m[2][3] = 0.0;				\
 509 | 						\
 510 |    m[3][0] = 0.0;				\
 511 |    m[3][1] = 0.0;				\
 512 |    m[3][2] = 0.0;				\
 513 |    m[3][3] = 1.0;				\
 514 | }
 515 | 
 516 | /* ========================================================== */
 517 | /* matrix copy */
 518 | 
 519 | #define COPY_MATRIX_2X2(b,a)	\
 520 | {				\
 521 |    b[0][0] = a[0][0];		\
 522 |    b[0][1] = a[0][1];		\
 523 | 				\
 524 |    b[1][0] = a[1][0];		\
 525 |    b[1][1] = a[1][1];		\
 526 | 				\
 527 | }
 528 | 
 529 | /* ========================================================== */
 530 | /* matrix copy */
 531 | 
 532 | #define COPY_MATRIX_2X3(b,a)	\
 533 | {				\
 534 |    b[0][0] = a[0][0];		\
 535 |    b[0][1] = a[0][1];		\
 536 |    b[0][2] = a[0][2];		\
 537 | 				\
 538 |    b[1][0] = a[1][0];		\
 539 |    b[1][1] = a[1][1];		\
 540 |    b[1][2] = a[1][2];		\
 541 | }
 542 | 
 543 | /* ========================================================== */
 544 | /* matrix copy */
 545 | 
 546 | #define COPY_MATRIX_3X3(b,a)	\
 547 | {				\
 548 |    b[0][0] = a[0][0];		\
 549 |    b[0][1] = a[0][1];		\
 550 |    b[0][2] = a[0][2];		\
 551 | 				\
 552 |    b[1][0] = a[1][0];		\
 553 |    b[1][1] = a[1][1];		\
 554 |    b[1][2] = a[1][2];		\
 555 | 				\
 556 |    b[2][0] = a[2][0];		\
 557 |    b[2][1] = a[2][1];		\
 558 |    b[2][2] = a[2][2];		\
 559 | }
 560 | 
 561 | /* ========================================================== */
 562 | /* matrix copy */
 563 | 
 564 | #define COPY_MATRIX_4X4(b,a)	\
 565 | {				\
 566 |    b[0][0] = a[0][0];		\
 567 |    b[0][1] = a[0][1];		\
 568 |    b[0][2] = a[0][2];		\
 569 |    b[0][3] = a[0][3];		\
 570 | 				\
 571 |    b[1][0] = a[1][0];		\
 572 |    b[1][1] = a[1][1];		\
 573 |    b[1][2] = a[1][2];		\
 574 |    b[1][3] = a[1][3];		\
 575 | 				\
 576 |    b[2][0] = a[2][0];		\
 577 |    b[2][1] = a[2][1];		\
 578 |    b[2][2] = a[2][2];		\
 579 |    b[2][3] = a[2][3];		\
 580 | 				\
 581 |    b[3][0] = a[3][0];		\
 582 |    b[3][1] = a[3][1];		\
 583 |    b[3][2] = a[3][2];		\
 584 |    b[3][3] = a[3][3];		\
 585 | }
 586 | 
 587 | /* ========================================================== */
 588 | /* matrix transpose */
 589 | 
 590 | #define TRANSPOSE_MATRIX_2X2(b,a)	\
 591 | {				\
 592 |    b[0][0] = a[0][0];		\
 593 |    b[0][1] = a[1][0];		\
 594 | 				\
 595 |    b[1][0] = a[0][1];		\
 596 |    b[1][1] = a[1][1];		\
 597 | }
 598 | 
 599 | /* ========================================================== */
 600 | /* matrix transpose */
 601 | 
 602 | #define TRANSPOSE_MATRIX_3X3(b,a)	\
 603 | {				\
 604 |    b[0][0] = a[0][0];		\
 605 |    b[0][1] = a[1][0];		\
 606 |    b[0][2] = a[2][0];		\
 607 | 				\
 608 |    b[1][0] = a[0][1];		\
 609 |    b[1][1] = a[1][1];		\
 610 |    b[1][2] = a[2][1];		\
 611 | 				\
 612 |    b[2][0] = a[0][2];		\
 613 |    b[2][1] = a[1][2];		\
 614 |    b[2][2] = a[2][2];		\
 615 | }
 616 | 
 617 | /* ========================================================== */
 618 | /* matrix transpose */
 619 | 
 620 | #define TRANSPOSE_MATRIX_4X4(b,a)	\
 621 | {				\
 622 |    b[0][0] = a[0][0];		\
 623 |    b[0][1] = a[1][0];		\
 624 |    b[0][2] = a[2][0];		\
 625 |    b[0][3] = a[3][0];		\
 626 | 				\
 627 |    b[1][0] = a[0][1];		\
 628 |    b[1][1] = a[1][1];		\
 629 |    b[1][2] = a[2][1];		\
 630 |    b[1][3] = a[3][1];		\
 631 | 				\
 632 |    b[2][0] = a[0][2];		\
 633 |    b[2][1] = a[1][2];		\
 634 |    b[2][2] = a[2][2];		\
 635 |    b[2][3] = a[3][2];		\
 636 | 				\
 637 |    b[3][0] = a[0][3];		\
 638 |    b[3][1] = a[1][3];		\
 639 |    b[3][2] = a[2][3];		\
 640 |    b[3][3] = a[3][3];		\
 641 | }
 642 | 
 643 | /* ========================================================== */
 644 | /* multiply matrix by scalar */
 645 | 
 646 | #define SCALE_MATRIX_2X2(b,s,a)		\
 647 | {					\
 648 |    b[0][0] = (s) * a[0][0];		\
 649 |    b[0][1] = (s) * a[0][1];		\
 650 | 					\
 651 |    b[1][0] = (s) * a[1][0];		\
 652 |    b[1][1] = (s) * a[1][1];		\
 653 | }
 654 | 
 655 | /* ========================================================== */
 656 | /* multiply matrix by scalar */
 657 | 
 658 | #define SCALE_MATRIX_3X3(b,s,a)		\
 659 | {					\
 660 |    b[0][0] = (s) * a[0][0];		\
 661 |    b[0][1] = (s) * a[0][1];		\
 662 |    b[0][2] = (s) * a[0][2];		\
 663 | 					\
 664 |    b[1][0] = (s) * a[1][0];		\
 665 |    b[1][1] = (s) * a[1][1];		\
 666 |    b[1][2] = (s) * a[1][2];		\
 667 | 					\
 668 |    b[2][0] = (s) * a[2][0];		\
 669 |    b[2][1] = (s) * a[2][1];		\
 670 |    b[2][2] = (s) * a[2][2];		\
 671 | }
 672 | 
 673 | /* ========================================================== */
 674 | /* multiply matrix by scalar */
 675 | 
 676 | #define SCALE_MATRIX_4X4(b,s,a)		\
 677 | {					\
 678 |    b[0][0] = (s) * a[0][0];		\
 679 |    b[0][1] = (s) * a[0][1];		\
 680 |    b[0][2] = (s) * a[0][2];		\
 681 |    b[0][3] = (s) * a[0][3];		\
 682 | 					\
 683 |    b[1][0] = (s) * a[1][0];		\
 684 |    b[1][1] = (s) * a[1][1];		\
 685 |    b[1][2] = (s) * a[1][2];		\
 686 |    b[1][3] = (s) * a[1][3];		\
 687 | 					\
 688 |    b[2][0] = (s) * a[2][0];		\
 689 |    b[2][1] = (s) * a[2][1];		\
 690 |    b[2][2] = (s) * a[2][2];		\
 691 |    b[2][3] = (s) * a[2][3];		\
 692 | 					\
 693 |    b[3][0] = s * a[3][0];		\
 694 |    b[3][1] = s * a[3][1];		\
 695 |    b[3][2] = s * a[3][2];		\
 696 |    b[3][3] = s * a[3][3];		\
 697 | }
 698 | 
 699 | /* ========================================================== */
 700 | /* multiply matrix by scalar */
 701 | 
 702 | #define ACCUM_SCALE_MATRIX_2X2(b,s,a)		\
 703 | {					\
 704 |    b[0][0] += (s) * a[0][0];		\
 705 |    b[0][1] += (s) * a[0][1];		\
 706 | 					\
 707 |    b[1][0] += (s) * a[1][0];		\
 708 |    b[1][1] += (s) * a[1][1];		\
 709 | }
 710 | 
 711 | /* +========================================================== */
 712 | /* multiply matrix by scalar */
 713 | 
 714 | #define ACCUM_SCALE_MATRIX_3X3(b,s,a)		\
 715 | {					\
 716 |    b[0][0] += (s) * a[0][0];		\
 717 |    b[0][1] += (s) * a[0][1];		\
 718 |    b[0][2] += (s) * a[0][2];		\
 719 | 					\
 720 |    b[1][0] += (s) * a[1][0];		\
 721 |    b[1][1] += (s) * a[1][1];		\
 722 |    b[1][2] += (s) * a[1][2];		\
 723 | 					\
 724 |    b[2][0] += (s) * a[2][0];		\
 725 |    b[2][1] += (s) * a[2][1];		\
 726 |    b[2][2] += (s) * a[2][2];		\
 727 | }
 728 | 
 729 | /* +========================================================== */
 730 | /* multiply matrix by scalar */
 731 | 
 732 | #define ACCUM_SCALE_MATRIX_4X4(b,s,a)		\
 733 | {					\
 734 |    b[0][0] += (s) * a[0][0];		\
 735 |    b[0][1] += (s) * a[0][1];		\
 736 |    b[0][2] += (s) * a[0][2];		\
 737 |    b[0][3] += (s) * a[0][3];		\
 738 | 					\
 739 |    b[1][0] += (s) * a[1][0];		\
 740 |    b[1][1] += (s) * a[1][1];		\
 741 |    b[1][2] += (s) * a[1][2];		\
 742 |    b[1][3] += (s) * a[1][3];		\
 743 | 					\
 744 |    b[2][0] += (s) * a[2][0];		\
 745 |    b[2][1] += (s) * a[2][1];		\
 746 |    b[2][2] += (s) * a[2][2];		\
 747 |    b[2][3] += (s) * a[2][3];		\
 748 | 					\
 749 |    b[3][0] += (s) * a[3][0];		\
 750 |    b[3][1] += (s) * a[3][1];		\
 751 |    b[3][2] += (s) * a[3][2];		\
 752 |    b[3][3] += (s) * a[3][3];		\
 753 | }
 754 | 
 755 | /* +========================================================== */
 756 | /* matrix product */
 757 | /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 758 | 
 759 | #define MATRIX_PRODUCT_2X2(c,a,b)		\
 760 | {						\
 761 |    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];	\
 762 |    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];	\
 763 | 						\
 764 |    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];	\
 765 |    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];	\
 766 | 						\
 767 | }
 768 | 
 769 | /* ========================================================== */
 770 | /* matrix product */
 771 | /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 772 | 
 773 | #define MATRIX_PRODUCT_3X3(c,a,b)				\
 774 | {								\
 775 |    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];	\
 776 |    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];	\
 777 |    c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];	\
 778 | 								\
 779 |    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];	\
 780 |    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];	\
 781 |    c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];	\
 782 | 								\
 783 |    c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];	\
 784 |    c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];	\
 785 |    c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];	\
 786 | }
 787 | 
 788 | /* ========================================================== */
 789 | /* matrix product */
 790 | /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 791 | 
 792 | #define MATRIX_PRODUCT_4X4(c,a,b)		\
 793 | {						\
 794 |    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
 795 |    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
 796 |    c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
 797 |    c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
 798 | 						\
 799 |    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
 800 |    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
 801 |    c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
 802 |    c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
 803 | 						\
 804 |    c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
 805 |    c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
 806 |    c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
 807 |    c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
 808 | 						\
 809 |    c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
 810 |    c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
 811 |    c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
 812 |    c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
 813 | }
 814 | 
 815 | /* ========================================================== */
 816 | /* matrix times vector */
 817 | 
 818 | #define MAT_DOT_VEC_2X2(p,m,v)					\
 819 | {								\
 820 |    p[0] = m[0][0]*v[0] + m[0][1]*v[1];				\
 821 |    p[1] = m[1][0]*v[0] + m[1][1]*v[1];				\
 822 | }
 823 | 
 824 | /* ========================================================== */
 825 | /* matrix times vector */
 826 | 
 827 | #define MAT_DOT_VEC_3X3(p,m,v)					\
 828 | {								\
 829 |    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];		\
 830 |    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];		\
 831 |    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];		\
 832 | }
 833 | 
 834 | /* ========================================================== */
 835 | /* matrix times vector */
 836 | 
 837 | #define MAT_DOT_VEC_4X4(p,m,v)					\
 838 | {								\
 839 |    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];	\
 840 |    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];	\
 841 |    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];	\
 842 |    p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];	\
 843 | }
 844 | 
 845 | /* ========================================================== */
 846 | /* vector transpose times matrix */
 847 | /* p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
 848 | 
 849 | #define VEC_DOT_MAT_3X3(p,v,m)					\
 850 | {								\
 851 |    p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];		\
 852 |    p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];		\
 853 |    p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];		\
 854 | }
 855 | 
 856 | /* ========================================================== */
 857 | /* affine matrix times vector */
 858 | /* The matrix is assumed to be an affine matrix, with last two 
 859 |  * entries representing a translation */
 860 | 
 861 | #define MAT_DOT_VEC_2X3(p,m,v)					\
 862 | {								\
 863 |    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];		\
 864 |    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];		\
 865 | }
 866 | 
 867 | /* ========================================================== */
 868 | /* inverse transpose of matrix times vector
 869 |  *
 870 |  * This macro computes inverse transpose of matrix m, 
 871 |  * and multiplies vector v into it, to yeild vector p
 872 |  *
 873 |  * DANGER !!! Do Not use this on normal vectors!!!
 874 |  * It will leave normals the wrong length !!!
 875 |  * See macro below for use on normals.
 876 |  */
 877 | 
 878 | #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)			\
 879 | {								\
 880 |    gleDouble det;						\
 881 | 								\
 882 |    det = m[0][0]*m[1][1] - m[0][1]*m[1][0];			\
 883 |    p[0] = m[1][1]*v[0] - m[1][0]*v[1];				\
 884 |    p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
 885 | 								\
 886 |    /* if matrix not singular, and not orthonormal, then renormalize */ \
 887 |    if ((det!=1.0) && (det != 0.0)) {				\
 888 |       det = 1.0 / det;						\
 889 |       p[0] *= det;						\
 890 |       p[1] *= det;						\
 891 |    }								\
 892 | }
 893 | 
 894 | /* ========================================================== */
 895 | /* transform normal vector by inverse transpose of matrix 
 896 |  * and then renormalize the vector 
 897 |  *
 898 |  * This macro computes inverse transpose of matrix m, 
 899 |  * and multiplies vector v into it, to yeild vector p
 900 |  * Vector p is then normalized.
 901 |  */
 902 | 
 903 | 
 904 | #define NORM_XFORM_2X2(p,m,v)					\
 905 | {								\
 906 |    double len;							\
 907 | 								\
 908 |    /* do nothing if off-diagonals are zero and diagonals are 	\
 909 |     * equal */							\
 910 |    if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
 911 |       p[0] = m[1][1]*v[0] - m[1][0]*v[1];			\
 912 |       p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
 913 | 								\
 914 |       len = p[0]*p[0] + p[1]*p[1];				\
 915 |       len = 1.0 / sqrt (len);					\
 916 |       p[0] *= len;						\
 917 |       p[1] *= len;						\
 918 |    } else {							\
 919 |       VEC_COPY_2 (p, v);					\
 920 |    }								\
 921 | }
 922 | 
 923 | /* ========================================================== */
 924 | /* outer product of vector times vector transpose 
 925 |  *
 926 |  * The outer product of vector v and vector transpose t yeilds 
 927 |  * dyadic matrix m.
 928 |  */
 929 | 
 930 | #define OUTER_PRODUCT_2X2(m,v,t)				\
 931 | {								\
 932 |    m[0][0] = v[0] * t[0];					\
 933 |    m[0][1] = v[0] * t[1];					\
 934 | 								\
 935 |    m[1][0] = v[1] * t[0];					\
 936 |    m[1][1] = v[1] * t[1];					\
 937 | }
 938 | 
 939 | /* ========================================================== */
 940 | /* outer product of vector times vector transpose 
 941 |  *
 942 |  * The outer product of vector v and vector transpose t yeilds 
 943 |  * dyadic matrix m.
 944 |  */
 945 | 
 946 | #define OUTER_PRODUCT_3X3(m,v,t)				\
 947 | {								\
 948 |    m[0][0] = v[0] * t[0];					\
 949 |    m[0][1] = v[0] * t[1];					\
 950 |    m[0][2] = v[0] * t[2];					\
 951 | 								\
 952 |    m[1][0] = v[1] * t[0];					\
 953 |    m[1][1] = v[1] * t[1];					\
 954 |    m[1][2] = v[1] * t[2];					\
 955 | 								\
 956 |    m[2][0] = v[2] * t[0];					\
 957 |    m[2][1] = v[2] * t[1];					\
 958 |    m[2][2] = v[2] * t[2];					\
 959 | }
 960 | 
 961 | /* ========================================================== */
 962 | /* outer product of vector times vector transpose 
 963 |  *
 964 |  * The outer product of vector v and vector transpose t yeilds 
 965 |  * dyadic matrix m.
 966 |  */
 967 | 
 968 | #define OUTER_PRODUCT_4X4(m,v,t)				\
 969 | {								\
 970 |    m[0][0] = v[0] * t[0];					\
 971 |    m[0][1] = v[0] * t[1];					\
 972 |    m[0][2] = v[0] * t[2];					\
 973 |    m[0][3] = v[0] * t[3];					\
 974 | 								\
 975 |    m[1][0] = v[1] * t[0];					\
 976 |    m[1][1] = v[1] * t[1];					\
 977 |    m[1][2] = v[1] * t[2];					\
 978 |    m[1][3] = v[1] * t[3];					\
 979 | 								\
 980 |    m[2][0] = v[2] * t[0];					\
 981 |    m[2][1] = v[2] * t[1];					\
 982 |    m[2][2] = v[2] * t[2];					\
 983 |    m[2][3] = v[2] * t[3];					\
 984 | 								\
 985 |    m[3][0] = v[3] * t[0];					\
 986 |    m[3][1] = v[3] * t[1];					\
 987 |    m[3][2] = v[3] * t[2];					\
 988 |    m[3][3] = v[3] * t[3];					\
 989 | }
 990 | 
 991 | /* +========================================================== */
 992 | /* outer product of vector times vector transpose 
 993 |  *
 994 |  * The outer product of vector v and vector transpose t yeilds 
 995 |  * dyadic matrix m.
 996 |  */
 997 | 
 998 | #define ACCUM_OUTER_PRODUCT_2X2(m,v,t)				\
 999 | {								\
1000 |    m[0][0] += v[0] * t[0];					\
1001 |    m[0][1] += v[0] * t[1];					\
1002 | 								\
1003 |    m[1][0] += v[1] * t[0];					\
1004 |    m[1][1] += v[1] * t[1];					\
1005 | }
1006 | 
1007 | /* +========================================================== */
1008 | /* outer product of vector times vector transpose 
1009 |  *
1010 |  * The outer product of vector v and vector transpose t yeilds 
1011 |  * dyadic matrix m.
1012 |  */
1013 | 
1014 | #define ACCUM_OUTER_PRODUCT_3X3(m,v,t)				\
1015 | {								\
1016 |    m[0][0] += v[0] * t[0];					\
1017 |    m[0][1] += v[0] * t[1];					\
1018 |    m[0][2] += v[0] * t[2];					\
1019 | 								\
1020 |    m[1][0] += v[1] * t[0];					\
1021 |    m[1][1] += v[1] * t[1];					\
1022 |    m[1][2] += v[1] * t[2];					\
1023 | 								\
1024 |    m[2][0] += v[2] * t[0];					\
1025 |    m[2][1] += v[2] * t[1];					\
1026 |    m[2][2] += v[2] * t[2];					\
1027 | }
1028 | 
1029 | /* +========================================================== */
1030 | /* outer product of vector times vector transpose 
1031 |  *
1032 |  * The outer product of vector v and vector transpose t yeilds 
1033 |  * dyadic matrix m.
1034 |  */
1035 | 
1036 | #define ACCUM_OUTER_PRODUCT_4X4(m,v,t)				\
1037 | {								\
1038 |    m[0][0] += v[0] * t[0];					\
1039 |    m[0][1] += v[0] * t[1];					\
1040 |    m[0][2] += v[0] * t[2];					\
1041 |    m[0][3] += v[0] * t[3];					\
1042 | 								\
1043 |    m[1][0] += v[1] * t[0];					\
1044 |    m[1][1] += v[1] * t[1];					\
1045 |    m[1][2] += v[1] * t[2];					\
1046 |    m[1][3] += v[1] * t[3];					\
1047 | 								\
1048 |    m[2][0] += v[2] * t[0];					\
1049 |    m[2][1] += v[2] * t[1];					\
1050 |    m[2][2] += v[2] * t[2];					\
1051 |    m[2][3] += v[2] * t[3];					\
1052 | 								\
1053 |    m[3][0] += v[3] * t[0];					\
1054 |    m[3][1] += v[3] * t[1];					\
1055 |    m[3][2] += v[3] * t[2];					\
1056 |    m[3][3] += v[3] * t[3];					\
1057 | }
1058 | 
1059 | /* +========================================================== */
1060 | /* determinant of matrix
1061 |  *
1062 |  * Computes determinant of matrix m, returning d
1063 |  */
1064 | 
1065 | #define DETERMINANT_2X2(d,m)					\
1066 | {								\
1067 |    d = m[0][0] * m[1][1] - m[0][1] * m[1][0];			\
1068 | }
1069 | 
1070 | /* ========================================================== */
1071 | /* determinant of matrix
1072 |  *
1073 |  * Computes determinant of matrix m, returning d
1074 |  */
1075 | 
1076 | #define DETERMINANT_3X3(d,m)					\
1077 | {								\
1078 |    d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);		\
1079 |    d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);	\
1080 |    d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);	\
1081 | }
1082 | 
1083 | /* ========================================================== */
1084 | /* i,j,th cofactor of a 4x4 matrix
1085 |  *
1086 |  */
1087 | 
1088 | #define COFACTOR_4X4_IJ(fac,m,i,j) 				\
1089 | {								\
1090 |    int ii[4], jj[4], k;						\
1091 | 								\
1092 |    /* compute which row, columnt to skip */			\
1093 |    for (k=0; k<i; k++) ii[k] = k;				\
1094 |    for (k=i; k<3; k++) ii[k] = k+1;				\
1095 |    for (k=0; k<j; k++) jj[k] = k;				\
1096 |    for (k=j; k<3; k++) jj[k] = k+1;				\
1097 | 								\
1098 |    (fac) = m[ii[0]][jj[0]] * (m[ii[1]][jj[1]]*m[ii[2]][jj[2]] 	\
1099 |                             - m[ii[1]][jj[2]]*m[ii[2]][jj[1]]); \
1100 |    (fac) -= m[ii[0]][jj[1]] * (m[ii[1]][jj[0]]*m[ii[2]][jj[2]]	\
1101 |                              - m[ii[1]][jj[2]]*m[ii[2]][jj[0]]);\
1102 |    (fac) += m[ii[0]][jj[2]] * (m[ii[1]][jj[0]]*m[ii[2]][jj[1]]	\
1103 |                              - m[ii[1]][jj[1]]*m[ii[2]][jj[0]]);\
1104 | 								\
1105 |    /* compute sign */						\
1106 |    k = i+j;							\
1107 |    if ( k != (k/2)*2) {						\
1108 |       (fac) = -(fac);						\
1109 |    }								\
1110 | }
1111 | 
1112 | /* ========================================================== */
1113 | /* determinant of matrix
1114 |  *
1115 |  * Computes determinant of matrix m, returning d
1116 |  */
1117 | 
1118 | #define DETERMINANT_4X4(d,m)					\
1119 | {								\
1120 |    double cofac;						\
1121 |    COFACTOR_4X4_IJ (cofac, m, 0, 0);				\
1122 |    d = m[0][0] * cofac;						\
1123 |    COFACTOR_4X4_IJ (cofac, m, 0, 1);				\
1124 |    d += m[0][1] * cofac;					\
1125 |    COFACTOR_4X4_IJ (cofac, m, 0, 2);				\
1126 |    d += m[0][2] * cofac;					\
1127 |    COFACTOR_4X4_IJ (cofac, m, 0, 3);				\
1128 |    d += m[0][3] * cofac;					\
1129 | }
1130 | 
1131 | /* ========================================================== */
1132 | /* cofactor of matrix
1133 |  *
1134 |  * Computes cofactor of matrix m, returning a
1135 |  */
1136 | 
1137 | #define COFACTOR_2X2(a,m)					\
1138 | {								\
1139 |    a[0][0] = (m)[1][1];						\
1140 |    a[0][1] = - (m)[1][0];						\
1141 |    a[1][0] = - (m)[0][1];						\
1142 |    a[1][1] = (m)[0][0];						\
1143 | }
1144 | 
1145 | /* ========================================================== */
1146 | /* cofactor of matrix
1147 |  *
1148 |  * Computes cofactor of matrix m, returning a
1149 |  */
1150 | 
1151 | #define COFACTOR_3X3(a,m)					\
1152 | {								\
1153 |    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
1154 |    a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
1155 |    a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
1156 |    a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
1157 |    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
1158 |    a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
1159 |    a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
1160 |    a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
1161 |    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
1162 | }
1163 | 
1164 | /* ========================================================== */
1165 | /* cofactor of matrix
1166 |  *
1167 |  * Computes cofactor of matrix m, returning a
1168 |  */
1169 | 
1170 | #define COFACTOR_4X4(a,m)					\
1171 | {								\
1172 |    int i,j;							\
1173 | 								\
1174 |    for (i=0; i<4; i++) {					\
1175 |       for (j=0; j<4; j++) {					\
1176 |          COFACTOR_4X4_IJ (a[i][j], m, i, j);			\
1177 |       }								\
1178 |    }								\
1179 | }
1180 | 
1181 | /* ========================================================== */
1182 | /* adjoint of matrix
1183 |  *
1184 |  * Computes adjoint of matrix m, returning a
1185 |  * (Note that adjoint is just the transpose of the cofactor matrix)
1186 |  */
1187 | 
1188 | #define ADJOINT_2X2(a,m)					\
1189 | {								\
1190 |    a[0][0] = (m)[1][1];						\
1191 |    a[1][0] = - (m)[1][0];						\
1192 |    a[0][1] = - (m)[0][1];						\
1193 |    a[1][1] = (m)[0][0];						\
1194 | }
1195 | 
1196 | /* ========================================================== */
1197 | /* adjoint of matrix
1198 |  *
1199 |  * Computes adjoint of matrix m, returning a
1200 |  * (Note that adjoint is just the transpose of the cofactor matrix)
1201 |  */
1202 | 
1203 | 
1204 | #define ADJOINT_3X3(a,m)					\
1205 | {								\
1206 |    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
1207 |    a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
1208 |    a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
1209 |    a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
1210 |    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
1211 |    a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
1212 |    a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
1213 |    a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
1214 |    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
1215 | }
1216 | 
1217 | /* ========================================================== */
1218 | /* adjoint of matrix
1219 |  *
1220 |  * Computes adjoint of matrix m, returning a
1221 |  * (Note that adjoint is just the transpose of the cofactor matrix)
1222 |  */
1223 | 
1224 | #define ADJOINT_4X4(a,m)					\
1225 | {								\
1226 |    int i,j;							\
1227 | 								\
1228 |    for (i=0; i<4; i++) {					\
1229 |       for (j=0; j<4; j++) {					\
1230 |          COFACTOR_4X4_IJ (a[j][i], m, i, j);			\
1231 |       }								\
1232 |    }								\
1233 | }
1234 | 
1235 | /* ========================================================== */
1236 | /* compute adjoint of matrix and scale
1237 |  *
1238 |  * Computes adjoint of matrix m, scales it by s, returning a
1239 |  */
1240 | 
1241 | #define SCALE_ADJOINT_2X2(a,s,m)				\
1242 | {								\
1243 |    a[0][0] = (s) * m[1][1];					\
1244 |    a[1][0] = - (s) * m[1][0];					\
1245 |    a[0][1] = - (s) * m[0][1];					\
1246 |    a[1][1] = (s) * m[0][0];					\
1247 | }
1248 | 
1249 | /* ========================================================== */
1250 | /* compute adjoint of matrix and scale
1251 |  *
1252 |  * Computes adjoint of matrix m, scales it by s, returning a
1253 |  */
1254 | 
1255 | #define SCALE_ADJOINT_3X3(a,s,m)				\
1256 | {								\
1257 |    a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);	\
1258 |    a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);	\
1259 |    a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);	\
1260 | 								\
1261 |    a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);	\
1262 |    a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);	\
1263 |    a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);	\
1264 | 								\
1265 |    a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);	\
1266 |    a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);	\
1267 |    a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);	\
1268 | }
1269 | 
1270 | /* ========================================================== */
1271 | /* compute adjoint of matrix and scale
1272 |  *
1273 |  * Computes adjoint of matrix m, scales it by s, returning a
1274 |  */
1275 | 
1276 | #define SCALE_ADJOINT_4X4(a,s,m)				\
1277 | {								\
1278 |    int i,j;							\
1279 | 								\
1280 |    for (i=0; i<4; i++) {					\
1281 |       for (j=0; j<4; j++) {					\
1282 |          COFACTOR_4X4_IJ (a[j][i], m, i, j);			\
1283 |          a[j][i] *= s;						\
1284 |       }								\
1285 |    }								\
1286 | }
1287 | 
1288 | /* ========================================================== */
1289 | /* inverse of matrix 
1290 |  *
1291 |  * Compute inverse of matrix a, returning determinant m and 
1292 |  * inverse b
1293 |  */
1294 | 
1295 | #define INVERT_2X2(b,det,a)			\
1296 | {						\
1297 |    double tmp;					\
1298 |    DETERMINANT_2X2 (det, a);			\
1299 |    tmp = 1.0 / (det);				\
1300 |    SCALE_ADJOINT_2X2 (b, tmp, a);		\
1301 | }
1302 | 
1303 | /* ========================================================== */
1304 | /* inverse of matrix 
1305 |  *
1306 |  * Compute inverse of matrix a, returning determinant m and 
1307 |  * inverse b
1308 |  */
1309 | 
1310 | #define INVERT_3X3(b,det,a)			\
1311 | {						\
1312 |    double tmp;					\
1313 |    DETERMINANT_3X3 (det, a);			\
1314 |    tmp = 1.0 / (det);				\
1315 |    SCALE_ADJOINT_3X3 (b, tmp, a);		\
1316 | }
1317 | 
1318 | /* ========================================================== */
1319 | /* inverse of matrix 
1320 |  *
1321 |  * Compute inverse of matrix a, returning determinant m and 
1322 |  * inverse b
1323 |  */
1324 | 
1325 | #define INVERT_4X4(b,det,a)			\
1326 | {						\
1327 |    double tmp;					\
1328 |    DETERMINANT_4X4 (det, a);			\
1329 |    tmp = 1.0 / (det);				\
1330 |    SCALE_ADJOINT_4X4 (b, tmp, a);		\
1331 | }
1332 | 
1333 | /* ========================================================== */
1334 | #if defined(__cplusplus) || defined(c_plusplus)
1335 | }
1336 | #endif
1337 | 
1338 | #endif /* __GUTIL_VECTOR_H__ */
1339 | /* ===================== END OF FILE ======================== */
1340 | 


--------------------------------------------------------------------------------