/* bring dcm matrix in order - adjust values to make
* orthonormal (or at least closer to orthonormal) */
static void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}
static char * test_arithmatic() {
Vector3D vector1 = vector3d(1.0, 4.0, 2.0);
Vector3D vector2 = vector3d(3.0, 1.0, 2.0);
Vector3D vector3 = vector3d(3.0, 4.0, -1.0);
Vector3D out = vector3d_subtract(
vector3d_add(vector2, vector3),
vector3d_scale(2, vector1)
);
mu_assert(out.x == 4, "Error: Vector3D x is not correct");
mu_assert(out.y == -3, "Error: Vector3D y is not correct");
mu_assert(out.z == -3, "Error: Vector3D z is not correct");
return 0;
}
