public void UpdateFrame(float[] floor) { FloorUtils.ConvertFloorFromVideoSenderMessageDataToPositionAndRotation(floor, out Vector3 position, out Quaternion rotation); floorTransform.localPosition = position; floorTransform.localRotation = rotation; FloorUtils.InvertPositionAndRotation(position, rotation, out Vector3 inversePosition, out Quaternion inverseRotation); FloorUtils.ConvertPositionAndRotationToNormalAndY(inversePosition, inverseRotation, out Vector3 inversePlaneNormal, out float 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; }
public void UpdateFrame(List <Tuple <int, VideoSenderMessageData> > videoMessageList) { VideoSenderMessageData videoMessageData = null; foreach (var videoMessagePair in videoMessageList) { if (videoMessagePair.Item2.floor != null) { videoMessageData = videoMessagePair.Item2; } } if (videoMessageData == null) { return; } Vector3 position; Quaternion rotation; FloorUtils.ConvertFloorFromVideoSenderMessageDataToPositionAndRotation(videoMessageData, 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; }