mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +03:00
Chainable vector operations (#13996)
* chainable vector ops * add subtraction * Reduce register pressure
This commit is contained in:
Jan Post
GitHub
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);
|
||||
}
|
||||
|
||||
void vector2Zero(vector2_t *v)
|
||||
vector2_t *vector2Zero(vector2_t *v)
|
||||
{
|
||||
v->x = 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->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->y = v->y * k;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float vector2Dot(const vector2_t *a, const vector2_t *b)
|
||||
|
|
@ -71,14 +85,14 @@ float vector2Norm(const vector2_t *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);
|
||||
|
||||
if (normSq > 0.0f) {
|
||||
vector2Scale(result, v, 1.0f / sqrtf(normSq));
|
||||
return vector2Scale(result, v, 1.0f / sqrtf(normSq));
|
||||
} 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);
|
||||
}
|
||||
|
||||
void vector3Zero(vector3_t *v)
|
||||
vector3_t *vector3Zero(vector3_t *v)
|
||||
{
|
||||
v->x = 0.0f;
|
||||
v->y = 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->y = a->y + b->y;
|
||||
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->y = v->y * k;
|
||||
result->z = v->z * k;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
const vector3_t tmpB = *b;
|
||||
vector3_t tmp;
|
||||
|
||||
result->x = tmpA.y * tmpB.z - tmpA.z * tmpB.y;
|
||||
result->y = tmpA.z * tmpB.x - tmpA.x * tmpB.z;
|
||||
result->z = tmpA.x * tmpB.y - tmpA.y * tmpB.x;
|
||||
tmp.x = a->y * b->z - a->z * b->y;
|
||||
tmp.y = a->z * b->x - a->x * b->z;
|
||||
tmp.z = a->x * b->y - a->y * b->x;
|
||||
|
||||
*result = tmp;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float vector3NormSq(const vector3_t *v)
|
||||
|
|
@ -133,36 +165,40 @@ float vector3Norm(const vector3_t *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);
|
||||
|
||||
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 {
|
||||
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;
|
||||
|
||||
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->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;
|
||||
|
||||
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->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 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 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][Y] = -cosy * sinz;
|
||||
result->m[0][Z] = siny;
|
||||
result->m[1][X] = sinzcosx + (coszsinx * siny);
|
||||
result->m[1][Y] = coszcosx - (sinzsinx * siny);
|
||||
result->m[1][X] = sinz * cosx + sinx * cosz * siny;
|
||||
result->m[1][Y] = cosz * cosx - sinx * sinz * siny;
|
||||
result->m[1][Z] = -sinx * cosy;
|
||||
result->m[2][X] = (sinzsinx) - (coszcosx * siny);
|
||||
result->m[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||||
result->m[2][X] = sinx * sinz - cosz * cosx * siny;
|
||||
result->m[2][Y] = sinx * cosz + sinz * cosx * siny;
|
||||
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 cosYaw = cos_approx(yaw);
|
||||
|
||||
result->m[0][0] = cosYaw;
|
||||
result->m[1][0] = sinYaw;
|
||||
result->m[2][0] = 0.0f;
|
||||
result->m[0][1] = -sinYaw;
|
||||
result->m[1][1] = cosYaw;
|
||||
result->m[2][1] = 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[2][0] = 0.0f;
|
||||
result->m[2][1] = 0.0f;
|
||||
result->m[2][2] = 1.0f;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,29 +48,31 @@ typedef struct matrix33_s {
|
|||
} matrix33_t;
|
||||
|
||||
bool vector2Equal(const vector2_t *a, const vector2_t *b);
|
||||
void vector2Zero(vector2_t *v);
|
||||
void 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 *vector2Zero(vector2_t *v);
|
||||
vector2_t *vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
|
||||
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 vector2Cross(const vector2_t *a, const vector2_t *b);
|
||||
float vector2NormSq(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);
|
||||
void vector3Zero(vector3_t *v);
|
||||
void 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 *vector3Zero(vector3_t *v);
|
||||
vector3_t *vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b);
|
||||
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);
|
||||
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 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);
|
||||
void matrixTrnVectorMul(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);
|
||||
vector3_t *matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
|
||||
|
||||
void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
|
||||
void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
|
||||
matrix33_t *buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
|
||||
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