//public struct vector3_innfos {
// public float x;
// public float y;
// public float z;
// public static explicit operator Vector3 (vector3_innfos m) {
// Vector3 temp;
// temp.x = m.x;
// temp.y = m.y;
// temp.z = m.z;
// return temp; }
// public static vector3_innfos operator +(vector3_innfos g , Vector3 e) {
// vector3_innfos gg;
// gg.x = g.x + e.x;
// gg.y = g.y + e.y;
// gg.z = g.z + e.z;
// return gg;
// }
// }
//vector3_innfos nomorlize(vector3_innfos t)
// {
// float tangfei = t.x * t.x + t.y * t.y + t.z * t.z;
// //向量的模
// if (tangfei == 1)
// {
// return t;
// }
// tangfei = 1 / Mathf.Sqrt(tangfei);
// float temp2 = Mathf.Sqrt(tangfei);
// t.x /= temp2;
// t.y /= temp2;
// t.z /= temp2;
// return t;
// }
Quate[] getPackage(short[] package)
{
for (int j = 0; j < 16; ++j)
{
for (int i = 0; i < 23; ++i)
{
receivedPackage[i] = package[i];
}
Quate qtemp;
qtemp.x = qtemp.y = qtemp.z = qtemp.w = 0;
quat.x = quat.y = quat.z = quat.w;
float aaa = 0f;
aaa = four_bytes(receivedPackage[3], receivedPackage[4], receivedPackage[5], receivedPackage[6]);
qtemp.w = aaa * 1 / (1 << 30);
aaa = four_bytes(receivedPackage[7], receivedPackage[8], receivedPackage[9], receivedPackage[10]);
qtemp.x = aaa * 1 / (1 << 30);
aaa = four_bytes(receivedPackage[11], receivedPackage[12], receivedPackage[13], receivedPackage[14]);
qtemp.y = aaa * 1 / (1 << 30);
aaa = four_bytes(receivedPackage[15], receivedPackage[16], receivedPackage[17], receivedPackage[18]);
qtemp.z = aaa * 1 / (1 << 30);
quat.w = qtemp.w; quat.x = qtemp.x; quat.y = qtemp.y; quat.z = qtemp.z;
quat = normalize(quat);
if (!bResult[receivedPackage[1]])
{
quatsStart[receivedPackage[1]] = quat;
bResult[receivedPackage[1]] = true;
}
quats[receivedPackage[1]] = quat / quatsStart[receivedPackage[1]];
}
return(quats);
}
public Quate normalize(Quate quat)//四元数归一化
{
quat = new Quate();
float n = quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w;
if (n == 1)
{
return(quat);
}
n = 1.0f / Mathf.Sqrt(n);
float sqrtn = Mathf.Sqrt(n);
quat.x /= sqrtn; quat.y /= sqrtn;
quat.z /= sqrtn; quat.w /= sqrtn;
return(quat);
}
