File content as of revision 14:6cb3c2204b9b:

#include "Matrix.h"
void Vector_Cross_Product(float C[3], float A[3], float B[3])
{
    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
  
    return;
}

void Vector_Scale(float C[3], float A[3], float b)
{
    for (int m = 0; m < 3; m++)
        C[m] = A[m] * b;
        
    return;
}

float Vector_Dot_Product(float A[3], float B[3])
{
    float result = 0.0;

    for (int i = 0; i < 3; i++) {
        result += A[i] * B[i];
    }
    
    return result;
}

void Vector_Add(float C[3], float A[3], float B[3])
{
    for (int m = 0; m < 3; m++)
        C[m] = A[m] + B[m];
        
    return;
}

void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
{
    for (int m = 0; m < 3; m++)
        for (int n = 0; n < 3; n++)
            C[m][n] = A[m][n] + B[m][n];
}

void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
{
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            C[i][j] = 0;
            for (int k = 0; k < 3; k++) {
               C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
{
    float c1 = cos(roll);
    float s1 = sin(roll);
    float c2 = cos(pitch);
    float s2 = sin(pitch);
    float c3 = cos(yaw);
    float s3 = sin(yaw);
    
    // Euler angles, right-handed, intrinsic, XYZ convention
    // (which means: rotate around body axes Z, Y', X'') 
    m[0][0] = c2 * c3;
    m[0][1] = c3 * s1 * s2 - c1 * s3;
    m[0][2] = s1 * s3 + c1 * c3 * s2;
    
    m[1][0] = c2 * s3;
    m[1][1] = c1 * c3 + s1 * s2 * s3;
    m[1][2] = c1 * s2 * s3 - c3 * s1;
    
    m[2][0] = -s2;
    m[2][1] = c2 * s1;
    m[2][2] = c1 * c2;
}