public Quat(Vector3 axis, real_t angle)
{
#if DEBUG
if (!axis.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(axis));
}
#endif
real_t d = axis.Length();
if (d == 0f)
{
x = 0f;
y = 0f;
z = 0f;
w = 0f;
}
else
{
real_t sinAngle = Mathf.Sin(angle * 0.5f);
real_t cosAngle = Mathf.Cos(angle * 0.5f);
real_t s = sinAngle / d;
x = axis.x * s;
y = axis.y * s;
z = axis.z * s;
w = cosAngle;
}
}
public Quaternion Exp()
{
Vector3 v = new Vector3(x, y, z);
real_t theta = v.Length();
v = v.Normalized();
if (theta < Mathf.Epsilon || !v.IsNormalized())
{
return(new Quaternion(0, 0, 0, 1));
}
return(new Quaternion(v, theta));
}