1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 #include <android-base/stringprintf.h>
17
18 #include "ModeSelector.h"
19
20 namespace android {
21 namespace media {
22 using android::base::StringAppendF;
23
ModeSelector(const Options & options,HeadTrackingMode initialMode)24 ModeSelector::ModeSelector(const Options& options, HeadTrackingMode initialMode)
25 : mOptions(options), mDesiredMode(initialMode), mActualMode(initialMode) {}
26
setDesiredMode(HeadTrackingMode mode)27 void ModeSelector::setDesiredMode(HeadTrackingMode mode) {
28 mDesiredMode = mode;
29 }
30
setScreenToStagePose(const Pose3f & screenToStage)31 void ModeSelector::setScreenToStagePose(const Pose3f& screenToStage) {
32 mScreenToStage = screenToStage;
33 }
34
setScreenToHeadPose(int64_t timestamp,const std::optional<Pose3f> & screenToHead)35 void ModeSelector::setScreenToHeadPose(int64_t timestamp,
36 const std::optional<Pose3f>& screenToHead) {
37 mScreenToHead = screenToHead;
38 mScreenToHeadTimestamp = timestamp;
39 }
40
setWorldToHeadPose(int64_t timestamp,const Pose3f & worldToHead)41 void ModeSelector::setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead) {
42 mWorldToHead = worldToHead;
43 mWorldToHeadTimestamp = timestamp;
44 }
45
setScreenStable(int64_t timestamp,bool stable)46 void ModeSelector::setScreenStable(int64_t timestamp, bool stable) {
47 mScreenStable = stable;
48 mScreenStableTimestamp = timestamp;
49 }
50
calculateActualMode(int64_t timestamp)51 void ModeSelector::calculateActualMode(int64_t timestamp) {
52 int64_t screenToHeadGap = timestamp - mScreenToHeadTimestamp;
53 int64_t worldToHeadGap = timestamp - mWorldToHeadTimestamp;
54 int64_t screenStableGap = timestamp - mScreenStableTimestamp;
55 bool isValidScreenToHead =
56 mScreenToHead.has_value() && screenToHeadGap < mOptions.freshnessTimeout;
57 bool isValidWorldToHead =
58 mWorldToHead.has_value() && worldToHeadGap < mOptions.freshnessTimeout;
59 bool isValidScreenStable =
60 mScreenStable.has_value() && screenStableGap < mOptions.freshnessTimeout;
61
62 HeadTrackingMode mode = mDesiredMode;
63
64 // Optional downgrade from screen-relative to world-relative.
65 if (mode == HeadTrackingMode::SCREEN_RELATIVE) {
66 if (!isValidScreenToHead) {
67 mode = HeadTrackingMode::WORLD_RELATIVE;
68 }
69 }
70
71 // Optional downgrade from world-relative to static.
72 if (mode == HeadTrackingMode::WORLD_RELATIVE) {
73 if (!isValidWorldToHead || !isValidScreenStable || !mScreenStable.value()) {
74 mode = HeadTrackingMode::STATIC;
75 }
76 }
77
78 if (mode != mActualMode) {
79 mLocalLog.log(
80 "HT mode change from %s to %s, this ts %0.4f ms, lastTs+gap [ScreenToHead %0.4f + "
81 "%0.4f, WorldToHead %0.4f + %0.4f, ScreenStable %0.4f + %0.4f] ms",
82 media::toString(mActualMode).c_str(), media::toString(mode).c_str(),
83 media::nsToFloatMs(timestamp), media::nsToFloatMs(mScreenToHeadTimestamp),
84 media::nsToFloatMs(screenToHeadGap), media::nsToFloatMs(mWorldToHeadTimestamp),
85 media::nsToFloatMs(worldToHeadGap), media::nsToFloatMs(mScreenStableTimestamp),
86 media::nsToFloatMs(screenStableGap));
87 mActualMode = mode;
88 }
89 }
90
calculate(int64_t timestamp)91 void ModeSelector::calculate(int64_t timestamp) {
92 calculateActualMode(timestamp);
93
94 switch (mActualMode) {
95 case HeadTrackingMode::STATIC:
96 mHeadToStage = mScreenToStage;
97 break;
98
99 case HeadTrackingMode::WORLD_RELATIVE:
100 mHeadToStage = mWorldToHead.value().inverse() * mScreenToStage;
101 break;
102
103 case HeadTrackingMode::SCREEN_RELATIVE:
104 mHeadToStage = mScreenToHead.value().inverse() * mScreenToStage;
105 break;
106 }
107 }
108
getHeadToStagePose() const109 Pose3f ModeSelector::getHeadToStagePose() const {
110 return mHeadToStage;
111 }
112
getActualMode() const113 HeadTrackingMode ModeSelector::getActualMode() const {
114 return mActualMode;
115 }
116
toString(unsigned level) const117 std::string ModeSelector::toString(unsigned level) const {
118 std::string prefixSpace(level, ' ');
119 std::string ss(prefixSpace);
120 ss.append("ModeSelector: ScreenToStage ")
121 .append(mScreenToStage.toString())
122 .append("\n")
123 .append(prefixSpace)
124 .append("Mode change history:\n")
125 .append(mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine));
126 return ss;
127 }
128
129 } // namespace media
130 } // namespace android
131