mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 13:25:30 +03:00
Chainable vector operations (#13996)
* chainable vector ops * add subtraction * Reduce register pressure
This commit is contained in:
parent
89f2ec9880
commit
061ebcebe7
2 changed files with 88 additions and 51 deletions
|
@ -33,22 +33,36 @@ bool vector2Equal(const vector2_t *a, const vector2_t *b)
|
||||||
return (a->x == b->x) && (a->y == b->y);
|
return (a->x == b->x) && (a->y == b->y);
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector2Zero(vector2_t *v)
|
vector2_t *vector2Zero(vector2_t *v)
|
||||||
{
|
{
|
||||||
v->x = 0.0f;
|
v->x = 0.0f;
|
||||||
v->y = 0.0f;
|
v->y = 0.0f;
|
||||||
|
|
||||||
|
return v;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b)
|
vector2_t *vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b)
|
||||||
{
|
{
|
||||||
result->x = a->x + b->x;
|
result->x = a->x + b->x;
|
||||||
result->y = a->y + b->y;
|
result->y = a->y + b->y;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector2Scale(vector2_t *result, const vector2_t *v, const float k)
|
vector2_t *vector2Sub(vector2_t *result, const vector2_t *a, const vector2_t *b)
|
||||||
|
{
|
||||||
|
result->x = a->x - b->x;
|
||||||
|
result->y = a->y - b->y;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
vector2_t *vector2Scale(vector2_t *result, const vector2_t *v, const float k)
|
||||||
{
|
{
|
||||||
result->x = v->x * k;
|
result->x = v->x * k;
|
||||||
result->y = v->y * k;
|
result->y = v->y * k;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
float vector2Dot(const vector2_t *a, const vector2_t *b)
|
float vector2Dot(const vector2_t *a, const vector2_t *b)
|
||||||
|
@ -71,14 +85,14 @@ float vector2Norm(const vector2_t *v)
|
||||||
return sqrtf(vector2NormSq(v));
|
return sqrtf(vector2NormSq(v));
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector2Normalize(vector2_t *result, const vector2_t *v)
|
vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v)
|
||||||
{
|
{
|
||||||
const float normSq = vector2NormSq(v);
|
const float normSq = vector2NormSq(v);
|
||||||
|
|
||||||
if (normSq > 0.0f) {
|
if (normSq > 0.0f) {
|
||||||
vector2Scale(result, v, 1.0f / sqrtf(normSq));
|
return vector2Scale(result, v, 1.0f / sqrtf(normSq));
|
||||||
} else {
|
} else {
|
||||||
vector2Zero(result);
|
return vector2Zero(result);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -87,25 +101,40 @@ bool vector3Equal(const vector3_t *a, const vector3_t *b)
|
||||||
return (a->x == b->x) && (a->y == b->y) && (a->z == b->z);
|
return (a->x == b->x) && (a->y == b->y) && (a->z == b->z);
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector3Zero(vector3_t *v)
|
vector3_t *vector3Zero(vector3_t *v)
|
||||||
{
|
{
|
||||||
v->x = 0.0f;
|
v->x = 0.0f;
|
||||||
v->y = 0.0f;
|
v->y = 0.0f;
|
||||||
v->z = 0.0f;
|
v->z = 0.0f;
|
||||||
|
|
||||||
|
return v;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b)
|
vector3_t *vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b)
|
||||||
{
|
{
|
||||||
result->x = a->x + b->x;
|
result->x = a->x + b->x;
|
||||||
result->y = a->y + b->y;
|
result->y = a->y + b->y;
|
||||||
result->z = a->z + b->z;
|
result->z = a->z + b->z;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector3Scale(vector3_t *result, const vector3_t *v, const float k)
|
vector3_t *vector3Sub(vector3_t *result, const vector3_t *a, const vector3_t *b)
|
||||||
|
{
|
||||||
|
result->x = a->x - b->x;
|
||||||
|
result->y = a->y - b->y;
|
||||||
|
result->z = a->z - b->z;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
vector3_t *vector3Scale(vector3_t *result, const vector3_t *v, const float k)
|
||||||
{
|
{
|
||||||
result->x = v->x * k;
|
result->x = v->x * k;
|
||||||
result->y = v->y * k;
|
result->y = v->y * k;
|
||||||
result->z = v->z * k;
|
result->z = v->z * k;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
float vector3Dot(const vector3_t *a, const vector3_t *b)
|
float vector3Dot(const vector3_t *a, const vector3_t *b)
|
||||||
|
@ -113,14 +142,17 @@ float vector3Dot(const vector3_t *a, const vector3_t *b)
|
||||||
return a->x * b->x + a->y * b->y + a->z * b->z;
|
return a->x * b->x + a->y * b->y + a->z * b->z;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b)
|
vector3_t *vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b)
|
||||||
{
|
{
|
||||||
const vector3_t tmpA = *a;
|
vector3_t tmp;
|
||||||
const vector3_t tmpB = *b;
|
|
||||||
|
|
||||||
result->x = tmpA.y * tmpB.z - tmpA.z * tmpB.y;
|
tmp.x = a->y * b->z - a->z * b->y;
|
||||||
result->y = tmpA.z * tmpB.x - tmpA.x * tmpB.z;
|
tmp.y = a->z * b->x - a->x * b->z;
|
||||||
result->z = tmpA.x * tmpB.y - tmpA.y * tmpB.x;
|
tmp.z = a->x * b->y - a->y * b->x;
|
||||||
|
|
||||||
|
*result = tmp;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
float vector3NormSq(const vector3_t *v)
|
float vector3NormSq(const vector3_t *v)
|
||||||
|
@ -133,36 +165,40 @@ float vector3Norm(const vector3_t *v)
|
||||||
return sqrtf(vector3NormSq(v));
|
return sqrtf(vector3NormSq(v));
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector3Normalize(vector3_t *result, const vector3_t *v)
|
vector3_t *vector3Normalize(vector3_t *result, const vector3_t *v)
|
||||||
{
|
{
|
||||||
const float normSq = vector3NormSq(v);
|
const float normSq = vector3NormSq(v);
|
||||||
|
|
||||||
if (normSq > 0) { // Hopefully sqrt(nonzero) is quite large
|
if (normSq > 0) { // Hopefully sqrt(nonzero) is quite large
|
||||||
vector3Scale(result, v, 1.0f / sqrtf(normSq));
|
return vector3Scale(result, v, 1.0f / sqrtf(normSq));
|
||||||
} else {
|
} else {
|
||||||
vector3Zero(result);
|
return vector3Zero(result);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void matrixVectorMul(vector3_t * result, const matrix33_t *mat, const vector3_t *v)
|
vector3_t *matrixVectorMul(vector3_t * result, const matrix33_t *mat, const vector3_t *v)
|
||||||
{
|
{
|
||||||
const vector3_t tmp = *v;
|
const vector3_t tmp = *v;
|
||||||
|
|
||||||
result->x = mat->m[0][0] * tmp.x + mat->m[0][1] * tmp.y + mat->m[0][2] * tmp.z;
|
result->x = mat->m[0][0] * tmp.x + mat->m[0][1] * tmp.y + mat->m[0][2] * tmp.z;
|
||||||
result->y = mat->m[1][0] * tmp.x + mat->m[1][1] * tmp.y + mat->m[1][2] * tmp.z;
|
result->y = mat->m[1][0] * tmp.x + mat->m[1][1] * tmp.y + mat->m[1][2] * tmp.z;
|
||||||
result->z = mat->m[2][0] * tmp.x + mat->m[2][1] * tmp.y + mat->m[2][2] * tmp.z;
|
result->z = mat->m[2][0] * tmp.x + mat->m[2][1] * tmp.y + mat->m[2][2] * tmp.z;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v)
|
vector3_t *matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v)
|
||||||
{
|
{
|
||||||
const vector3_t tmp = *v;
|
const vector3_t tmp = *v;
|
||||||
|
|
||||||
result->x = mat->m[0][0] * tmp.x + mat->m[1][0] * tmp.y + mat->m[2][0] * tmp.z;
|
result->x = mat->m[0][0] * tmp.x + mat->m[1][0] * tmp.y + mat->m[2][0] * tmp.z;
|
||||||
result->y = mat->m[0][1] * tmp.x + mat->m[1][1] * tmp.y + mat->m[2][1] * tmp.z;
|
result->y = mat->m[0][1] * tmp.x + mat->m[1][1] * tmp.y + mat->m[2][1] * tmp.z;
|
||||||
result->z = mat->m[0][2] * tmp.x + mat->m[1][2] * tmp.y + mat->m[2][2] * tmp.z;
|
result->z = mat->m[0][2] * tmp.x + mat->m[1][2] * tmp.y + mat->m[2][2] * tmp.z;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy)
|
matrix33_t *buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy)
|
||||||
{
|
{
|
||||||
const float cosx = cos_approx(rpy->angles.roll);
|
const float cosx = cos_approx(rpy->angles.roll);
|
||||||
const float sinx = sin_approx(rpy->angles.roll);
|
const float sinx = sin_approx(rpy->angles.roll);
|
||||||
|
@ -171,39 +207,38 @@ void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy)
|
||||||
const float cosz = cos_approx(rpy->angles.yaw);
|
const float cosz = cos_approx(rpy->angles.yaw);
|
||||||
const float sinz = sin_approx(rpy->angles.yaw);
|
const float sinz = sin_approx(rpy->angles.yaw);
|
||||||
|
|
||||||
const float coszcosx = cosz * cosx;
|
|
||||||
const float sinzcosx = sinz * cosx;
|
|
||||||
const float coszsinx = sinx * cosz;
|
|
||||||
const float sinzsinx = sinx * sinz;
|
|
||||||
|
|
||||||
result->m[0][X] = cosz * cosy;
|
result->m[0][X] = cosz * cosy;
|
||||||
result->m[0][Y] = -cosy * sinz;
|
result->m[0][Y] = -cosy * sinz;
|
||||||
result->m[0][Z] = siny;
|
result->m[0][Z] = siny;
|
||||||
result->m[1][X] = sinzcosx + (coszsinx * siny);
|
result->m[1][X] = sinz * cosx + sinx * cosz * siny;
|
||||||
result->m[1][Y] = coszcosx - (sinzsinx * siny);
|
result->m[1][Y] = cosz * cosx - sinx * sinz * siny;
|
||||||
result->m[1][Z] = -sinx * cosy;
|
result->m[1][Z] = -sinx * cosy;
|
||||||
result->m[2][X] = (sinzsinx) - (coszcosx * siny);
|
result->m[2][X] = sinx * sinz - cosz * cosx * siny;
|
||||||
result->m[2][Y] = (coszsinx) + (sinzcosx * siny);
|
result->m[2][Y] = sinx * cosz + sinz * cosx * siny;
|
||||||
result->m[2][Z] = cosy * cosx;
|
result->m[2][Z] = cosy * cosx;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix)
|
vector3_t *applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix)
|
||||||
{
|
{
|
||||||
matrixTrnVectorMul(v, rotationMatrix, v);
|
return matrixTrnVectorMul(v, rotationMatrix, v);
|
||||||
}
|
}
|
||||||
|
|
||||||
void yawToRotationMatrixZ(matrix33_t *result, const float yaw)
|
matrix33_t *yawToRotationMatrixZ(matrix33_t *result, const float yaw)
|
||||||
{
|
{
|
||||||
const float sinYaw = sin_approx(yaw);
|
const float sinYaw = sin_approx(yaw);
|
||||||
const float cosYaw = cos_approx(yaw);
|
const float cosYaw = cos_approx(yaw);
|
||||||
|
|
||||||
result->m[0][0] = cosYaw;
|
result->m[0][0] = cosYaw;
|
||||||
result->m[1][0] = sinYaw;
|
|
||||||
result->m[2][0] = 0.0f;
|
|
||||||
result->m[0][1] = -sinYaw;
|
result->m[0][1] = -sinYaw;
|
||||||
result->m[1][1] = cosYaw;
|
|
||||||
result->m[2][1] = 0.0f;
|
|
||||||
result->m[0][2] = 0.0f;
|
result->m[0][2] = 0.0f;
|
||||||
|
result->m[1][0] = sinYaw;
|
||||||
|
result->m[1][1] = cosYaw;
|
||||||
result->m[1][2] = 0.0f;
|
result->m[1][2] = 0.0f;
|
||||||
|
result->m[2][0] = 0.0f;
|
||||||
|
result->m[2][1] = 0.0f;
|
||||||
result->m[2][2] = 1.0f;
|
result->m[2][2] = 1.0f;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
|
@ -48,29 +48,31 @@ typedef struct matrix33_s {
|
||||||
} matrix33_t;
|
} matrix33_t;
|
||||||
|
|
||||||
bool vector2Equal(const vector2_t *a, const vector2_t *b);
|
bool vector2Equal(const vector2_t *a, const vector2_t *b);
|
||||||
void vector2Zero(vector2_t *v);
|
vector2_t *vector2Zero(vector2_t *v);
|
||||||
void vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
|
vector2_t *vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
|
||||||
void vector2Scale(vector2_t *result, const vector2_t *v, const float k);
|
vector2_t *vector2Sub(vector2_t *result, const vector2_t *a, const vector2_t *b);
|
||||||
|
vector2_t *vector2Scale(vector2_t *result, const vector2_t *v, const float k);
|
||||||
float vector2Dot(const vector2_t *a, const vector2_t *b);
|
float vector2Dot(const vector2_t *a, const vector2_t *b);
|
||||||
float vector2Cross(const vector2_t *a, const vector2_t *b);
|
float vector2Cross(const vector2_t *a, const vector2_t *b);
|
||||||
float vector2NormSq(const vector2_t *v);
|
float vector2NormSq(const vector2_t *v);
|
||||||
float vector2Norm(const vector2_t *v);
|
float vector2Norm(const vector2_t *v);
|
||||||
void vector2Normalize(vector2_t *result, const vector2_t *v);
|
vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v);
|
||||||
|
|
||||||
bool vector3Equal(const vector3_t *a, const vector3_t *b);
|
bool vector3Equal(const vector3_t *a, const vector3_t *b);
|
||||||
void vector3Zero(vector3_t *v);
|
vector3_t *vector3Zero(vector3_t *v);
|
||||||
void vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
vector3_t *vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
||||||
void vector3Scale(vector3_t *result, const vector3_t *v, const float k);
|
vector3_t *vector3Sub(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
||||||
|
vector3_t *vector3Scale(vector3_t *result, const vector3_t *v, const float k);
|
||||||
float vector3Dot(const vector3_t *a, const vector3_t *b);
|
float vector3Dot(const vector3_t *a, const vector3_t *b);
|
||||||
void vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
vector3_t *vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
||||||
float vector3NormSq(const vector3_t *v);
|
float vector3NormSq(const vector3_t *v);
|
||||||
float vector3Norm(const vector3_t *v);
|
float vector3Norm(const vector3_t *v);
|
||||||
void vector3Normalize(vector3_t *result, const vector3_t *v);
|
vector3_t *vector3Normalize(vector3_t *result, const vector3_t *v);
|
||||||
|
|
||||||
void matrixVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
|
vector3_t *matrixVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
|
||||||
void matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
|
vector3_t *matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
|
||||||
|
|
||||||
void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
|
matrix33_t *buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
|
||||||
void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
|
vector3_t *applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
|
||||||
|
|
||||||
void yawToRotationMatrixZ(matrix33_t *result, const float yaw);
|
matrix33_t *yawToRotationMatrixZ(matrix33_t *result, const float yaw);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue