public static void ConvertFloorSenderPacketDataToPositionAndRotation(FloorSenderPacketData floorSenderPacketData, out Vector3 position, out Quaternion rotation)
{
//Vector3 upVector = new Vector3(floorSenderPacketData.a, floorSenderPacketData.b, floorSenderPacketData.c);
// y component is fliped since the coordinate system of unity and azure kinect is different.
Plane floorPacketPlane = new Plane(new Vector3(floorSenderPacketData.a, -floorSenderPacketData.b, floorSenderPacketData.c), floorSenderPacketData.d);
//Vector3 upVector = new Vector3(floorSenderPacketData.a, -floorSenderPacketData.b, floorSenderPacketData.c);
position = floorPacketPlane.normal * floorPacketPlane.distance;
rotation = Quaternion.FromToRotation(Vector3.up, floorPacketPlane.normal);
}
public static FloorSenderPacketData Parse(byte[] packetBytes)
{
var reader = new BinaryReader(new MemoryStream(packetBytes));
reader.BaseStream.Position = 5;
var floorSenderPacketData = new FloorSenderPacketData();
floorSenderPacketData.a = reader.ReadSingle();
floorSenderPacketData.b = reader.ReadSingle();
floorSenderPacketData.c = reader.ReadSingle();
floorSenderPacketData.d = reader.ReadSingle();
return(floorSenderPacketData);
}
private static void CollectPacket(UdpSocketPacket packet, SenderPacketCollection senderPacketCollection)
{
int senderSessionId = PacketHelper.getSessionIdFromSenderPacketBytes(packet.Bytes);
SenderPacketType packetType = PacketHelper.getPacketTypeFromSenderPacketBytes(packet.Bytes);
if (packetType == SenderPacketType.Confirm)
{
senderPacketCollection.ConfirmPacketInfoList.Add(new ConfirmPacketInfo(packet.EndPoint, senderSessionId, ConfirmSenderPacketData.Parse(packet.Bytes)));
return;
}
SenderPacketSet senderPacketSet;
if (!senderPacketCollection.SenderPacketSets.TryGetValue(senderSessionId, out senderPacketSet))
{
return;
}
// Heartbeat packets turns on ReceivedAny.
senderPacketSet.ReceivedAny = true;
switch (packetType)
{
case SenderPacketType.VideoInit:
senderPacketSet.InitPacketDataList.Add(VideoInitSenderPacketData.Parse(packet.Bytes));
break;
case SenderPacketType.Frame:
senderPacketSet.VideoPacketDataList.Add(VideoSenderPacketData.Parse(packet.Bytes));
break;
case SenderPacketType.Parity:
senderPacketSet.FecPacketDataList.Add(ParitySenderPacketData.Parse(packet.Bytes));
break;
case SenderPacketType.Audio:
senderPacketSet.AudioPacketDataList.Add(AudioSenderPacketData.Parse(packet.Bytes));
break;
case SenderPacketType.Floor:
senderPacketSet.FloorPacketDataList.Add(FloorSenderPacketData.Parse(packet.Bytes));
break;
}
}
public static SenderPacketSet Receive(UdpSocket udpSocket)
{
var senderPacketSet = new SenderPacketSet();
while (true)
{
var packet = udpSocket.Receive();
if (packet == null)
{
break;
}
//int sessionId = PacketHelper.getSessionIdFromSenderPacketBytes(packet);
senderPacketSet.ReceivedAny = true;
switch (PacketHelper.getPacketTypeFromSenderPacketBytes(packet))
{
case SenderPacketType.Init:
senderPacketSet.InitPacketDataList.Add(InitSenderPacketData.Parse(packet));
break;
case SenderPacketType.Frame:
senderPacketSet.VideoPacketDataList.Add(VideoSenderPacketData.Parse(packet));
break;
case SenderPacketType.Parity:
senderPacketSet.FecPacketDataList.Add(ParitySenderPacketData.Parse(packet));
break;
case SenderPacketType.Audio:
senderPacketSet.AudioPacketDataList.Add(AudioSenderPacketData.Parse(packet));
break;
case SenderPacketType.Floor:
senderPacketSet.FloorPacketDataList.Add(FloorSenderPacketData.Parse(packet));
break;
}
}
return(senderPacketSet);
}
public void UpdateFrame(List <FloorSenderPacketData> floorPacketDataList)
{
if (floorPacketDataList.Count == 0)
{
return;
}
FloorSenderPacketData floorSenderPacketData = floorPacketDataList[floorPacketDataList.Count - 1];
Vector3 position;
Quaternion rotation;
FloorUtils.ConvertFloorSenderPacketDataToPositionAndRotation(floorSenderPacketData, out position, out rotation);
floorTransform.localPosition = position;
floorTransform.localRotation = rotation;
Vector3 inversePosition;
Quaternion inverseRotation;
FloorUtils.InvertPositionAndRotation(position, rotation, out inversePosition, out inverseRotation);
Vector3 inversePlaneNormal;
float inversePlaneHeight;
FloorUtils.ConvertPositionAndRotationToNormalAndY(inversePosition, inverseRotation, out inversePlaneNormal, out inversePlaneHeight);
//floorTransformInverterTransform.localPosition = new Vector3(0.0f, inversePositionY, 0.0f);
//floorTransformInverterTransform.localRotation = inverseRotation;
inversePlaneNormalQueue.Enqueue(inversePlaneNormal);
inversePlaneHeightQueue.Enqueue(inversePlaneHeight);
if (inversePlaneNormalQueue.Count > 30)
{
inversePlaneNormalQueue.Dequeue();
}
if (inversePlaneHeightQueue.Count > 30)
{
inversePlaneHeightQueue.Dequeue();
}
Vector3 inversePlaneNormalSum = Vector3.zero;
foreach (var normal in inversePlaneNormalQueue)
{
inversePlaneNormalSum += normal;
}
float inversePlaneHeightSum = 0.0f;
foreach (var height in inversePlaneHeightQueue)
{
inversePlaneHeightSum += height;
}
Vector3 inversePlaneNormalAverage = inversePlaneNormalSum / inversePlaneHeightQueue.Count;
float inversePlaneHeightAverage = inversePlaneHeightSum / inversePlaneHeightQueue.Count;
Quaternion inversePlaneNormalAverageRotation = Quaternion.FromToRotation(Vector3.up, inversePlaneNormalAverage);
//floorTransformInverterTransform.localPosition = new Vector3(0.0f, inversePlaneHeightAverage, 0.0f);
gimbalTransform.localRotation = inversePlaneNormalAverageRotation;
}
