public static Quaternion EulerToQuaternion(Vector3 eulerAngle)
{
//角度转弧度
eulerAngle = MoMath.Deg2Rad(eulerAngle);
float cX = MathF.Cos(eulerAngle.X / 2.0f);
float sX = MathF.Sin(eulerAngle.X / 2.0f);
float cY = MathF.Cos(eulerAngle.Y / 2.0f);
float sY = MathF.Sin(eulerAngle.Y / 2.0f);
float cZ = MathF.Cos(eulerAngle.Z / 2.0f);
float sZ = MathF.Sin(eulerAngle.Z / 2.0f);
Quaternion qX = new Quaternion(sX, 0, 0, cX);
Quaternion qY = new Quaternion(0, sY, 0, cY);
Quaternion qZ = new Quaternion(0, 0, sZ, cZ);
Quaternion q = (qY * qX) * qZ;
#if DEBUG
if (!MoMath.CompareApproximate(q.LengthSquared(), 1f))
{
string msg = string.Format($"EulerToQuaternion failed, {q.X} {q.Y} {q.Z} {q.W} sqrLength={q.LengthSquared()}");
MoLog.Log(ELogType.Assert, msg);
}
#endif
return(q);
}
/// <summary>
/// 改变状态
/// </summary>
public void ChangeState(int stateType)
{
MoFsmState state = GetState(stateType);
if (state == null)
{
MoLog.Log(ELogType.Error, "Can not found state {0}", stateType);
return;
}
//全局状态不需要检测
if (_runState.Type != _globalState.Type && state.Type != _globalState.Type)
{
if (_runState.CanChangeTo(stateType) == false)
{
MoLog.Log(ELogType.Error, "Can not change state {0} to {1}", _runState, state);
return;
}
}
MoLog.Log(ELogType.Log, "Change state {0} to {1}", _runState, state);
_preState = _runState;
_runState.Exit();
_runState = state;
_runState.Enter();
}
private void HandleError(bool isDispose, string format, params object[] args)
{
MoLog.Log(ELogType.Error, format, args);
if (isDispose)
{
Dispose();
}
}
public MoCfgTab GetTab(int key)
{
if (_tabs.ContainsKey(key))
{
return(_tabs[key]);
}
else
{
MoLog.Log(ELogType.Warning, "Faild to get tab. File is {0}, key is {1}", _path, key);
return(null);
}
}
/// <summary>
/// check Quaternion is invalid. if value is NaN or +/- infinity
/// </summary>
public static void CheckInvalid(Quaternion q)
{
if (MoMath.IsInfinity(q))
{
MoLog.Log(ELogType.Assert, "Quaternion isInfinity");
}
if (MoMath.IsNaN(q))
{
MoLog.Log(ELogType.Assert, "Quaternion isNaN");
}
}
/// <summary>
/// check Vector3 is invalid. if value is NaN or +/- infinity
/// </summary>
public static void CheckInvalid(Vector3 v)
{
if (MoMath.IsInfinity(v))
{
MoLog.Log(ELogType.Assert, "Vector3 isInfinity");
}
if (MoMath.IsNaN(v))
{
MoLog.Log(ELogType.Assert, "Vector3 isNaN");
}
}
private static Matrix3x3 QuaternionToMatrix(Quaternion q)
{
#if DEBUG
// If q is guaranteed to be a unit quaternion, s will always be 1.
// In that case, this calculation can be optimized out.
if (!MoMath.CompareApproximate(q.LengthSquared(), 1.0F))
{
string msg = string.Format($"QuaternionToMatrix conversion failed, because input Quaternion is invalid {q.X} {q.Y} {q.Z} {q.W} sqrLength={q.LengthSquared()}");
MoLog.Log(ELogType.Assert, msg);
}
#endif
// Precalculate coordinate products
float x = q.X * 2.0F;
float y = q.Y * 2.0F;
float z = q.Z * 2.0F;
float xx = q.X * x;
float yy = q.Y * y;
float zz = q.Z * z;
float xy = q.X * y;
float xz = q.X * z;
float yz = q.Y * z;
float wx = q.W * x;
float wy = q.W * y;
float wz = q.W * z;
// Calculate 3x3 matrix from orthonormal basis
Matrix3x3 m = Matrix3x3.Identity;
m.Data[0] = 1.0f - (yy + zz);
m.Data[1] = xy + wz;
m.Data[2] = xz - wy;
m.Data[3] = xy - wz;
m.Data[4] = 1.0f - (xx + zz);
m.Data[5] = yz + wx;
m.Data[6] = xz + wy;
m.Data[7] = yz - wx;
m.Data[8] = 1.0f - (xx + yy);
return(m);
}
private void ProcessConnected(object obj)
{
SocketAsyncEventArgs e = obj as SocketAsyncEventArgs;
//如果失败
if (e.SocketError != SocketError.Success)
{
MoLog.Log(ELogType.Error, "ProcessConnected error : {0}", e.SocketError);
return;
}
//创建频道
MoTChannel channel = new MoTChannel();
channel.InitSocket(e.ConnectSocket);
//加入到频道列表
lock (_allChannels)
{
_allChannels.Add(channel);
}
}
/// <summary>
/// 处理Accept请求
/// </summary>
private void ProcessAccept(object obj)
{
SocketAsyncEventArgs e = obj as SocketAsyncEventArgs;
//如果失败
if (e.SocketError != SocketError.Success)
{
MoLog.Log(ELogType.Error, "ProcessConnected error : {0}", e.SocketError);
return;
}
//创建频道
MoTChannel channel = new MoTChannel();
channel.InitSocket(e.AcceptSocket);
//添加频道
AddChannel(channel);
// 投递下一个接收请求
StartAccept(e);
}
private static Quaternion MatrixToQuaternion(Matrix3x3 kRot)
{
Quaternion q = new Quaternion();
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternionf Calculus and Fast Animation".
#if DEBUG
float det = kRot.GetDeterminant();
if (!MoMath.CompareApproximate(det, 1.0F, .005f))
{
MoLog.Log(ELogType.Assert, "MatrixToQuaternion det assert.");
}
#endif
float fTrace = kRot.Get(0, 0) + kRot.Get(1, 1) + kRot.Get(2, 2);
float fRoot;
if (fTrace > 0.0f)
{
// |w| > 1/2, may as well choose w > 1/2
fRoot = MathF.Sqrt(fTrace + 1.0f); // 2w
q.W = 0.5f * fRoot;
fRoot = 0.5f / fRoot; // 1/(4w)
q.X = (kRot.Get(2, 1) - kRot.Get(1, 2)) * fRoot;
q.Y = (kRot.Get(0, 2) - kRot.Get(2, 0)) * fRoot;
q.Z = (kRot.Get(1, 0) - kRot.Get(0, 1)) * fRoot;
}
else
{
// |w| <= 1/2
int[] s_iNext = new int[3] {
1, 2, 0
};
int i = 0;
if (kRot.Get(1, 1) > kRot.Get(0, 0))
{
i = 1;
}
if (kRot.Get(2, 2) > kRot.Get(i, i))
{
i = 2;
}
int j = s_iNext[i];
int k = s_iNext[j];
fRoot = MathF.Sqrt(kRot.Get(i, i) - kRot.Get(j, j) - kRot.Get(k, k) + 1.0f);
float[] apkQuat = new float[3] {
q.X, q.Y, q.Z
};
#if DEBUG
if (fRoot < Vector3Helper.Epsilon)
{
MoLog.Log(ELogType.Assert, "MatrixToQuaternion fRoot assert.");
}
#endif
apkQuat[i] = 0.5f * fRoot;
fRoot = 0.5f / fRoot;
q.W = (kRot.Get(k, j) - kRot.Get(j, k)) * fRoot;
apkQuat[j] = (kRot.Get(j, i) + kRot.Get(i, j)) * fRoot;
apkQuat[k] = (kRot.Get(k, i) + kRot.Get(i, k)) * fRoot;
q.X = apkQuat[0];
q.Y = apkQuat[1];
q.Z = apkQuat[2];
}
q = Quaternion.Normalize(q);
return(q);
}
