if (index == 0 || index == 1) {
if (index == 0) {
yaw0 = closest(Math.atan2(c00, -c01), yawRef);
} else if (index == 1) {
yaw0 = closest(Math.atan2(c10, -c11), yawRef);
double sina = (cosc * m01 + sinc * m11) / sinp;
double cosa = (cosc * m02 + sinc * m12) / sinp;
roll0 = closest(Math.atan2(sina, cosa), rollRef);
roll0 = closest(Math.atan2(a00, -a01), rollRef);
} else if (index == 3) {
roll0 = closest(Math.atan2(a10, -a11), rollRef);
double sinc = (sina*m11 + cosa*m12) / sinp;
double cosc = (sina*m01 + cosa*m02) / sinp;
yaw0 = closest(Math.atan2(sinc, cosc), yawRef);