| private Zrot ( Quaternion r ) : System.Vector3 | ||
| r | Quaternion | |
| return | System.Vector3 | |
private Vector3 Zrot(Quaternion r)
{
double x, y, z, m;
m = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W;
if (Math.Abs(1.0 - m) > 0.000001)
{
m = 1.0 / Math.Sqrt(m);
r.X *= (float)m;
r.Y *= (float)m;
r.Z *= (float)m;
r.W *= (float)m;
}
x = 2 * (r.X * r.Z + r.Y * r.W);
y = 2 * (-r.X * r.W + r.Y * r.Z);
z = -r.X * r.X - r.Y * r.Y + r.Z * r.Z + r.W * r.W;
return new Vector3((float)x, (float)y, (float)z);
}