package physics;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
public class QuatUtil {
public static Vector3d rotateVector(Vector3d v, Quat4d q) {
double tempX = q.w * q.w * v.getX() + 2 * q.y * q.w * v.getZ() - 2 * q.z * q.w * v.getY() + q.x * q.x * v.getX()
+ 2 * q.y * q.x * v.getY() + 2 * q.z * q.x * v.getZ() - q.z * q.z * v.getX() - q.y * q.y * v.getX();
double tempY = 2 * q.x * q.y * v.getX() + q.y * q.y * v.getY() + 2 * q.z * q.y * v.getZ() + 2 * q.w * q.z
* v.getX() - q.z * q.z * v.getY() + q.w * q.w * v.getY() - 2 * q.x * q.w * v.getZ() - q.x * q.x
* v.getY();
double tempZ = 2 * q.x * q.z * v.getX() + 2 * q.y * q.z * v.getY() + q.z * q.z * v.getZ() - 2 * q.w * q.y * v.getX()
- q.y * q.y * v.getZ() + 2 * q.w * q.x * v.getY() - q.x * q.x * v.getZ() + q.w * q.w * v.getZ();
return new Vector3d(tempX, tempY, tempZ);
}
}