From 6261959cd00942dd73b17758a77b7cedafe42201 Mon Sep 17 00:00:00 2001 From: sushi rui Date: Mon, 30 Jun 2025 21:34:54 +0800 Subject: [PATCH] =?UTF-8?q?=E7=89=9B=E9=A1=BF=E8=BF=AD=E4=BB=A3=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 54 +-------- main/CMakeLists.txt | 7 +- main/include/catchPoint.h | 33 ++++++ main/include/moveInfo.h | 2 +- main/include/parameter.h | 21 +++- main/include/ps2_controller.h | 28 ++--- main/main.cpp | 33 ++++-- main/src/catchPoint.cpp | 111 ++++++++++++++++++ main/src/moveInfo.cpp | 29 ++++- main/src/ps2_controller.cpp | 13 ++- main/src/uartTool.cpp | 2 +- main/vec/include/matrix2f.h | 40 +++++++ main/vec/include/vector2f.h | 65 +++++++++++ main/vec/include/vector3f.h | 64 +++++++++++ main/vec/src/matrix2f.cpp | 132 ++++++++++++++++++++++ main/vec/src/vector2f.cpp | 178 +++++++++++++++++++++++++++++ main/vec/src/vector3f.cpp | 205 ++++++++++++++++++++++++++++++++++ 17 files changed, 929 insertions(+), 88 deletions(-) create mode 100644 main/include/catchPoint.h create mode 100644 main/src/catchPoint.cpp create mode 100644 main/vec/include/matrix2f.h create mode 100644 main/vec/include/vector2f.h create mode 100644 main/vec/include/vector3f.h create mode 100644 main/vec/src/matrix2f.cpp create mode 100644 main/vec/src/vector2f.cpp create mode 100644 main/vec/src/vector3f.cpp diff --git a/README.md b/README.md index c3e1ee3..4c62d72 100644 --- a/README.md +++ b/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 | -| ----------------- | ----- | -------- | -------- | -------- | -------- | --------- | -------- | -------- | -------- | -------- | ----- | - -# 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. +NO README now. \ No newline at end of file diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 7000002..0291028 100644 --- a/main/CMakeLists.txt +++ b/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") diff --git a/main/include/catchPoint.h b/main/include/catchPoint.h new file mode 100644 index 0000000..1ed1a7a --- /dev/null +++ b/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 +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 \ No newline at end of file diff --git a/main/include/moveInfo.h b/main/include/moveInfo.h index 37390d5..c48c880 100644 --- a/main/include/moveInfo.h +++ b/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(); diff --git a/main/include/parameter.h b/main/include/parameter.h index 7f1ae20..e6bc617 100644 --- a/main/include/parameter.h +++ b/main/include/parameter.h @@ -2,13 +2,32 @@ #define PARAMETER_H #include 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 \ No newline at end of file diff --git a/main/include/ps2_controller.h b/main/include/ps2_controller.h index 53042c0..48630d4 100644 --- a/main/include/ps2_controller.h +++ b/main/include/ps2_controller.h @@ -22,10 +22,10 @@ #define BUTTON_CROSS 0x4000 #define BUTTON_SQUARE 0x8000 -#define LX 5 -#define LY 6 -#define RX 7 -#define RY 8 +#define LX 7 +#define LY 8 +#define RX 5 +#define RY 6 #include #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 getLy(){return data[LY];} - int getRx(){return data[RX];} - int getRy(){return data[RY];} - void launch(); - -private: + int getLx() { return data[LX]; } + int getLy() { return data[LY]; } + int getRx() { return data[RX]; } + int getRy() { return data[RY]; } + uint8_t *getData() { return data; } + 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(); diff --git a/main/main.cpp b/main/main.cpp index 3ddc9e8..0cf1fb9 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -6,17 +6,34 @@ #include "freertos/task.h" #include "esp_timer.h" #include "uartTool.h" +#include "catchPoint.h" +#include extern "C" void app_main() { - 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); + PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23); ps2.launch(); - - while(1) + CatchPoint catcher; + 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()); - std::string cmd= wheel.getWheelInfo(); - uartTool.sendData("WheelMove",cmd); - vTaskDelay(1 / portTICK_PERIOD_MS); //延 + //ps2.updateStatic(); + WheelMoveInfo wheel(WHEEL_FL, WHEEL_FR, WHEEL_BL, WHEEL_BR, ps2.getLx(), ps2.getLy()); + std::string cmd1 = wheel.getWheelInfo(); + + 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< 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; +} \ No newline at end of file diff --git a/main/src/moveInfo.cpp b/main/src/moveInfo.cpp index 9d793df..9cfce03 100644 --- a/main/src/moveInfo.cpp +++ b/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 y = ly * 1.0 - 255.0 / 2; + double x = (lx * 1.0 - 255.0 / 2) / 127.5; + 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); - back_left.setInfo(fl, 1500 + leftRatio * MAX_WHEEL_SPEED, FRAME_TIME); - + front_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(); diff --git a/main/src/ps2_controller.cpp b/main/src/ps2_controller.cpp index 37bb384..bdc414d 100644 --- a/main/src/ps2_controller.cpp +++ b/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) { - vTaskDelay(1000 / portTICK_PERIOD_MS); - } + //while (1) { + // 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); } diff --git a/main/src/uartTool.cpp b/main/src/uartTool.cpp index af2e0c1..64b3b27 100644 --- a/main/src/uartTool.cpp +++ b/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; } \ No newline at end of file diff --git a/main/vec/include/matrix2f.h b/main/vec/include/matrix2f.h new file mode 100644 index 0000000..a9d2c83 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/main/vec/include/vector2f.h b/main/vec/include/vector2f.h new file mode 100644 index 0000000..a49a2d2 --- /dev/null +++ b/main/vec/include/vector2f.h @@ -0,0 +1,65 @@ +#ifndef VECTOR2F_H +#define VECTOR2F_H +#include +#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 \ No newline at end of file diff --git a/main/vec/include/vector3f.h b/main/vec/include/vector3f.h new file mode 100644 index 0000000..bad6dab --- /dev/null +++ b/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 \ No newline at end of file diff --git a/main/vec/src/matrix2f.cpp b/main/vec/src/matrix2f.cpp new file mode 100644 index 0000000..b2340f3 --- /dev/null +++ b/main/vec/src/matrix2f.cpp @@ -0,0 +1,132 @@ +#include "matrix2f.h" +#include +#include +#include +#include +#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; +} diff --git a/main/vec/src/vector2f.cpp b/main/vec/src/vector2f.cpp new file mode 100644 index 0000000..215905a --- /dev/null +++ b/main/vec/src/vector2f.cpp @@ -0,0 +1,178 @@ +#include "vector2f.h" +#include "parameter.h" +#include +#include +////////////////////////////////////////////////////////////////////////// +// 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() +#include +////////////////////////////////////////////////////////////////////////// +// 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); +} \ No newline at end of file