Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Estimation/Matrix/Matrix.h@0:826c6171fc1b, 2012-06-20 (annotated)
- Committer:
- shimniok
- Date:
- Wed Jun 20 14:57:48 2012 +0000
- Revision:
- 0:826c6171fc1b
Updated documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:826c6171fc1b | 1 | #ifndef __DCM_MATRIX_H |
shimniok | 0:826c6171fc1b | 2 | #define __DCM_MATRIX_H |
shimniok | 0:826c6171fc1b | 3 | |
shimniok | 0:826c6171fc1b | 4 | /** Matrix math library */ |
shimniok | 0:826c6171fc1b | 5 | |
shimniok | 0:826c6171fc1b | 6 | #define SINGULAR_MATRIX (1<<0) |
shimniok | 0:826c6171fc1b | 7 | #define ERR1 (1<<1) |
shimniok | 0:826c6171fc1b | 8 | #define ERR2 (1<<2) |
shimniok | 0:826c6171fc1b | 9 | #define ERR3 (1<<3) |
shimniok | 0:826c6171fc1b | 10 | |
shimniok | 0:826c6171fc1b | 11 | /** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut |
shimniok | 0:826c6171fc1b | 12 | * @param v1 is the first vector |
shimniok | 0:826c6171fc1b | 13 | * @param v2 is the second vector |
shimniok | 0:826c6171fc1b | 14 | * @returns vectorOut = v1 x v2 |
shimniok | 0:826c6171fc1b | 15 | */ |
shimniok | 0:826c6171fc1b | 16 | void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); |
shimniok | 0:826c6171fc1b | 17 | |
shimniok | 0:826c6171fc1b | 18 | /** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2 |
shimniok | 0:826c6171fc1b | 19 | * @param vectorIn the vector |
shimniok | 0:826c6171fc1b | 20 | * @param scale2 is the scalar |
shimniok | 0:826c6171fc1b | 21 | * @returns vectorOut the result |
shimniok | 0:826c6171fc1b | 22 | */ |
shimniok | 0:826c6171fc1b | 23 | void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); |
shimniok | 0:826c6171fc1b | 24 | |
shimniok | 0:826c6171fc1b | 25 | /** TDot product of two 3x1 vectors vector1 . vector2 |
shimniok | 0:826c6171fc1b | 26 | * @param vector1 is the first vector |
shimniok | 0:826c6171fc1b | 27 | * @param vector2 is the second vector |
shimniok | 0:826c6171fc1b | 28 | * @returns float result of dot product |
shimniok | 0:826c6171fc1b | 29 | */ |
shimniok | 0:826c6171fc1b | 30 | float Vector_Dot_Product(float vector1[3], float vector2[3]); |
shimniok | 0:826c6171fc1b | 31 | |
shimniok | 0:826c6171fc1b | 32 | /** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2 |
shimniok | 0:826c6171fc1b | 33 | * @param vectorIn1 is the first vector |
shimniok | 0:826c6171fc1b | 34 | * @param vectorIn2 is the second vector |
shimniok | 0:826c6171fc1b | 35 | * @returns vectorOut is the result of the addition |
shimniok | 0:826c6171fc1b | 36 | */ |
shimniok | 0:826c6171fc1b | 37 | void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); |
shimniok | 0:826c6171fc1b | 38 | |
shimniok | 0:826c6171fc1b | 39 | /** Adds two 3x3 matrices: C = A + B |
shimniok | 0:826c6171fc1b | 40 | * @param A is the first vector |
shimniok | 0:826c6171fc1b | 41 | * @param B is the second vector |
shimniok | 0:826c6171fc1b | 42 | * @returns C is the result of the addition |
shimniok | 0:826c6171fc1b | 43 | */ |
shimniok | 0:826c6171fc1b | 44 | void Matrix_Add(float C[3][3], float A[3][3], float B[3][3]); |
shimniok | 0:826c6171fc1b | 45 | |
shimniok | 0:826c6171fc1b | 46 | /** Adds two n x m matrices: C = A + B |
shimniok | 0:826c6171fc1b | 47 | * @param A is the first vector |
shimniok | 0:826c6171fc1b | 48 | * @param B is the second vector |
shimniok | 0:826c6171fc1b | 49 | * @param n is the rows of A, B, C |
shimniok | 0:826c6171fc1b | 50 | * @param m is the columns of A, B, C |
shimniok | 0:826c6171fc1b | 51 | * @returns C is the result of the addition |
shimniok | 0:826c6171fc1b | 52 | */ |
shimniok | 0:826c6171fc1b | 53 | void Matrix_Add(int n, int m, float *C, float *A, float *B); |
shimniok | 0:826c6171fc1b | 54 | |
shimniok | 0:826c6171fc1b | 55 | void Matrix_Subtract(int n, int m, float *C, float *A, float *B); |
shimniok | 0:826c6171fc1b | 56 | |
shimniok | 0:826c6171fc1b | 57 | /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab |
shimniok | 0:826c6171fc1b | 58 | * @param a is the first vector |
shimniok | 0:826c6171fc1b | 59 | * @param b is the second vector |
shimniok | 0:826c6171fc1b | 60 | * @returns c as the result of the mutliplication |
shimniok | 0:826c6171fc1b | 61 | */ |
shimniok | 0:826c6171fc1b | 62 | void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]); |
shimniok | 0:826c6171fc1b | 63 | |
shimniok | 0:826c6171fc1b | 64 | /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab |
shimniok | 0:826c6171fc1b | 65 | * @param A is the first matrix |
shimniok | 0:826c6171fc1b | 66 | * @param B is the second matrix |
shimniok | 0:826c6171fc1b | 67 | * @param n is the rows of A |
shimniok | 0:826c6171fc1b | 68 | * @param p is the columns of A and rows of B |
shimniok | 0:826c6171fc1b | 69 | * @param m is the columns of B |
shimniok | 0:826c6171fc1b | 70 | * @returns C as the result of the mutliplication |
shimniok | 0:826c6171fc1b | 71 | */ |
shimniok | 0:826c6171fc1b | 72 | void Matrix_Multiply(int n, int m, int p, float *C, float *A, float *B); |
shimniok | 0:826c6171fc1b | 73 | |
shimniok | 0:826c6171fc1b | 74 | void Matrix_Transpose(int n, int m, float *C, float *A); |
shimniok | 0:826c6171fc1b | 75 | |
shimniok | 0:826c6171fc1b | 76 | void Matrix_Inverse(int n, float *A); |
shimniok | 0:826c6171fc1b | 77 | |
shimniok | 0:826c6171fc1b | 78 | void Matrix_Inverse(int n, float *C, float *A); |
shimniok | 0:826c6171fc1b | 79 | |
shimniok | 0:826c6171fc1b | 80 | void Matrix_Copy(int n, int m, float *C, float *A); |
shimniok | 0:826c6171fc1b | 81 | |
shimniok | 0:826c6171fc1b | 82 | /** Prints out an m x m matrix |
shimniok | 0:826c6171fc1b | 83 | * @param A is the matrix to print |
shimniok | 0:826c6171fc1b | 84 | * @param name is the name to print out along with the vector |
shimniok | 0:826c6171fc1b | 85 | */ |
shimniok | 0:826c6171fc1b | 86 | void Matrix_print(int n, int m, float *A, const char *name); |
shimniok | 0:826c6171fc1b | 87 | |
shimniok | 0:826c6171fc1b | 88 | /** Prints out a 1 x 3 vector |
shimniok | 0:826c6171fc1b | 89 | * @param A is the 1 x 3 vector to print |
shimniok | 0:826c6171fc1b | 90 | * @param name is the name to print out along with the vector |
shimniok | 0:826c6171fc1b | 91 | */ |
shimniok | 0:826c6171fc1b | 92 | void Vector_Print(float A[3], const char *name); |
shimniok | 0:826c6171fc1b | 93 | |
shimniok | 0:826c6171fc1b | 94 | #endif |