/// <summary>
/// pass-in a rotation, ensure return a quaternion meets the limit
/// </summary>
public Quaternion FixRotation(Quaternion q)
{
Vector3 euler = q.eulerAngles;
for (int i = 0; i < 3; ++i)
{
float v = Misc.NormalizeAngle(euler[i]);
AxisLimit limit = m_xyz[i];
float from = limit.first;
float to = limit.second;
bool bValid = false;
if (to < from)
{
bValid = (v <= to || from <= v);
}
else
{
bValid = (from <= v && v <= to);
}
if (!bValid)
{
float distTo = Misc.AngleDist(v, to);
float distFrom = Misc.AngleDist(v, from);
//Dbg.Log("v: {0}, from:{1}, to:{2}, distTo:{3}, distFrom:{4}", v, from, to, distTo, distFrom);
if (distTo <= distFrom)
{
v = to;
}
else
{
v = from;
}
euler[i] = v;
}
}
Quaternion newQ = Quaternion.Euler(euler);
return(newQ);
}
