56 lines
1.7 KiB
C
56 lines
1.7 KiB
C
#include <math.h>
|
|
#include "quad_pos_control_math.h"
|
|
float vector2f_norm(const Vector2f *v) {
|
|
return sqrtf(v->x * v->x + v->y * v->y);
|
|
}
|
|
|
|
Vector2f vector2f_add(const Vector2f *v1, const Vector2f *v2) {
|
|
Vector2f res = {v1->x + v2->x, v1->y + v2->y};
|
|
return res;
|
|
}
|
|
|
|
Vector2f vector2f_sub(const Vector2f *v1, const Vector2f *v2) {
|
|
Vector2f res = {v1->x - v2->x, v1->y - v2->y};
|
|
return res;
|
|
}
|
|
|
|
float vector2f_dot(const Vector2f *v1, const Vector2f *v2) {
|
|
return v1->x * v2->x + v1->y * v2->y;
|
|
}
|
|
|
|
Vector2f vector2f_normalized(const Vector2f *v) {
|
|
float norm = vector2f_norm(v);
|
|
if (norm < 1e-6f) return (Vector2f){0.0f, 0.0f};
|
|
Vector2f res = {v->x / norm, v->y / norm};
|
|
return res;
|
|
}
|
|
|
|
Vector2f vector2f_scale(const Vector2f *v, float s) {
|
|
Vector2f res = {v->x * s, v->y * s};
|
|
return res;
|
|
}
|
|
|
|
// 限幅函数
|
|
Vector2f constrainXY(const Vector2f *v0, const Vector2f *v1, float max) {
|
|
Vector2f sum = vector2f_add(v0, v1);
|
|
float sum_norm = vector2f_norm(&sum);
|
|
|
|
if (sum_norm <= max) {
|
|
return sum;
|
|
} else if (vector2f_norm(v0) >= max) {
|
|
return vector2f_scale(v0, max / vector2f_norm(v0));
|
|
} else if (vector2f_norm(v0) < 0.001f) {
|
|
return vector2f_scale(v1, max / vector2f_norm(v1));
|
|
} else {
|
|
// 计算 v1 方向的单位向量
|
|
Vector2f u1 = vector2f_normalized(v1);
|
|
float m = vector2f_dot(&u1, v0);
|
|
float c = vector2f_dot(v0, v0) - max * max;
|
|
float discriminant = m * m - c;
|
|
|
|
// 处理负判别式（数值稳定性）
|
|
if (discriminant < 0) discriminant = 0;
|
|
float s = -m + sqrtf(discriminant);
|
|
return vector2f_add(v0, vector2f_scale(&u1, s));
|
|
}
|
|
} |