//{
// bool left_foot_contact = NCGetContactState(index, i, 6);
// bool right_foot_contact = NCGetContactState(index, i, 3);
// cout << right_foot_contact << endl;
// if (right_foot_contact == true)
// {
// float * getx = NCGetX(index, i);
// float * get_posi_in_ned = NCGetPosiInNED(index, i, 6);
// cout << getx[6 * 6 + 0] << "\t" << getx[6 * 6 + 1] << "\t" << getx[6 * 6 + 2] << endl;
// cout << getx[3 * 6 + 0] << "\t" << getx[3 * 6 + 1] << "\t" << getx[3 * 6 + 2] << endl;
// //cout << get_posi_in_ned[0] << "\t" << get_posi_in_ned[1] << "\t" << get_posi_in_ned[2] << endl;
// }
//}
//method1
//Eigen::Quaternionf q_s = { arm_q_start[0], arm_q_start[1], arm_q_start[2], arm_q_start[3] };
//Eigen::Quaternionf q_e = { arm_q_end[0], arm_q_end[1], arm_q_end[2], arm_q_end[3] };
//Eigen::Quaternionf q_res = q_s * q_e;
//Eigen::Vector3f euler_angles = q_res.matrix().eulerAngles(1, 0, 2); // YXZ
//Eigen::Vector3f euler_angels_new = euler_angles * 180.0f / 3.14159265;
//cout << euler_angels_new << endl;
//method2
/* Eigen::Quaternionf q_e = { arm_q_end[0], arm_q_end[1], arm_q_end[2], arm_q_end[3] };
Eigen::Vector3f ned_vector = { 0,0,1 };
cout << q_e.matrix().col(0) << endl;
float a = acos(q_e.matrix().col(0).dot(ned_vector)) * 180.0f / 3.14159265;
cout << a << endl;*/
Eigen::Matrix3f aa(3, 3),BB(3,6), cc(3,6);
aa.setZero();
BB.setZero();
cc.setZero();
aa = q.matrix();
BB = aa.inverse();
cc = q.matrix().inverse() * BB;
cout << cc << endl;