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; }