public static Quat4d makeRotate(double angle, Vector3d axis) {
double epsilon = 0.0000001;
double x = axis.x;
double y = axis.y;
double z = axis.z;
double length = Math.sqrt(x * x + y * y + z * z);
if (length < epsilon) {
// ~zero length axis, so reset rotation to zero.
return new Quat4d();
}
double inversenorm = 1.0 / length;
double coshalfangle = Math.cos(0.5 * angle);
double sinhalfangle = Math.sin(0.5 * angle);
Quat4d res = new Quat4d();
res.x = x * sinhalfangle * inversenorm;
res.y = y * sinhalfangle * inversenorm;
res.z = z * sinhalfangle * inversenorm;
res.w = coshalfangle;
return res;
}
