/* 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]));
}
quaternion
quaternion_mul(quaternion q1, quaternion q2)
{
quaternion res;
double prodscal=vector3d_scalarproduct(q1.V,q2.V);
vector3d prodvec=vector3d_vectorialproduct(q1.V,q2.V);
vector3d wVp,wpV,vec_add;
vector3d_init(&wVp,q1.w*q2.V.dx,q1.w*q2.V.dy,q1.w*q2.V.dz);
vector3d_init(&wpV,q2.w*q1.V.dx,q2.w*q1.V.dy,q2.w*q1.V.dz);
vec_add=vector3d_add(wVp,vector3d_add(wpV,prodvec));
quaternion_init(&res,q1.w*q2.w-prodscal,vec_add.dx,vec_add.dy,vec_add.dz);
return res;
}
quaternion
quaternion_add(quaternion q1, quaternion q2)
{
quaternion res;
vector3d vec_add=vector3d_add(q1.V,q2.V);
quaternion_init(&res,q1.w+q2.w,vec_add.dx,vec_add.dy,vec_add.dz);
return res;
}
Camera camera_new(Vector3D pos,Vector3D aim,Vector3D up,float fov,float ratio){
Camera camera;
camera.m_pos = pos;
Vector3D z = vector3d_normalize(vector3d_sub(aim,pos));
camera.x = vector3d_normalize(vector3d_cross_product(z,up));
camera.y = vector3d_mul_float(vector3d_cross_product(camera.x,z),1.f/ratio);
camera.m_center = vector3d_add(camera.m_pos,vector3d_mul_float(z,(1.f / tan((fov * 3.14159265358979323846 / 180.f) * 0.5f))));
return camera;
}
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;
}
//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
static void dcm_rotate(float dcm[3][3], float w[3]){
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
uint32_t i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for(i=0;i<3;i++){
vector3d_cross(w, dcm[i], dR);
vector3d_add(dcm[i], dR, dcm[i]);
}
//make matrix orthonormal again
dcm_orthonormalize(dcm);
}
