mirror of
https://github.com/MeowLynxSea/MarsCar.git
synced 2025-07-09 10:44:35 +00:00
牛顿迭代法
This commit is contained in:
sushi rui
parent
07263108ff
commit
6261959cd0
54
README.md
54
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.
|
||||
@ -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")
|
||||
|
||||
33
main/include/catchPoint.h
Normal file
33
main/include/catchPoint.h
Normal file
@ -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
|
||||
@ -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
|
||||
@ -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 <stdio.h>
|
||||
#include "driver/gpio.h"
|
||||
@ -40,18 +40,20 @@ public:
|
||||
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();
|
||||
|
||||
@ -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);
|
||||
UartTool uartTool(UART_NUM_2,GPIO_NUM_17,GPIO_NUM_16);
|
||||
ps2.launch();
|
||||
|
||||
CatchPoint catcher;
|
||||
UartTool uartTool(UART_NUM_2, GPIO_NUM_17, GPIO_NUM_16);
|
||||
while (1)
|
||||
{
|
||||
//ps2.updateStatic();
|
||||
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); //延
|
||||
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<<cmd3<<std::endl;
|
||||
}
|
||||
//printf("update!\n");
|
||||
uartTool.sendData("Move",cmd1+cmd2+cmd3);
|
||||
// uartTool.sendData("SteerMove","#013P1500T0100!");
|
||||
|
||||
vTaskDelay(10 / portTICK_PERIOD_MS); // 延
|
||||
}
|
||||
}
|
||||
111
main/src/catchPoint.cpp
Normal file
111
main/src/catchPoint.cpp
Normal file
@ -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;
|
||||
}
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
40
main/vec/include/matrix2f.h
Normal file
40
main/vec/include/matrix2f.h
Normal file
@ -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
|
||||
65
main/vec/include/vector2f.h
Normal file
65
main/vec/include/vector2f.h
Normal file
@ -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
|
||||
64
main/vec/include/vector3f.h
Normal file
64
main/vec/include/vector3f.h
Normal file
@ -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
|
||||
132
main/vec/src/matrix2f.cpp
Normal file
132
main/vec/src/matrix2f.cpp
Normal file
@ -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;
|
||||
}
|
||||
178
main/vec/src/vector2f.cpp
Normal file
178
main/vec/src/vector2f.cpp
Normal file
@ -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);
|
||||
}
|
||||
205
main/vec/src/vector3f.cpp
Normal file
205
main/vec/src/vector3f.cpp
Normal file
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user