//获取调整后的方向
FixVector2 getAjustedDirection(FixVector2 currentDirection, FixVector2 destDirection, Fix64 angleTolerance)
{
Fix64 cosAngle = Fix64.Cos(angleTolerance);
//不需要旋转，足够了
if (FixVector2.dot(currentDirection, destDirection) > cosAngle)
{
return(currentDirection);
}
Fix64 crossV = FixVector2.cross(destDirection, currentDirection);
if (crossV != Fix64.Zero)
{
crossV = crossV / Fix64.FastAbs(crossV);
destDirection = FixVector2.rotate(destDirection, angleTolerance * crossV);
}
return(destDirection);
}
FixVector2 getClampedDirection(FixVector2 direction, FixVector2 expectDirection, Fix64 clampAngle)
{
FixVector2 destDirection = expectDirection;
Fix64 dif = FixVector2.dot(expectDirection, direction);
//角度过大
if (dif < Fix64.Cos(clampAngle))
{
//旋转angleClamp
Fix64 c = FixVector2.cross(expectDirection, direction);
if (c < Fix64.Zero)//逆时针旋转为正
{
destDirection = FixVector2.rotate(expectDirection, -clampAngle);
}
else
{
destDirection = FixVector2.rotate(expectDirection, clampAngle);
}
}
return(destDirection);
}