public static bool CompressedQuaternionUnitTest()
{
var stream = new VRage.Library.Collections.BitStream();
stream.ResetWrite();
Quaternion q = Quaternion.Identity;
stream.WriteQuaternionNormCompressed(q);
stream.ResetRead();
var q2 = stream.ReadQuaternionNormCompressed();
bool fail = !q.Equals(q2, 1 / 511.0f);
stream.ResetWrite();
q = Quaternion.CreateFromAxisAngle(Vector3.Forward, (float)Math.PI / 3.0f);
stream.WriteQuaternionNormCompressed(q);
stream.ResetRead();
q2 = stream.ReadQuaternionNormCompressed();
fail |= !q.Equals(q2, 1 / 511.0f);
stream.ResetWrite();
var v = new Vector3(1, -1, 3);
v.Normalize();
q = Quaternion.CreateFromAxisAngle(v, (float)Math.PI / 3.0f);
stream.WriteQuaternionNormCompressed(q);
stream.ResetRead();
q2 = stream.ReadQuaternionNormCompressed();
fail |= !q.Equals(q2, 1 / 511.0f);
return(fail);
}
bool ReadTransform(BitStream stream, MyEntity entity, Vector3D? deltaPosBase, bool applyWhenReading, bool movingOnServer, ref Vector3D outPosition, ref Quaternion outOrientation, ref MatrixD outWorldMartix, Func<MyEntity, Vector3D, bool> posValidation = null, MovedDelegate moveHandler = null)
{
Vector3D position;
if (stream.ReadBool())
{
position = stream.ReadVector3D(); // 24 B
}
else
{
Vector3 pos = stream.ReadVector3(); // 6 B
if (deltaPosBase != null)
{
position = pos + deltaPosBase.Value;
}
else
{
position = pos;
}
}
Quaternion orientation;
bool lowPrecisionOrientation = stream.ReadBool();
if (lowPrecisionOrientation)
{
orientation = stream.ReadQuaternionNormCompressed(); // 29b
}
else
{
orientation = stream.ReadQuaternionNorm(); // 52b
}
if (entity != null)
{
movingOnServer |= (entity.PositionComp.GetPosition() - position).LengthSquared() > epsilonSq;
if (movingOnServer && applyWhenReading && (posValidation == null || posValidation(entity, position)))
{
MatrixD matrix = MatrixD.CreateFromQuaternion(orientation);
if (matrix.IsValid())
{
matrix.Translation = position;
outPosition = matrix.Translation;
outOrientation = orientation;
outWorldMartix = matrix;
return true;
}
return false;
}
}
return false;
}