public static void setEuler(Quat4d q, double yaw, double pitch, double roll) {
double halfYaw = yaw * 0.5f;
double halfPitch = pitch * 0.5f;
double halfRoll = roll * 0.5f;
double cosYaw = (double) Math.cos(halfYaw);
double sinYaw = (double) Math.sin(halfYaw);
double cosPitch = (double) Math.cos(halfPitch);
double sinPitch = (double) Math.sin(halfPitch);
double cosRoll = (double) Math.cos(halfRoll);
double sinRoll = (double) Math.sin(halfRoll);
q.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
q.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
}
public static void inverse(Quat4d q, Quat4d src) {
q.x = -src.x;
q.y = -src.y;
q.z = -src.z;
q.w = src.w;
}
public static void inverse(Quat4d q) {
q.x = -q.x;
q.y = -q.y;
q.z = -q.z;
}