void test_of_axis_angle(void) { struct FloatVect3 axis = { 0., 1., 0.}; FLOAT_VECT3_NORMALIZE(axis); DISPLAY_FLOAT_VECT3("axis", axis); const float angle = RadOfDeg(30.); printf("angle %f\n", DegOfRad(angle)); struct FloatQuat my_q; FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q); struct FloatRMat my_r1; float_rmat_of_quat(&my_r1, &my_q); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1); DISPLAY_FLOAT_RMAT("rmat1", my_r1); struct FloatRMat my_r; FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r); DISPLAY_FLOAT_RMAT("rmat", my_r); printf("\n"); struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.}; struct FloatVect3 uz = { 0., 0., 1.}; struct FloatRMat r_yaw; FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi); struct FloatVect3 uy = { 0., 1., 0.}; struct FloatRMat r_pitch; FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta); struct FloatVect3 ux = { 1., 0., 0.}; struct FloatRMat r_roll; FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatRMat r_yaw_pitch; float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch); struct FloatRMat r_yaw_pitch_roll; float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_EULERS_DEG("eul", eul); struct FloatRMat rmat1; float_rmat_of_eulers(&rmat1, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1); DISPLAY_FLOAT_RMAT("rmat1", rmat1); }
float test_quat(void) { struct FloatVect3 u = { 1., 2., 3.}; FLOAT_VECT3_NORMALIZE(u); float angle = RadOfDeg(30.); struct FloatQuat q; FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle); DISPLAY_FLOAT_QUAT("q ", q); struct FloatQuat iq; FLOAT_QUAT_INVERT(iq, q); DISPLAY_FLOAT_QUAT("iq", iq); struct FloatQuat q1; FLOAT_QUAT_COMP(q1, q, iq); DISPLAY_FLOAT_QUAT("q1", q1); struct FloatQuat q2; FLOAT_QUAT_COMP(q2, q, iq); DISPLAY_FLOAT_QUAT("q2", q2); struct FloatQuat qe; QUAT_EXPLEMENTARY(qe, q); DISPLAY_FLOAT_QUAT("qe", qe); struct FloatVect3 a = { 2., 1., 3.}; DISPLAY_FLOAT_VECT3("a ", a); struct FloatVect3 a1; FLOAT_QUAT_VMULT(a1, q, a); DISPLAY_FLOAT_VECT3("a1", a1); struct FloatVect3 a2; FLOAT_QUAT_VMULT(a2, qe, a); DISPLAY_FLOAT_VECT3("a2", a2); return 0.; }