diff --git a/src/main/common/vector.c b/src/main/common/vector.c index df1f4099a7..8481051d7f 100644 --- a/src/main/common/vector.c +++ b/src/main/common/vector.c @@ -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; } diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 8a4fcba17c..541348ec13 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -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);