-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
4 changed files
with
22 additions
and
31 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -7,6 +7,5 @@ | |
|
||
#define PITCH 0 | ||
#define YAW 1 | ||
#define ROLL 2 | ||
|
||
#define X 0 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,43 +1,35 @@ | ||
void Math_RotateVector(const float vector[VECTOR_SIZE], const float degAngles[VECTOR_SIZE], float result[VECTOR_SIZE]) { | ||
float radAngles[VECTOR_SIZE]; | ||
void Math_RotateVector(const float vector[VECTOR_SIZE], float pitchDeg, float yawDeg, float result[VECTOR_SIZE]) { | ||
float rotationMatrix[VECTOR_SIZE][VECTOR_SIZE]; | ||
float pitchRad = DegToRad(pitchDeg); | ||
float yawRad = DegToRad(yawDeg); | ||
|
||
Math_DegreesToRadiansVector(degAngles, radAngles); | ||
Math_GetRotationMatrix(radAngles, rotationMatrix); | ||
Math_GetRotationMatrix(pitchRad, yawRad, rotationMatrix); | ||
Math_MultiplyMatrixByVector(rotationMatrix, vector, result); | ||
} | ||
|
||
void Math_DegreesToRadiansVector(const float degAngles[VECTOR_SIZE], float radAngles[VECTOR_SIZE]) { | ||
radAngles[PITCH] = DegToRad(degAngles[PITCH]); | ||
radAngles[YAW] = DegToRad(degAngles[YAW]); | ||
radAngles[ROLL] = DegToRad(degAngles[ROLL]); | ||
} | ||
|
||
void Math_GetRotationMatrix(const float angles[VECTOR_SIZE], float rotationMat[VECTOR_SIZE][VECTOR_SIZE]) { | ||
float cosBeta = Cosine(angles[PITCH]); | ||
float sinBeta = Sine(angles[PITCH]); | ||
float cosAlpha = Cosine(angles[YAW]); | ||
float sinAlpha = Sine(angles[YAW]); | ||
float cosGamma = Cosine(angles[ROLL]); | ||
float sinGamma = Sine(angles[ROLL]); | ||
void Math_GetRotationMatrix(float pitchRad, float yawRad, float matrix[VECTOR_SIZE][VECTOR_SIZE]) { | ||
float cosBeta = Cosine(pitchRad); | ||
float sinBeta = Sine(pitchRad); | ||
float cosAlpha = Cosine(yawRad); | ||
float sinAlpha = Sine(yawRad); | ||
|
||
rotationMat[0][0] = cosAlpha * cosBeta; | ||
rotationMat[0][1] = cosAlpha * sinBeta * sinGamma - sinAlpha * cosGamma; | ||
rotationMat[0][2] = cosAlpha * sinBeta * cosGamma + sinAlpha * sinGamma; | ||
rotationMat[1][0] = sinAlpha * cosBeta; | ||
rotationMat[1][1] = sinAlpha * sinBeta * sinGamma + cosAlpha * cosGamma; | ||
rotationMat[1][2] = sinAlpha * sinBeta * cosGamma - cosAlpha * sinGamma; | ||
rotationMat[2][0] = -sinBeta; | ||
rotationMat[2][1] = cosBeta * sinGamma; | ||
rotationMat[2][2] = cosBeta * cosGamma; | ||
matrix[0][0] = cosAlpha * cosBeta; | ||
matrix[0][1] = cosAlpha * sinBeta - sinAlpha; | ||
matrix[0][2] = cosAlpha * sinBeta + sinAlpha; | ||
matrix[1][0] = sinAlpha * cosBeta; | ||
matrix[1][1] = sinAlpha * sinBeta + cosAlpha; | ||
matrix[1][2] = sinAlpha * sinBeta - cosAlpha; | ||
matrix[2][0] = -sinBeta; | ||
matrix[2][1] = cosBeta; | ||
matrix[2][2] = cosBeta; | ||
} | ||
|
||
void Math_MultiplyMatrixByVector(const float mat[VECTOR_SIZE][VECTOR_SIZE], const float vec[VECTOR_SIZE], float result[VECTOR_SIZE]) { | ||
void Math_MultiplyMatrixByVector(const float matrix[VECTOR_SIZE][VECTOR_SIZE], const float vector[VECTOR_SIZE], float result[VECTOR_SIZE]) { | ||
for (int i = 0; i < VECTOR_SIZE; i++) { | ||
result[i] = 0.0; | ||
|
||
for (int j = 0; j < VECTOR_SIZE; j++) { | ||
result[i] += mat[i][j] * vec[j]; | ||
result[i] += matrix[i][j] * vector[j]; | ||
} | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters