牛顿迭代法

README.md

@@ -1,53 +1 @@
-| Supported Targets | ESP32 | ESP32-C2 | ESP32-C3 | ESP32-C5 | ESP32-C6 | ESP32-C61 | ESP32-H2 | ESP32-P4 | ESP32-S2 | ESP32-S3 | Linux |
+NO README now.
-| ----------------- | ----- | -------- | -------- | -------- | -------- | --------- | -------- | -------- | -------- | -------- | ----- |
-
-# Hello World Example
-
-Starts a FreeRTOS task to print "Hello World".
-
-(See the README.md file in the upper level 'examples' directory for more information about examples.)
-
-## How to use example
-
-Follow detailed instructions provided specifically for this example.
-
-Select the instructions depending on Espressif chip installed on your development board:
-
-- [ESP32 Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/stable/get-started/index.html)
-- [ESP32-S2 Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
-
-
-## Example folder contents
-
-The project **hello_world** contains one source file in C language [hello_world_main.c](main/hello_world_main.c). The file is located in folder [main](main).
-
-ESP-IDF projects are built using CMake. The project build configuration is contained in `CMakeLists.txt` files that provide set of directives and instructions describing the project's source files and targets (executable, library, or both).
-
-Below is short explanation of remaining files in the project folder.
-
-```
-├── CMakeLists.txt
-├── pytest_hello_world.py      Python script used for automated testing
-├── main
-│   ├── CMakeLists.txt
-│   └── hello_world_main.c
-└── README.md                  This is the file you are currently reading
-```
-
-For more information on structure and contents of ESP-IDF projects, please refer to Section [Build System](https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/build-system.html) of the ESP-IDF Programming Guide.
-
-## Troubleshooting
-
-* Program upload failure
-
-    * Hardware connection is not correct: run `idf.py -p PORT monitor`, and reboot your board to see if there are any output logs.
-    * The baud rate for downloading is too high: lower your baud rate in the `menuconfig` menu, and try again.
-
-## Technical support and feedback
-
-Please use the following feedback channels:
-
-* For technical queries, go to the [esp32.com](https://esp32.com/) forum
-* For a feature request or bug report, create a [GitHub issue](https://github.com/espressif/esp-idf/issues)
-
-We will get back to you as soon as possible.

main/CMakeLists.txt

@@ -2,4 +2,9 @@ idf_component_register(SRCS "main.cpp"
     "src/ps2_controller.cpp"
     "src/moveInfo.cpp"
     "src/uartTool.cpp"
-    INCLUDE_DIRS "include")
+    "vec/src/vector3f.cpp"
+    "vec/src/vector2f.cpp"
+    "vec/src/matrix2f.cpp"
+    "src/catchPoint.cpp"
+    INCLUDE_DIRS "include"
+        "vec/include")

main/include/catchPoint.h

@@ -0,0 +1,33 @@
+#ifndef CATCHPOINT_H
+#define CATCHPOINT_H
+#include "vector3f.h"
+#include "vector2f.h"
+#include "parameter.h"
+#include "matrix2f.h"
+#include "moveInfo.h"
+#include "ps2_controller.h"
+#include <string>
+class CatchPoint
+{
+public:
+    CatchPoint(const Vector3f &p=Vector3f(0,0,ARM_LENGTH_1+ARM_LENGTH_2)) : target3(p), o(Vector3f::ZERO)
+    {
+    };
+    ~CatchPoint() = default;
+    void move(const PS2Controller &ps2);
+    bool solution(double Eps);
+    void setSteerInfo();
+    std::string getSteerInfo();
+private:
+    void calculateJacobi(Matrix2f &jacobi,Vector2f &f,Vector2f &sol);
+    Vector3f target3;
+    Vector2f target2;
+    double angle_base;
+    Vector2f angle;
+    Vector3f o;
+
+    MoveInfo steer_base;
+    MoveInfo steer_1;
+    MoveInfo steer_2;
+};
+#endif // CATCHPOINT_H

main/include/moveInfo.h

@@ -42,7 +42,7 @@ public:
     SteerMoveInfo(std::string fl, int pwm_fl, int t_fl, std::string fr, int pwm_fr, int t_fr,
                   std::string bl, int pwm_bl, int t_bl, std::string br, int pwm_br, int t_br)
         : front_left(fl, pwm_fl, t_fl), front_right(fr, pwm_fr, t_fr),
-          back_left(bl, pwm_bl, t_bl), back_right(fl, pwm_br, t_br) {};
+          back_left(bl, pwm_bl, t_bl), back_right(fl, pwm_br, t_br){};
     ~SteerMoveInfo() = default;
 
     std::string getSteerInfo();

main/include/parameter.h

@@ -2,13 +2,32 @@
 #define PARAMETER_H
 #include <string>
 static const int MAX_WHEEL_SPEED = 1000;
+static const int MAX_STEER_PWM = 700;
+static const double CATCHER_SPEED = 0.01;
+
 static const std::string WHEEL_FL="001";
 static const std::string WHEEL_FR="002";
 static const std::string WHEEL_BL="003";
 static const std::string WHEEL_BR="004";
 
+static const std::string STEER_FL="011";
+static const std::string STEER_FR="012";
+static const std::string STEER_BL="013";
+static const std::string STEER_BR="014";
 
-static const int FRAME_TIME= 10;
+static const std::string CATCHER_1="021";
+static const std::string CATCHER_2="022";
+static const std::string CATCHER_3="023";
+static const std::string CATCHER_4="024";
 
+static const int CATCHER_TIME= 1000;
+static const int FRAME_TIME= 100;
+static const double EPS=1e-4;
+
+static const double H=0;
+static const double ARM_LENGTH_1=5;
+static const double ARM_LENGTH_2=5;
+
+static const double PI=3.1415926535;
 
 #endif //PARAMETER_H

main/include/ps2_controller.h

@@ -22,10 +22,10 @@
 #define BUTTON_CROSS 0x4000
 #define BUTTON_SQUARE 0x8000
 
-#define LX 5
+#define LX 7
-#define LY 6
+#define LY 8
-#define RX 7
+#define RX 5
-#define RY 8
+#define RY 6
 
 #include <stdio.h>
 #include "driver/gpio.h"
@@ -36,22 +36,24 @@ public:
     PS2Controller(gpio_num_t dat_pin, gpio_num_t cmd_pin, gpio_num_t sel_pin, gpio_num_t clk_pin);
     ~PS2Controller() = default;
 
-    int getLx(){return data[LX];}
+    int getLx() { return data[LX]; }
-    int getLy(){return data[LY];}
+    int getLy() { return data[LY]; }
-    int getRx(){return data[RX];}
+    int getRx() { return data[RX]; }
-    int getRy(){return data[RY];}
+    int getRy() { return data[RY]; }
-    void launch();
+    uint8_t *getData() { return data; }
-    
-private:
 
+    bool ButtonPressed(uint16_t button) const;
+    bool Button(uint16_t button) const;
+    bool updateStatic();
+    void launch();
+
+private:
     uint8_t sendAndReceive(uint8_t const &byte) const;
     bool readState(bool motor1, uint8_t motor2);
     int config(bool pressures, bool rumble);
     void reconfig();
     void sendCommandString(uint8_t *cmd, int length) const;
-    bool Button(uint16_t button) const;
     bool NewButtonState(uint16_t button) const;
-    bool ButtonPressed(uint16_t button) const;
     bool ButtonReleased(uint16_t button) const;
 
     void ps2Task();

main/main.cpp

@@ -6,17 +6,34 @@
 #include "freertos/task.h"
 #include "esp_timer.h"
 #include "uartTool.h"
+#include "catchPoint.h"
+#include <iostream>
 extern "C" void app_main()
 {
-    PS2Controller ps2(GPIO_NUM_19,GPIO_NUM_18,GPIO_NUM_15,GPIO_NUM_23);
+    PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23);
-    UartTool uartTool(UART_NUM_2,GPIO_NUM_17,GPIO_NUM_16);
     ps2.launch();
-    
+    CatchPoint catcher;
-    while(1)
+    UartTool uartTool(UART_NUM_2, GPIO_NUM_17, GPIO_NUM_16);
+    while (1)
     {
-        WheelMoveInfo wheel(WHEEL_FL,WHEEL_FR,WHEEL_BL,WHEEL_BR,ps2.getLx(),ps2.getLy());
+        //ps2.updateStatic();
-        std::string cmd= wheel.getWheelInfo();
+        WheelMoveInfo wheel(WHEEL_FL, WHEEL_FR, WHEEL_BL, WHEEL_BR, ps2.getLx(), ps2.getLy());
-        uartTool.sendData("WheelMove",cmd);       
+        std::string cmd1 = wheel.getWheelInfo();
-        vTaskDelay(1 / portTICK_PERIOD_MS);                  //延
+
+        SteerMoveInfo steer(STEER_FL, STEER_FR, STEER_BL, STEER_BR, ps2.getRx(), ps2.getRy());
+        std::string cmd2 = steer.getSteerInfo();
+
+        catcher.move(ps2);
+        std::string cmd3="";
+        if (catcher.solution(1e-2))
+        {
+            std::string cmd3 = catcher.getSteerInfo();
+            std::cout<<cmd3<<std::endl;
+       }
+        //printf("update!\n");
+        uartTool.sendData("Move",cmd1+cmd2+cmd3);
+        // uartTool.sendData("SteerMove","#013P1500T0100!");
+        
+        vTaskDelay(10 / portTICK_PERIOD_MS); // 延
     }
 }

main/src/catchPoint.cpp

@@ -0,0 +1,111 @@
+#include "catchPoint.h"
+#include "esp_random.h"
+#include "parameter.h"
+#include "matrix2f.h"
+#include "vector2f.h"
+#include "ps2_controller.h"
+void CatchPoint::move(const PS2Controller &ps2)
+{
+    Vector3f speed = Vector3f::ZERO;
+    if (ps2.Button(BUTTON_PAD_UP))
+        speed[1] += 1;
+    if (ps2.Button(BUTTON_PAD_DOWN))
+        speed[1] -= 1;
+    if (ps2.Button(BUTTON_PAD_RIGHT))
+        speed[0] -= 1;
+    if (ps2.Button(BUTTON_PAD_LEFT))
+        speed[0] += 1;
+    if (ps2.Button(BUTTON_TRIANGLE))
+        speed[2] += 1;
+    if (ps2.Button(BUTTON_CROSS))
+        speed[2] -= 1;
+    if (speed.length() > EPS)
+        speed = speed.normalized() * CATCHER_SPEED;
+    target3 += speed;
+    target2 = Vector2f((target3.xy().length()), target3.z());
+    // target3.print();
+}
+
+bool CatchPoint::solution(double Eps = 1e-2)
+{
+    bool success = false;
+
+    angle_base = target3.xy().angleWithX();
+
+    // 牛顿迭代20次,尝试10次不同初值
+    for (int now = 0; now < 10; now++)
+    {
+        uint32_t random_integer = esp_random();
+        double random_double = (double)random_integer / UINT32_MAX;
+        Vector2f sol;
+        sol[0] = random_double * 2 * PI;
+        random_integer = esp_random();
+        random_double = (double)random_integer / UINT32_MAX;
+        sol[1] = random_double * 2 * PI;
+
+        Matrix2f jacobi;
+        Vector2f f;
+        double eps;
+        for (int i = 0; i < 50; i++)
+        {
+            int zero;
+            calculateJacobi(jacobi, f, sol);
+            Vector2f last_sol = sol;
+            sol = sol - jacobi.inverse(zero) * f;
+            eps = (sol - last_sol).length();
+            if (zero)
+                break;
+            if (eps < Eps)
+            {
+                success = true;
+                break;
+            }
+        }
+
+        if (success)
+        {
+            while (sol[0] > PI)
+                sol[0] -= 2 * PI;
+            while (sol[0] < -PI)
+                sol[0] += 2 * PI;
+            while (sol[1] > PI)
+                sol[1] -= 2 * PI;
+            while (sol[1] < -PI)
+                sol[1] += 2 * PI;
+
+            sol[1] = sol[1] - sol[0] + PI / 2;
+            angle = sol;
+            setSteerInfo();
+            break;
+        }
+    }
+
+    if (success)
+        printf("有解! ");
+    else
+        printf("当前钩爪无解！\n");
+
+    return success;
+}
+void CatchPoint::calculateJacobi(Matrix2f &jacobi, Vector2f &f, Vector2f &sol)
+{
+    jacobi = Matrix2f(-ARM_LENGTH_1 * std::sin(sol[0]), -ARM_LENGTH_2 * std::sin(sol[1]),
+                      ARM_LENGTH_1 * std::cos(sol[0]), ARM_LENGTH_2 * std::cos(sol[1]));
+    f = Vector2f(ARM_LENGTH_1 * std::cos(sol[0]) + ARM_LENGTH_2 * std::cos(sol[1]) - target2.x(),
+                 ARM_LENGTH_1 * std::sin(sol[0]) + ARM_LENGTH_2 * std::sin(sol[1]) - target2.y());
+}
+void CatchPoint::setSteerInfo()
+{
+    double pwm = 1020 + (angle_base * 1.0f / PI) * 980;
+    steer_base.setInfo(CATCHER_1, (int)pwm, CATCHER_TIME);
+    pwm = 850 + (angle[0] * 1.0f / PI) / 1300;
+    steer_1.setInfo(CATCHER_2, (int)pwm, CATCHER_TIME);
+    // TODO:
+    pwm = 0;
+    steer_2.setInfo(CATCHER_3, (int)pwm, CATCHER_TIME);
+}
+std::string CatchPoint::getSteerInfo()
+{
+    std::string result = steer_base.getInfo() + steer_1.getInfo() + steer_2.getInfo();
+    return result;
+}

main/src/moveInfo.cpp

@@ -28,8 +28,8 @@ WheelMoveInfo::WheelMoveInfo(std::string fl, std::string fr, std::string bl, std
     auto max = [](auto x, auto y)
     { return x > y ? x : y; };
 
-    double x = lx * 1.0 - 255.0 / 2;
+    double x = (lx * 1.0 - 255.0 / 2) / 127.5;
-    double y = ly * 1.0 - 255.0 / 2;
+    double y = ((255.0 - ly) * 1.0 - 255.0 / 2) / 127.5;
     double leftRatio, rightRatio;
     if (x < 0 && y > 0)
     {
@@ -58,9 +58,8 @@ WheelMoveInfo::WheelMoveInfo(std::string fl, std::string fr, std::string bl, std
             leftRatio = x + 2 * y;
     }
 
-    front_left.setInfo(fl, 1500 + leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+    front_left.setInfo(fl, 1500 - leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
-    back_left.setInfo(fl, 1500 + leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+    back_left.setInfo(bl, 1500 - leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
-
     front_right.setInfo(fr, 1500 + rightRatio * MAX_WHEEL_SPEED, FRAME_TIME);
     back_right.setInfo(br, 1500 + rightRatio * MAX_WHEEL_SPEED, FRAME_TIME);
 }
@@ -71,6 +70,26 @@ std::string WheelMoveInfo::getWheelInfo()
 }
 
 //==================底盘舵机============//
+
+SteerMoveInfo::SteerMoveInfo(std::string fl, std::string fr, std::string bl, std::string br, int rx, int ry)
+{
+
+    auto abs = [](auto x)
+    { return x > 0 ? x : -x; };
+    auto max = [](auto x, auto y)
+    { return x > y ? x : y; };
+    auto clamp = [](auto x)
+    {if(x<-1)return -1.0;else if(x>1)return 1.0;else return x; };
+    double x = (rx * 1.0 - 255.0 / 2) / 127.5;
+    double y = ((255.0 - ry) * 1.0 - 255.0 / 2) / 127.5;
+    double frontRatio = clamp(x*1.0 - y*1.0);
+    double backRatio = -y;
+
+    front_left.setInfo(fl, 1500 + frontRatio * MAX_STEER_PWM, FRAME_TIME);
+    back_left.setInfo(bl, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME);
+    front_right.setInfo(fr, 1500 + frontRatio * MAX_STEER_PWM, FRAME_TIME);
+    back_right.setInfo(br, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME);
+}
 std::string SteerMoveInfo::getSteerInfo()
 {
     std::string result = front_left.getInfo() + front_right.getInfo() + back_left.getInfo() + back_right.getInfo();

main/src/ps2_controller.cpp

@@ -38,6 +38,7 @@ PS2Controller::PS2Controller(gpio_num_t dat_pin, gpio_num_t cmd_pin,
     controller_type = 0;
     en_Rumble = false;
     en_Pressures = false;
+    printf("ps2初始化！");
 }
 uint8_t PS2Controller::sendAndReceive(uint8_t const &byte) const
 {
@@ -230,9 +231,9 @@ void PS2Controller::ps2Task()
     int error = config(true, true);
     if (error) {
         printf("Error configuring controller\n");
-        while (1) {
+        //while (1) {
-            vTaskDelay(1000 / portTICK_PERIOD_MS);
+        //    vTaskDelay(1000 / portTICK_PERIOD_MS);
-        }
+        //}
     }
     
     printf("Found Controller, configured successful\n");
@@ -261,6 +262,7 @@ void PS2Controller::ps2Task()
         }
         
         // 打印按钮状态
+        /*
         if (Button(BUTTON_SELECT)) printf("SELECT pressed\n");
         if (Button(BUTTON_START)) printf("START pressed\n");
         if (Button(BUTTON_PAD_UP)) printf("UP pressed\n");
@@ -275,14 +277,15 @@ void PS2Controller::ps2Task()
         if (Button(BUTTON_R1)) printf("R1 pressed\n");
         if (Button(BUTTON_L2)) printf("L2 pressed\n");
         if (Button(BUTTON_R2)) printf("R2 pressed\n");
-        
+        */
         // 模拟摇杆值
+        /*
         if (en_Pressures) {
             printf("LX: %d, LY: %d, RX: %d, RY: %d\n",
                    data[LX], data[LY], 
                    data[RX], data[RY]);
         }
-        
+        */
         vTaskDelay(READ_DELAY_MS / portTICK_PERIOD_MS);
     }
     

main/src/uartTool.cpp

@@ -3,6 +3,6 @@
 int UartTool::sendData(const std::string logName, const std::string data)
 {
     const int txBytes = uart_write_bytes(UART_NUM, data.c_str(), data.length());
-    ESP_LOGI(logName.c_str(), "Wrote %d bytes", txBytes); // log打印
+    //ESP_LOGI(logName.c_str(), "Wrote %d bytes", txBytes); // log打印
     return txBytes;
 }

main/vec/include/matrix2f.h

@@ -0,0 +1,40 @@
+#ifndef MATRIX2F_H
+#define MATRIX2F_H
+#include "parameter.h"
+class Vector2f;
+class Matrix2f
+{
+public:
+    Matrix2f(float fill = 0.f);
+    Matrix2f(float m00, float m01,
+             float m10, float m11);
+
+    const float &operator()(int i, int j) const;
+    float &operator()(int i, int j);
+
+    float determinant();
+    Matrix2f inverse(int &error,float epsilon = EPS);
+
+    void print();
+
+    static float determinant2x2(float m00, float m01,
+                                float m10, float m11);
+
+    static Matrix2f ones();
+    static Matrix2f identity();
+
+private:
+    float e[4];
+};
+
+// Scalar-Matrix multiplication
+Matrix2f operator * ( float f, const Matrix2f& m );
+Matrix2f operator * ( const Matrix2f& m, float f );
+
+// Matrix-Vector multiplication
+// 2x2 * 2x1 ==> 2x1
+Vector2f operator * ( const Matrix2f& m, const Vector2f& v );
+
+// Matrix-Matrix multiplication
+Matrix2f operator * ( const Matrix2f& x, const Matrix2f& y );
+#endif

main/vec/include/vector2f.h

@@ -0,0 +1,65 @@
+#ifndef VECTOR2F_H
+#define VECTOR2F_H
+#include <math.h>
+#include "parameter.h"
+class Vector2f
+{
+public:
+    static const Vector2f ZERO;
+
+    Vector2f(float f = 0.f);
+    Vector2f(float x, float y);
+    Vector2f(const Vector2f &rv);
+
+    // assignment operators
+    Vector2f &operator=(const Vector2f &rv);
+
+    // returns the ith element
+    const float &operator[](int i) const;
+    float &operator[](int i);
+
+    float &x();
+    float &y();
+    float x() const;
+    float y() const;
+
+    float length() const;
+    float squaredLength() const;
+
+    void normalize();
+    Vector2f normalized() const;
+
+    Vector2f &operator+=(const Vector2f &v);
+    Vector2f &operator-=(const Vector2f &v);
+    Vector2f &operator*=(float f);
+    Vector2f &operator/=(float f);
+
+    void print() const;
+    double angleWithX()
+    {
+        double angle = std::atan2(e[1], e[0]);
+        if (angle < 0)
+            angle += 2 * PI;
+        return angle;
+    }
+
+private:
+    float e[2];
+};
+
+// component-wise operators
+Vector2f operator+(const Vector2f &v0, const Vector2f &v1);
+Vector2f operator-(const Vector2f &v0, const Vector2f &v1);
+double operator*(const Vector2f &v0, const Vector2f &v1);
+double operator^(const Vector2f &v0, const Vector2f &v1);
+
+Vector2f operator-(const Vector2f &v);
+
+// multiply and divide by scalar
+Vector2f operator*(float f, const Vector2f &v);
+Vector2f operator*(const Vector2f &v, float f);
+Vector2f operator/(const Vector2f &v, float f);
+
+bool operator==(const Vector2f &v0, const Vector2f &v1);
+bool operator!=(const Vector2f &v0, const Vector2f &v1);
+#endif

main/vec/include/vector3f.h

@@ -0,0 +1,64 @@
+#ifndef VECTOR3F_H
+#define VECTOR3F_H
+#include "vector2f.h"
+class Vector3f
+{
+public:
+    static const Vector3f ZERO;
+    static const Vector3f UP;
+    static const Vector3f RIGHT;
+    static const Vector3f FORWARD;
+
+    Vector3f(float f = 0.f);
+    Vector3f(float x, float y, float z);
+    Vector3f(const Vector3f &rv);
+
+    // assignment operators
+    Vector3f &operator=(const Vector3f &rv);
+
+    // returns the ith element
+    const float &operator[](int i) const;
+    float &operator[](int i);
+
+    float &x();
+    float &y();
+    float &z();
+
+    float x() const;
+    float y() const;
+    float z() const;
+    Vector2f xy()const;
+
+    float length() const;
+    float squaredLength() const;
+
+    void normalize();
+    Vector3f normalized() const;
+
+    Vector3f &operator+=(const Vector3f &v);
+    Vector3f &operator-=(const Vector3f &v);
+    Vector3f &operator*=(float f);
+    Vector3f &operator/=(float f);
+
+    void print() const;
+
+private:
+    float e[3];
+};
+
+// component-wise operators
+Vector3f operator+(const Vector3f &v0, const Vector3f &v1);
+Vector3f operator-(const Vector3f &v0, const Vector3f &v1);
+float operator*(const Vector3f &v0, const Vector3f &v1);
+Vector3f operator^(const Vector3f &v0, const Vector3f &v1);
+
+Vector3f operator-(const Vector3f &v);
+
+// multiply and divide by scalar
+Vector3f operator*(float f, const Vector3f &v);
+Vector3f operator*(const Vector3f &v, float f);
+Vector3f operator/(const Vector3f &v, float f);
+
+bool operator==(const Vector3f &v0, const Vector3f &v1);
+bool operator!=(const Vector3f &v0, const Vector3f &v1);
+#endif

main/vec/src/matrix2f.cpp

@@ -0,0 +1,132 @@
+#include "matrix2f.h"
+#include <cassert>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include "vector2f.h"
+Matrix2f::Matrix2f(float fill)
+{
+    for (int i = 0; i < 4; ++i)
+    {
+        e[i] = fill;
+    }
+}
+
+Matrix2f::Matrix2f(float m00, float m01,
+                   float m10, float m11)
+{
+    e[0] = m00;
+    e[1] = m10;
+
+    e[2] = m01;
+    e[3] = m11;
+}
+
+
+const float &Matrix2f::operator()(int i, int j) const
+{
+    return e[j * 2 + i];
+}
+
+float &Matrix2f::operator()(int i, int j)
+{
+    return e[j * 2 + i];
+}
+
+float Matrix2f::determinant()
+{
+    return Matrix2f::determinant2x2(
+        e[0], e[2],
+        e[1], e[3]);
+}
+
+Matrix2f Matrix2f::inverse(int &error, float epsilon)
+{
+    error=1;
+    float determinant = e[0] * e[3] - e[2] * e[1];
+
+    bool isSingular = (fabs(determinant) < epsilon);
+    if (isSingular)
+        return Matrix2f();
+    else
+    {
+        error=0;
+        float reciprocalDeterminant = 1.0f / determinant;
+        return Matrix2f(
+            e[3] * reciprocalDeterminant, -e[2] * reciprocalDeterminant,
+            -e[1] * reciprocalDeterminant, e[0] * reciprocalDeterminant);
+    }
+}
+
+void Matrix2f::print()
+{
+    printf("[ %.4f %.4f ]\n[ %.4f %.4f ]\n",
+           e[0], e[2],
+           e[1], e[3]);
+}
+
+// static
+float Matrix2f::determinant2x2(float m00, float m01,
+                               float m10, float m11)
+{
+    return (m00 * m11 - m01 * m10);
+}
+
+// static
+Matrix2f Matrix2f::ones()
+{
+    Matrix2f m;
+    for (int i = 0; i < 4; ++i)
+        m.e[i] = 1;
+
+    return m;
+}
+
+// static
+Matrix2f Matrix2f::identity()
+{
+    Matrix2f m;
+
+    m(0, 0) = 1;
+    m(1, 1) = 1;
+
+    return m;
+}
+
+Matrix2f operator*(float f, const Matrix2f &m)
+{
+    Matrix2f output;
+
+    for (int i = 0; i < 2; ++i)
+        for (int j = 0; j < 2; ++j)
+            output(i, j) = f * m(i, j);
+    return output;
+}
+
+Matrix2f operator*(const Matrix2f &m, float f)
+{
+    return f * m;
+}
+
+Vector2f operator*(const Matrix2f &m, const Vector2f &v)
+{
+    Vector2f output(0, 0);
+
+    for (int i = 0; i < 2; ++i)
+        for (int j = 0; j < 2; ++j)
+            output[i] += m(i, j) * v[j];
+
+    return output;
+}
+
+Matrix2f operator*(const Matrix2f &x, const Matrix2f &y)
+{
+    Matrix2f product; // zeroes
+
+    for (int i = 0; i < 2; ++i)
+        for (int j = 0; j < 2; ++j)
+            for (int k = 0; k < 2; ++k)
+                product(i, k) += x(i, j) * y(j, k);
+
+    return product;
+}

main/vec/src/vector2f.cpp

@@ -0,0 +1,178 @@
+#include "vector2f.h"
+#include "parameter.h"
+#include <stdio.h>
+#include <math.h>
+//////////////////////////////////////////////////////////////////////////
+// Public
+//////////////////////////////////////////////////////////////////////////
+
+// static
+const Vector2f Vector2f::ZERO = Vector2f(0, 0);
+Vector2f::Vector2f(float f)
+{
+    e[0] = f;
+    e[1] = f;
+}
+
+Vector2f::Vector2f(float x, float y)
+{
+    e[0] = x;
+    e[1] = y;
+}
+Vector2f::Vector2f(const Vector2f &rv)
+{
+    e[0] = rv[0];
+    e[1] = rv[1];
+}
+
+Vector2f &Vector2f::operator=(const Vector2f &rv)
+{
+    if (this != &rv)
+    {
+        e[0] = rv[0];
+        e[1] = rv[1];
+    }
+    return *this;
+}
+const float &Vector2f::operator[](int i) const
+{
+    return e[i];
+}
+
+float &Vector2f::operator[](int i)
+{
+    return e[i];
+}
+float &Vector2f::x()
+{
+    return e[0];
+}
+
+float &Vector2f::y()
+{
+    return e[1];
+}
+
+float Vector2f::x() const
+{
+    return e[0];
+}
+
+float Vector2f::y() const
+{
+    return e[1];
+}
+
+float Vector2f::length() const
+{
+    return std::sqrt(e[0] * e[0] + e[1] * e[1]);
+}
+
+float Vector2f::squaredLength() const
+{
+    return (
+        e[0] * e[0] +
+        e[1] * e[1] );
+}
+
+void Vector2f::normalize()
+{
+    float norm = length();
+    e[0] /= norm;
+    e[1] /= norm;
+}
+
+Vector2f Vector2f::normalized() const
+{
+    float norm = length();
+    return Vector2f(
+        e[0] / norm,
+        e[1] / norm);
+}
+void Vector2f::print() const
+{
+    printf("< %.4f, %.4f >\n",
+           e[0], e[1]);
+}
+
+Vector2f &Vector2f::operator+=(const Vector2f &v)
+{
+    e[0] += v.e[0];
+    e[1] += v.e[1];
+    return *this;
+}
+
+Vector2f &Vector2f::operator-=(const Vector2f &v)
+{
+    e[0] -= v.e[0];
+    e[1] -= v.e[1];
+    return *this;
+}
+
+Vector2f &Vector2f::operator*=(float f)
+{
+    e[0] *= f;
+    e[1] *= f;
+    return *this;
+}
+
+Vector2f &Vector2f::operator/=(float f)
+{
+    if (f != 0)
+    {
+        e[0] /= f;
+        e[1] /= f;
+    }
+    return *this;
+}
+
+Vector2f operator+(const Vector2f &v0, const Vector2f &v1)
+{
+    return Vector2f(v0[0] + v1[0], v0[1] + v1[1]);
+}
+
+Vector2f operator-(const Vector2f &v0, const Vector2f &v1)
+{
+    return Vector2f(v0[0] - v1[0], v0[1] - v1[1]);
+}
+
+double operator*(const Vector2f &v0, const Vector2f &v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1];
+}
+
+double operator^(const Vector2f &v0, const Vector2f &v1)
+{
+    return v0[0]*v1[1]-v0[1]*v1[0];
+}
+
+Vector2f operator-(const Vector2f &v)
+{
+    return Vector2f(-v[0], -v[1]);
+}
+
+Vector2f operator*(float f, const Vector2f &v)
+{
+	return Vector2f(v[0] * f, v[1] * f);
+}
+
+Vector2f operator*(const Vector2f &v, float f)
+{
+	return Vector2f(v[0] * f, v[1] * f);
+}
+
+Vector2f operator/(const Vector2f &v, float f)
+{
+	return Vector2f(v[0] / f, v[1] / f);
+}
+
+
+bool operator==(const Vector2f &v0, const Vector2f &v1)
+{
+	return (v0-v1).length()<EPS;
+}
+
+bool operator!=(const Vector2f &v0, const Vector2f &v1)
+{
+	return !(v0 == v1);
+}

main/vec/src/vector3f.cpp

@@ -0,0 +1,205 @@
+#include "vector3f.h"
+#include "parameter.h"
+#include <stdio.h>
+#include <math.h>
+//////////////////////////////////////////////////////////////////////////
+// Public
+//////////////////////////////////////////////////////////////////////////
+
+// static
+const Vector3f Vector3f::ZERO = Vector3f(0, 0, 0);
+const Vector3f Vector3f::UP = Vector3f(0, 1, 0);
+const Vector3f Vector3f::RIGHT = Vector3f(1, 0, 0);
+const Vector3f Vector3f::FORWARD = Vector3f(0, 0, -1);
+
+Vector3f::Vector3f(float f)
+{
+    e[0] = f;
+    e[1] = f;
+    e[2] = f;
+}
+
+Vector3f::Vector3f(float x, float y, float z)
+{
+    e[0] = x;
+    e[1] = y;
+    e[2] = z;
+}
+Vector3f::Vector3f(const Vector3f &rv)
+{
+    e[0] = rv[0];
+    e[1] = rv[1];
+    e[2] = rv[2];
+}
+
+Vector3f &Vector3f::operator=(const Vector3f &rv)
+{
+    if (this != &rv)
+    {
+        e[0] = rv[0];
+        e[1] = rv[1];
+        e[2] = rv[2];
+    }
+    return *this;
+}
+const float &Vector3f::operator[](int i) const
+{
+    return e[i];
+}
+
+float &Vector3f::operator[](int i)
+{
+    return e[i];
+}
+float &Vector3f::x()
+{
+    return e[0];
+}
+
+float &Vector3f::y()
+{
+    return e[1];
+}
+
+float &Vector3f::z()
+{
+    return e[2];
+}
+
+float Vector3f::x() const
+{
+    return e[0];
+}
+
+float Vector3f::y() const
+{
+    return e[1];
+}
+
+float Vector3f::z() const
+{
+    return e[2];
+}
+Vector2f Vector3f::xy() const
+{
+    return Vector2f(e[0], e[1]);
+}
+float Vector3f::length() const
+{
+    return std::sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
+}
+
+float Vector3f::squaredLength() const
+{
+    return (
+        e[0] * e[0] +
+        e[1] * e[1] +
+        e[2] * e[2]);
+}
+
+void Vector3f::normalize()
+{
+    float norm = length();
+    e[0] /= norm;
+    e[1] /= norm;
+    e[2] /= norm;
+}
+
+Vector3f Vector3f::normalized() const
+{
+    float norm = length();
+    return Vector3f(
+        e[0] / norm,
+        e[1] / norm,
+        e[2] / norm);
+}
+void Vector3f::print() const
+{
+    printf("< %.4f, %.4f, %.4f >\n",
+           e[0], e[1], e[2]);
+}
+
+Vector3f &Vector3f::operator+=(const Vector3f &v)
+{
+    e[0] += v.e[0];
+    e[1] += v.e[1];
+    e[2] += v.e[2];
+    return *this;
+}
+
+Vector3f &Vector3f::operator-=(const Vector3f &v)
+{
+    e[0] -= v.e[0];
+    e[1] -= v.e[1];
+    e[2] -= v.e[2];
+    return *this;
+}
+
+Vector3f &Vector3f::operator*=(float f)
+{
+    e[0] *= f;
+    e[1] *= f;
+    e[2] *= f;
+    return *this;
+}
+
+Vector3f &Vector3f::operator/=(float f)
+{
+    if (f != 0)
+    {
+        e[0] /= f;
+        e[1] /= f;
+        e[2] /= f;
+    }
+    return *this;
+}
+
+Vector3f operator+(const Vector3f &v0, const Vector3f &v1)
+{
+    return Vector3f(v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]);
+}
+
+Vector3f operator-(const Vector3f &v0, const Vector3f &v1)
+{
+    return Vector3f(v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]);
+}
+
+float operator*(const Vector3f &v0, const Vector3f &v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2];
+}
+
+Vector3f operator^(const Vector3f &v0, const Vector3f &v1)
+{
+    return Vector3f(v0[1] * v1[2] - v0[2] * v1[1], -v0[0] * v1[2] + v0[2] * v1[0], v0[0] * v1[1] - v0[1] * v1[0]);
+}
+
+Vector3f operator-(const Vector3f &v)
+{
+    return Vector3f(-v[0], -v[1], -v[2]);
+}
+
+Vector3f operator*(float f, const Vector3f &v)
+{
+    return Vector3f(v[0] * f, v[1] * f, v[2] * f);
+}
+
+Vector3f operator*(const Vector3f &v, float f)
+{
+    return Vector3f(v[0] * f, v[1] * f, v[2] * f);
+}
+
+Vector3f operator/(const Vector3f &v, float f)
+{
+    return Vector3f(v[0] / f, v[1] / f, v[2] / f);
+}
+
+bool operator==(const Vector3f &v0, const Vector3f &v1)
+{
+    return (v0 - v1).length() < EPS;
+}
+
+bool operator!=(const Vector3f &v0, const Vector3f &v1)
+{
+    return !(v0 == v1);
+}