public void setRotation(Matrix3d rotation) {
double r11, r12, r13, r21, r22, r23, r31, r32, r33;
r11 = rotation.getM00();
r12 = rotation.getM01();
r13 = rotation.getM02();
r21 = rotation.getM10();
r22 = rotation.getM11();
r23 = rotation.getM12();
r31 = rotation.getM20();
r32 = rotation.getM21();
r33 = rotation.getM22();
q_qs.set(Math.sqrt((1.0 + r11 + r22 + r33) / 4.0));
q_qx.set((r32 - r23) / (4.0 * q_qs.getDoubleValue()));
q_qy.set((r13 - r31) / (4.0 * q_qs.getDoubleValue()));
q_qz.set((r21 - r12) / (4.0 * q_qs.getDoubleValue()));
}
