131 lines
4.5 KiB
C++
131 lines
4.5 KiB
C++
/*
|
|
* Copyright (C) 2021 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#include <android-base/stringprintf.h>
|
|
|
|
#include "ModeSelector.h"
|
|
|
|
namespace android {
|
|
namespace media {
|
|
using android::base::StringAppendF;
|
|
|
|
ModeSelector::ModeSelector(const Options& options, HeadTrackingMode initialMode)
|
|
: mOptions(options), mDesiredMode(initialMode), mActualMode(initialMode) {}
|
|
|
|
void ModeSelector::setDesiredMode(HeadTrackingMode mode) {
|
|
mDesiredMode = mode;
|
|
}
|
|
|
|
void ModeSelector::setScreenToStagePose(const Pose3f& screenToStage) {
|
|
mScreenToStage = screenToStage;
|
|
}
|
|
|
|
void ModeSelector::setScreenToHeadPose(int64_t timestamp,
|
|
const std::optional<Pose3f>& screenToHead) {
|
|
mScreenToHead = screenToHead;
|
|
mScreenToHeadTimestamp = timestamp;
|
|
}
|
|
|
|
void ModeSelector::setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead) {
|
|
mWorldToHead = worldToHead;
|
|
mWorldToHeadTimestamp = timestamp;
|
|
}
|
|
|
|
void ModeSelector::setScreenStable(int64_t timestamp, bool stable) {
|
|
mScreenStable = stable;
|
|
mScreenStableTimestamp = timestamp;
|
|
}
|
|
|
|
void ModeSelector::calculateActualMode(int64_t timestamp) {
|
|
int64_t screenToHeadGap = timestamp - mScreenToHeadTimestamp;
|
|
int64_t worldToHeadGap = timestamp - mWorldToHeadTimestamp;
|
|
int64_t screenStableGap = timestamp - mScreenStableTimestamp;
|
|
bool isValidScreenToHead =
|
|
mScreenToHead.has_value() && screenToHeadGap < mOptions.freshnessTimeout;
|
|
bool isValidWorldToHead =
|
|
mWorldToHead.has_value() && worldToHeadGap < mOptions.freshnessTimeout;
|
|
bool isValidScreenStable =
|
|
mScreenStable.has_value() && screenStableGap < mOptions.freshnessTimeout;
|
|
|
|
HeadTrackingMode mode = mDesiredMode;
|
|
|
|
// Optional downgrade from screen-relative to world-relative.
|
|
if (mode == HeadTrackingMode::SCREEN_RELATIVE) {
|
|
if (!isValidScreenToHead) {
|
|
mode = HeadTrackingMode::WORLD_RELATIVE;
|
|
}
|
|
}
|
|
|
|
// Optional downgrade from world-relative to static.
|
|
if (mode == HeadTrackingMode::WORLD_RELATIVE) {
|
|
if (!isValidWorldToHead || !isValidScreenStable || !mScreenStable.value()) {
|
|
mode = HeadTrackingMode::STATIC;
|
|
}
|
|
}
|
|
|
|
if (mode != mActualMode) {
|
|
mLocalLog.log(
|
|
"HT mode change from %s to %s, this ts %0.4f ms, lastTs+gap [ScreenToHead %0.4f + "
|
|
"%0.4f, WorldToHead %0.4f + %0.4f, ScreenStable %0.4f + %0.4f] ms",
|
|
media::toString(mActualMode).c_str(), media::toString(mode).c_str(),
|
|
media::nsToFloatMs(timestamp), media::nsToFloatMs(mScreenToHeadTimestamp),
|
|
media::nsToFloatMs(screenToHeadGap), media::nsToFloatMs(mWorldToHeadTimestamp),
|
|
media::nsToFloatMs(worldToHeadGap), media::nsToFloatMs(mScreenStableTimestamp),
|
|
media::nsToFloatMs(screenStableGap));
|
|
mActualMode = mode;
|
|
}
|
|
}
|
|
|
|
void ModeSelector::calculate(int64_t timestamp) {
|
|
calculateActualMode(timestamp);
|
|
|
|
switch (mActualMode) {
|
|
case HeadTrackingMode::STATIC:
|
|
mHeadToStage = mScreenToStage;
|
|
break;
|
|
|
|
case HeadTrackingMode::WORLD_RELATIVE:
|
|
mHeadToStage = mWorldToHead.value().inverse() * mScreenToStage;
|
|
break;
|
|
|
|
case HeadTrackingMode::SCREEN_RELATIVE:
|
|
mHeadToStage = mScreenToHead.value().inverse() * mScreenToStage;
|
|
break;
|
|
}
|
|
}
|
|
|
|
Pose3f ModeSelector::getHeadToStagePose() const {
|
|
return mHeadToStage;
|
|
}
|
|
|
|
HeadTrackingMode ModeSelector::getActualMode() const {
|
|
return mActualMode;
|
|
}
|
|
|
|
std::string ModeSelector::toString(unsigned level) const {
|
|
std::string prefixSpace(level, ' ');
|
|
std::string ss(prefixSpace);
|
|
ss.append("ModeSelector: ScreenToStage ")
|
|
.append(mScreenToStage.toString())
|
|
.append("\n")
|
|
.append(prefixSpace)
|
|
.append("Mode change history:\n")
|
|
.append(mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine));
|
|
return ss;
|
|
}
|
|
|
|
} // namespace media
|
|
} // namespace android
|