Merge "Check in Animation class." into rvc-dev am: 34cc4a549f am: ab4838662b
Original change: https://googleplex-android-review.googlesource.com/c/platform/packages/services/Car/+/11671641
Change-Id: Ide187e68bc9478bba1bc72d00a354788a2508ba6
diff --git a/surround_view/service-impl/Android.bp b/surround_view/service-impl/Android.bp
index f26d02c..02da338 100644
--- a/surround_view/service-impl/Android.bp
+++ b/surround_view/service-impl/Android.bp
@@ -44,6 +44,36 @@
}
cc_library{
+ name : "libanimation_module",
+ vendor : true,
+ srcs : [
+ "AnimationModule.cpp",
+ ],
+ shared_libs : [
+ "android.hardware.automotive.vehicle@2.0",
+ "libbase",
+ "libhidlbase",
+ "libutils",
+ ],
+}
+
+cc_test{
+ name : "animation_module_tests",
+ test_suites : ["device-tests"],
+ vendor : true,
+ srcs : ["AnimationModuleTests.cpp"],
+ shared_libs : [
+ "android.hardware.automotive.vehicle@2.0",
+ "libanimation_module",
+ "libcutils",
+ "libbase",
+ "libhidlbase",
+ "libhardware",
+ "libutils",
+ ],
+}
+
+cc_library{
name : "libvhal_handler",
vendor : true,
srcs : [
diff --git a/surround_view/service-impl/AnimationModule.cpp b/surround_view/service-impl/AnimationModule.cpp
new file mode 100644
index 0000000..ae9eb3f
--- /dev/null
+++ b/surround_view/service-impl/AnimationModule.cpp
@@ -0,0 +1,513 @@
+/*
+ * Copyright 2020 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 "AnimationModule.h"
+#include "MathHelp.h"
+
+#include <android-base/logging.h>
+#include <algorithm>
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace sv {
+namespace V1_0 {
+namespace implementation {
+namespace {
+std::array<float, 3> operator*(std::array<float, 3> lvector, float scalar) {
+ return std::array<float, 3>{
+ lvector[0] * scalar,
+ lvector[1] * scalar,
+ lvector[2] * scalar,
+ };
+}
+
+inline float getRationalNumber(const Range& mappedRange, float percentage) {
+ return mappedRange.start + (mappedRange.end - mappedRange.start) * percentage;
+}
+
+inline float getRationalNumber(const Range& mappedRange, const Range& rawRange, float rawValue) {
+ if (0 == rawRange.end - rawRange.start) {
+ return mappedRange.start;
+ }
+ const float percentage = (rawValue - rawRange.start) / (rawRange.end - rawRange.start);
+ return mappedRange.start + (mappedRange.end - mappedRange.start) * percentage > 1
+ ? 1
+ : percentage < 0 ? 0 : percentage;
+}
+
+inline uint64_t getCombinedId(const VehiclePropValue& vhalValueFloat) {
+ return static_cast<uint64_t>(vhalValueFloat.prop) << 32 | vhalValueFloat.areaId;
+}
+
+float getVhalValueFloat(const VehiclePropValue& vhalValue) {
+ int32_t type = vhalValue.prop & 0x00FF0000;
+ switch (type) {
+ case (int32_t)VehiclePropertyType::BOOLEAN:
+ return 0 == vhalValue.value.int32Values[0] ? 0.0f : 1.0f;
+ case (int32_t)VehiclePropertyType::FLOAT:
+ return vhalValue.value.floatValues[0];
+ case (int32_t)VehiclePropertyType::INT32:
+ return (float)vhalValue.value.int32Values[0];
+ case (int32_t)VehiclePropertyType::INT64:
+ return (float)vhalValue.value.int64Values[0];
+ default:
+ return 0;
+ }
+}
+} // namespace
+
+AnimationModule::AnimationModule(const std::map<std::string, CarPart>& partsMap,
+ const std::map<std::string, CarTexture>& texturesMap,
+ const std::vector<AnimationInfo>& animations) :
+ mIsCalled(false), mPartsMap(partsMap), mTexturesMap(texturesMap), mAnimations(animations) {
+ mapVhalToParts();
+ initCarPartStatus();
+}
+
+void AnimationModule::mapVhalToParts() {
+ for (const auto& animationInfo : mAnimations) {
+ auto partId = animationInfo.partId;
+ for (const auto& gammaOp : animationInfo.gammaOpsMap) {
+ if (mVhalToPartsMap.find(gammaOp.first) != mVhalToPartsMap.end()) {
+ mVhalToPartsMap.at(gammaOp.first).insert(partId);
+ } else {
+ mVhalToPartsMap.emplace(
+ std::make_pair(gammaOp.first, std::set<std::string>{partId}));
+ }
+ }
+ for (const auto& textureOp : animationInfo.textureOpsMap) {
+ if (mVhalToPartsMap.find(textureOp.first) != mVhalToPartsMap.end()) {
+ mVhalToPartsMap.at(textureOp.first).insert(partId);
+ } else {
+ mVhalToPartsMap.emplace(
+ std::make_pair(textureOp.first, std::set<std::string>{partId}));
+ }
+ }
+ for (const auto& rotationOp : animationInfo.rotationOpsMap) {
+ if (mVhalToPartsMap.find(rotationOp.first) != mVhalToPartsMap.end()) {
+ mVhalToPartsMap.at(rotationOp.first).insert(partId);
+ } else {
+ mVhalToPartsMap.emplace(
+ std::make_pair(rotationOp.first, std::set<std::string>{partId}));
+ }
+ }
+ for (const auto& translationOp : animationInfo.translationOpsMap) {
+ if (mVhalToPartsMap.find(translationOp.first) != mVhalToPartsMap.end()) {
+ mVhalToPartsMap.at(translationOp.first).insert(partId);
+ } else {
+ mVhalToPartsMap.emplace(
+ std::make_pair(translationOp.first, std::set<std::string>{partId}));
+ }
+ }
+ mPartsToAnimationMap.emplace(std::make_pair(partId, animationInfo));
+ }
+}
+
+void AnimationModule::initCarPartStatus() {
+ for (const auto& part : mPartsMap) {
+ mCarPartsStatusMap.emplace(std::make_pair(part.first,
+ CarPartStatus{
+ .partId = part.first,
+ .childIds = part.second.child_part_ids,
+ .parentModel = gMat4Identity,
+ .localModel = gMat4Identity,
+ .currentModel = gMat4Identity,
+ .gamma = 1,
+ }));
+ }
+
+ for (const auto& eachVhalToParts : mVhalToPartsMap) {
+ for (const auto& part : eachVhalToParts.second) {
+ if (mCarPartsStatusMap.at(part).vhalProgressMap.find(eachVhalToParts.first) !=
+ mCarPartsStatusMap.at(part).vhalProgressMap.end()) {
+ mCarPartsStatusMap.at(part).vhalProgressMap.at(eachVhalToParts.first) = 0.0f;
+ } else {
+ mCarPartsStatusMap.at(part).vhalProgressMap.emplace(
+ std::make_pair(eachVhalToParts.first, 0.0f));
+ }
+ if (mCarPartsStatusMap.at(part).vhalOffMap.find(eachVhalToParts.first) !=
+ mCarPartsStatusMap.at(part).vhalOffMap.end()) {
+ mCarPartsStatusMap.at(part).vhalOffMap.at(eachVhalToParts.first) = true;
+ } else {
+ mCarPartsStatusMap.at(part).vhalOffMap.emplace(
+ std::make_pair(eachVhalToParts.first, true));
+ }
+ }
+ }
+}
+
+// This implementation assumes the tree level is small. If tree level is large,
+// we may need to traverse the tree once and process each node(part) during
+// the reaversal.
+void AnimationModule::updateChildrenParts(const std::string& partId, const Mat4x4& parentModel) {
+ for (auto& childPart : mCarPartsStatusMap.at(partId).childIds) {
+ mCarPartsStatusMap.at(childPart).parentModel = parentModel;
+ appendMat(parentModel, mCarPartsStatusMap.at(childPart).parentModel);
+ mCarPartsStatusMap.at(childPart).currentModel =
+ appendMat(mCarPartsStatusMap.at(childPart).localModel,
+ mCarPartsStatusMap.at(childPart).parentModel);
+ if (mUpdatedPartsMap.find(childPart) == mUpdatedPartsMap.end()) {
+ AnimationParam animationParam(partId);
+ animationParam.SetModelMatrix(mCarPartsStatusMap.at(childPart).currentModel);
+ mUpdatedPartsMap.emplace(std::make_pair(partId, animationParam));
+ } else { // existing part in the map
+ mUpdatedPartsMap.at(partId).SetModelMatrix(
+ mCarPartsStatusMap.at(childPart).currentModel);
+ }
+ updateChildrenParts(childPart, mCarPartsStatusMap.at(childPart).currentModel);
+ }
+}
+
+void AnimationModule::performGammaOp(const std::string& partId, uint64_t vhalProperty,
+ const GammaOp& gammaOp) {
+ CarPartStatus& currentCarPartStatus = mCarPartsStatusMap.at(partId);
+ float& currentProgress = currentCarPartStatus.vhalProgressMap.at(vhalProperty);
+ if (currentCarPartStatus.vhalOffMap.at(vhalProperty)) { // process off signal
+ if (currentProgress > 0) { // part not rest
+ if (0 == gammaOp.animationTime) {
+ currentCarPartStatus.gamma = gammaOp.gammaRange.start;
+ currentProgress = 0.0f;
+ } else {
+ const float progressDelta =
+ (mCurrentCallTime - mLastCallTime) / gammaOp.animationTime;
+ if (progressDelta > currentProgress) {
+ currentCarPartStatus.gamma = gammaOp.gammaRange.start;
+ currentProgress = 0.0f;
+ } else {
+ currentCarPartStatus.gamma =
+ getRationalNumber(gammaOp.gammaRange, currentProgress - progressDelta);
+ currentProgress -= progressDelta;
+ }
+ }
+ } else {
+ return;
+ }
+ } else { // regular signal process
+ if (0 == gammaOp.animationTime) { // continuous value
+ currentCarPartStatus.gamma =
+ getRationalNumber(gammaOp.gammaRange, gammaOp.vhalRange,
+ mVhalStatusMap.at(vhalProperty).vhalValueFloat);
+ currentProgress = mVhalStatusMap.at(vhalProperty).vhalValueFloat;
+ } else { // non-continuous value
+ const float progressDelta = (mCurrentCallTime - mLastCallTime) / gammaOp.animationTime;
+ if (gammaOp.type == ADJUST_GAMMA_ONCE) {
+ if (progressDelta + currentCarPartStatus.vhalProgressMap.at(vhalProperty) > 1) {
+ currentCarPartStatus.gamma = gammaOp.gammaRange.end;
+ currentProgress = 1.0f;
+ } else {
+ currentCarPartStatus.gamma =
+ getRationalNumber(gammaOp.gammaRange, currentProgress + progressDelta);
+ currentProgress += progressDelta;
+ }
+ } else if (gammaOp.type == ADJUST_GAMMA_REPEAT) {
+ if (progressDelta + currentCarPartStatus.vhalProgressMap.at(vhalProperty) > 1) {
+ if (progressDelta + currentCarPartStatus.vhalProgressMap.at(vhalProperty) - 1 >
+ 1) {
+ currentCarPartStatus.gamma =
+ currentCarPartStatus.vhalProgressMap.at(vhalProperty) > 0.5
+ ? gammaOp.gammaRange.start
+ : gammaOp.gammaRange.end;
+ currentProgress =
+ currentCarPartStatus.vhalProgressMap.at(vhalProperty) > 0.5 ? 0.0f
+ : 1.0f;
+ } else {
+ currentCarPartStatus.gamma =
+ getRationalNumber(gammaOp.gammaRange,
+ progressDelta +
+ currentCarPartStatus.vhalProgressMap.at(
+ vhalProperty) -
+ 1);
+ currentProgress += progressDelta - 1;
+ }
+ } else {
+ currentCarPartStatus.gamma =
+ getRationalNumber(gammaOp.gammaRange, currentProgress + progressDelta);
+ currentProgress += progressDelta;
+ }
+ } else {
+ LOG(ERROR) << "Error type of gamma op: " << gammaOp.type;
+ }
+ }
+ }
+
+ if (mUpdatedPartsMap.find(partId) == mUpdatedPartsMap.end()) {
+ AnimationParam animationParam(partId);
+ animationParam.SetGamma(currentCarPartStatus.gamma);
+ mUpdatedPartsMap.emplace(std::make_pair(partId, animationParam));
+ } else { // existing part in the map
+ mUpdatedPartsMap.at(partId).SetGamma(currentCarPartStatus.gamma);
+ }
+}
+
+void AnimationModule::performTranslationOp(const std::string& partId, uint64_t vhalProperty,
+ const TranslationOp& translationOp) {
+ CarPartStatus& currentCarPartStatus = mCarPartsStatusMap.at(partId);
+ float& currentProgress = currentCarPartStatus.vhalProgressMap.at(vhalProperty);
+ if (currentCarPartStatus.vhalOffMap.at(vhalProperty)) { // process off signal
+ if (currentProgress > 0) {
+ // part not rest
+ if (0 == translationOp.animationTime) {
+ currentCarPartStatus.localModel = gMat4Identity;
+ currentCarPartStatus.currentModel = currentCarPartStatus.parentModel;
+ currentProgress = 0.0f;
+ } else {
+ const float progressDelta =
+ (mCurrentCallTime - mLastCallTime) / translationOp.animationTime;
+ float translationUnit =
+ getRationalNumber(translationOp.translationRange,
+ std::max(currentProgress - progressDelta, 0.0f));
+ currentCarPartStatus.localModel =
+ translationMatrixToMat4x4(translationOp.direction * translationUnit);
+ currentCarPartStatus.currentModel = appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress = std::max(currentProgress - progressDelta, 0.0f);
+ }
+ } else {
+ return;
+ }
+ } else { // regular signal process
+ if (translationOp.type == TRANSLATION) {
+ if (0 == translationOp.animationTime) {
+ float translationUnit =
+ getRationalNumber(translationOp.translationRange, translationOp.vhalRange,
+ mVhalStatusMap.at(vhalProperty).vhalValueFloat);
+ currentCarPartStatus.localModel =
+ translationMatrixToMat4x4(translationOp.direction * translationUnit);
+ currentCarPartStatus.currentModel = appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress = mVhalStatusMap.at(vhalProperty).vhalValueFloat;
+ } else {
+ float progressDelta =
+ (mCurrentCallTime - mLastCallTime) / translationOp.animationTime;
+ if (progressDelta + currentCarPartStatus.vhalProgressMap.at(vhalProperty) > 1) {
+ float translationUnit = translationOp.translationRange.end;
+
+ currentCarPartStatus.localModel =
+ translationMatrixToMat4x4(translationOp.direction * translationUnit);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress = 1.0f;
+ } else {
+ float translationUnit = getRationalNumber(translationOp.translationRange,
+ progressDelta + currentProgress);
+ currentCarPartStatus.localModel =
+ translationMatrixToMat4x4(translationOp.direction * translationUnit);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress += progressDelta;
+ }
+ }
+ } else {
+ LOG(ERROR) << "Error type of translation op: " << translationOp.type;
+ }
+ }
+ if (mUpdatedPartsMap.find(partId) == mUpdatedPartsMap.end()) {
+ AnimationParam animationParam(partId);
+ animationParam.SetModelMatrix(currentCarPartStatus.currentModel);
+ mUpdatedPartsMap.emplace(std::make_pair(partId, animationParam));
+ } else { // existing part in the map
+ mUpdatedPartsMap.at(partId).SetModelMatrix(currentCarPartStatus.currentModel);
+ }
+ updateChildrenParts(partId, currentCarPartStatus.currentModel);
+}
+
+void AnimationModule::performRotationOp(const std::string& partId, uint64_t vhalProperty,
+ const RotationOp& rotationOp) {
+ CarPartStatus& currentCarPartStatus = mCarPartsStatusMap.at(partId);
+ float& currentProgress = currentCarPartStatus.vhalProgressMap.at(vhalProperty);
+ if (currentCarPartStatus.vhalOffMap.at(vhalProperty)) {
+ // process off signal
+ if (currentProgress > 0) { // part not rest
+ if (0 == rotationOp.animationTime) {
+ currentCarPartStatus.localModel = gMat4Identity;
+ currentCarPartStatus.currentModel = currentCarPartStatus.parentModel;
+ currentProgress = 0.0f;
+ } else {
+ const float progressDelta =
+ (mCurrentCallTime - mLastCallTime) / rotationOp.animationTime;
+ if (progressDelta > currentProgress) {
+ currentCarPartStatus.localModel = gMat4Identity;
+ currentCarPartStatus.currentModel = currentCarPartStatus.parentModel;
+ currentProgress = 0.0f;
+ } else {
+ float anlgeInDegree = getRationalNumber(rotationOp.rotationRange,
+ currentProgress - progressDelta);
+ currentCarPartStatus.localModel =
+ rotationAboutPoint(anlgeInDegree, rotationOp.axis.rotationPoint,
+ rotationOp.axis.axisVector);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress -= progressDelta;
+ }
+ }
+ } else {
+ return;
+ }
+ } else { // regular signal process
+ if (rotationOp.type == ROTATION_ANGLE) {
+ if (0 == rotationOp.animationTime) {
+ float angleInDegree =
+ getRationalNumber(rotationOp.rotationRange, rotationOp.vhalRange,
+ mVhalStatusMap.at(vhalProperty).vhalValueFloat);
+ currentCarPartStatus.localModel =
+ rotationAboutPoint(angleInDegree, rotationOp.axis.rotationPoint,
+ rotationOp.axis.axisVector);
+ currentCarPartStatus.currentModel = appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress = mVhalStatusMap.at(vhalProperty).vhalValueFloat;
+ } else {
+ float progressDelta = (mCurrentCallTime - mLastCallTime) / rotationOp.animationTime;
+ if (progressDelta + currentProgress > 1) {
+ float angleInDegree = rotationOp.rotationRange.end;
+ currentCarPartStatus.localModel =
+ rotationAboutPoint(angleInDegree, rotationOp.axis.rotationPoint,
+ rotationOp.axis.axisVector);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress = 1.0f;
+ } else {
+ float anlgeInDegree = getRationalNumber(rotationOp.rotationRange,
+ currentProgress + progressDelta);
+ currentCarPartStatus.localModel =
+ rotationAboutPoint(anlgeInDegree, rotationOp.axis.rotationPoint,
+ rotationOp.axis.axisVector);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel,
+ currentCarPartStatus.parentModel);
+ currentProgress += progressDelta;
+ }
+ }
+ } else if (rotationOp.type == ROTATION_SPEED) {
+ float angleDelta = (mCurrentCallTime - mLastCallTime) *
+ getRationalNumber(rotationOp.rotationRange, rotationOp.vhalRange,
+ mVhalStatusMap.at(vhalProperty)
+ .vhalValueFloat); // here vhalValueFloat unit is
+ // radian/ms.
+ currentCarPartStatus.localModel =
+ appendMat(rotationAboutPoint(angleDelta, rotationOp.axis.rotationPoint,
+ rotationOp.axis.axisVector),
+ currentCarPartStatus.localModel);
+ currentCarPartStatus.currentModel =
+ appendMatrix(currentCarPartStatus.localModel, currentCarPartStatus.parentModel);
+ currentProgress = 1.0f;
+ } else {
+ LOG(ERROR) << "Error type of rotation op: " << rotationOp.type;
+ }
+ }
+ if (mUpdatedPartsMap.find(partId) == mUpdatedPartsMap.end()) {
+ AnimationParam animationParam(partId);
+ animationParam.SetModelMatrix(currentCarPartStatus.currentModel);
+ mUpdatedPartsMap.emplace(std::make_pair(partId, animationParam));
+ } else { // existing part in the map
+ mUpdatedPartsMap.at(partId).SetModelMatrix(currentCarPartStatus.currentModel);
+ }
+ updateChildrenParts(partId, currentCarPartStatus.currentModel);
+}
+
+std::vector<AnimationParam> AnimationModule::getUpdatedAnimationParams(
+ const std::vector<VehiclePropValue>& vehiclePropValue) {
+ mLastCallTime = mCurrentCallTime;
+ if (!mIsCalled) {
+ mIsCalled = true;
+ mLastCallTime = (float)elapsedRealtimeNano() / 1e6;
+ }
+
+ // get current time
+ mCurrentCallTime = (float)elapsedRealtimeNano() / 1e6;
+
+ // reset mUpdatedPartsMap
+ mUpdatedPartsMap.clear();
+
+ for (const auto& vhalSignal : vehiclePropValue) {
+ // existing vhal signal
+ const uint64_t combinedId = getCombinedId(vhalSignal);
+ if (mVhalToPartsMap.find(combinedId) != mVhalToPartsMap.end()) {
+ const float valueFloat = getVhalValueFloat(vhalSignal);
+ if (mVhalStatusMap.find(combinedId) != mVhalStatusMap.end()) {
+ mVhalStatusMap.at(combinedId).vhalValueFloat = valueFloat;
+ } else {
+ mVhalStatusMap.emplace(std::make_pair(combinedId,
+ VhalStatus{
+ .vhalValueFloat = valueFloat,
+ }));
+ }
+ bool offStatus = 0 == valueFloat;
+ for (const auto& eachPart : mVhalToPartsMap.at(combinedId)) {
+ mCarPartsStatusMap.at(eachPart).vhalOffMap.at(combinedId) = offStatus;
+ }
+ }
+ }
+
+ for (auto& vhalStatus : mVhalStatusMap) {
+ // VHAL signal not found in animation
+ uint64_t vhalProperty = vhalStatus.first;
+ if (mVhalToPartsMap.find(vhalProperty) == mVhalToPartsMap.end()) {
+ LOG(WARNING) << "VHAL " << vhalProperty << " not processed.";
+ } else { // VHAL signal found
+ const auto& partsSet = mVhalToPartsMap.at(vhalProperty);
+ for (const auto& partId : partsSet) {
+ const auto& animationInfo = mPartsToAnimationMap.at(partId);
+ if (animationInfo.gammaOpsMap.find(vhalProperty) !=
+ animationInfo.gammaOpsMap.end()) {
+ LOG(INFO) << "Processing VHAL " << vhalProperty << " for gamma op.";
+ // TODO(b/158244276): add priority check.
+ for (const auto& gammaOp : animationInfo.gammaOpsMap.at(vhalProperty)) {
+ performGammaOp(partId, vhalProperty, gammaOp);
+ }
+ }
+ if (animationInfo.textureOpsMap.find(vhalProperty) !=
+ animationInfo.textureOpsMap.end()) {
+ LOG(INFO) << "Processing VHAL " << vhalProperty << " for texture op.";
+ LOG(INFO) << "Texture op currently not supported. Skipped.";
+ // TODO(b158244721): do texture op.
+ }
+ if (animationInfo.rotationOpsMap.find(vhalProperty) !=
+ animationInfo.rotationOpsMap.end()) {
+ LOG(INFO) << "Processing VHAL " << vhalProperty << " for rotation op.";
+ for (const auto& rotationOp : animationInfo.rotationOpsMap.at(vhalProperty)) {
+ performRotationOp(partId, vhalProperty, rotationOp);
+ }
+ }
+ if (animationInfo.translationOpsMap.find(vhalProperty) !=
+ animationInfo.translationOpsMap.end()) {
+ LOG(INFO) << "Processing VHAL " << vhalProperty << " for translation op.";
+ for (const auto& translationOp :
+ animationInfo.translationOpsMap.at(vhalProperty)) {
+ performTranslationOp(partId, vhalProperty, translationOp);
+ }
+ }
+ }
+ }
+ }
+
+ std::vector<AnimationParam> output;
+ for (auto& updatedPart : mUpdatedPartsMap) {
+ output.push_back(updatedPart.second);
+ }
+ return output;
+}
+
+} // namespace implementation
+} // namespace V1_0
+} // namespace sv
+} // namespace automotive
+} // namespace hardware
+} // namespace android
diff --git a/surround_view/service-impl/AnimationModule.h b/surround_view/service-impl/AnimationModule.h
new file mode 100644
index 0000000..52845ac
--- /dev/null
+++ b/surround_view/service-impl/AnimationModule.h
@@ -0,0 +1,356 @@
+/*
+ * Copyright 2020 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.
+ */
+
+#ifndef SURROUND_VIEW_SERVICE_IMPL_ANIMATION_H_
+#define SURROUND_VIEW_SERVICE_IMPL_ANIMATION_H_
+
+#include "core_lib.h"
+
+#include <utils/SystemClock.h>
+#include <cstdint>
+#include <map>
+#include <set>
+#include <vector>
+
+#include <android/hardware/automotive/vehicle/2.0/IVehicle.h>
+
+using namespace ::android::hardware::automotive::vehicle::V2_0;
+using namespace android_auto::surround_view;
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace sv {
+namespace V1_0 {
+namespace implementation {
+
+struct Range {
+ // Range start.
+ // Start value may be greater than end value.
+ float start;
+
+ // Range end.
+ float end;
+};
+
+// Rotation axis
+struct RotationAxis {
+ // Unit axis direction vector.
+ std::array<float, 3> axisVector;
+
+ // Rotate about this point.
+ std::array<float, 3> rotationPoint;
+};
+
+enum AnimationType {
+ // Rotate a part about an axis from a start to end angle.
+ ROTATION_ANGLE = 0,
+
+ // Continuously rotate a part about an axis by a specified angular speed.
+ ROTATION_SPEED = 1,
+
+ // Linearly translates a part from one point to another.
+ TRANSLATION = 2,
+
+ // Switch to another texture once.
+ SWITCH_TEXTURE_ONCE = 3,
+
+ // Adjust the brightness of the texture once.
+ ADJUST_GAMMA_ONCE = 4,
+
+ // Repeatedly toggle between two textures.
+ SWITCH_TEXTURE_REPEAT = 5,
+
+ // Repeatedly toggle between two gamma values.
+ ADJUST_GAMMA_REPEAT = 6,
+};
+
+// Rotation operation
+struct RotationOp {
+ // VHAL signal to trigger operation.
+ uint64_t vhalProperty;
+
+ // Rotation operation type.
+ AnimationType type;
+
+ // Rotation axis.
+ RotationAxis axis;
+
+ // Default rotation (angle/speed) value.
+ // It is used for default rotation when the signal is on while vhal_range is
+ // not provided.
+ float defaultRotationValue;
+
+ // Default animation time elapsed to finish the rotation operation.
+ // It is ignored if VHAL provides continuous signal value.
+ float animationTime;
+
+ // physical rotation range with start mapped to vhal_range start and
+ // end mapped to vhal_range end.
+ Range rotationRange;
+
+ // VHAL signal range.
+ // Un-supported types: STRING, BYTES and VEC
+ // Refer: hardware/interfaces/automotive/vehicle/2.0/types.hal
+ // VehiclePropertyType
+ Range vhalRange;
+};
+
+// Translation operation.
+struct TranslationOp {
+ // VHAL signal to trigger operation.
+ uint64_t vhalProperty;
+
+ // Translation operation type.
+ AnimationType type;
+
+ // Unit direction vector.
+ std::array<float, 3> direction;
+
+ // Default translation value.
+ // It is used for default translation when the signal is on while vhal_range
+ // is not provided.
+ float defaultTranslationValue;
+
+ // Default animation time elapsed to finish the texture operation.
+ // It is ignored if VHAL provides continuous signal value.
+ float animationTime;
+
+ // Physical translation range with start mapped to vhal_range start and
+ // end mapped to vhal_range end.
+ Range translationRange;
+
+ // VHAL signal range.
+ // Un-supported types: STRING, BYTES and VEC
+ // Refer: hardware/interfaces/automotive/vehicle/2.0/types.hal
+ // VehiclePropertyType
+ Range vhalRange;
+};
+
+// Texture operation.
+struct TextureOp {
+ // VHAL signal to trigger operation.
+ uint64_t vhalProperty;
+
+ // Texture operation type.
+ AnimationType type;
+
+ // Default texture id.
+ // It is used as default texture when the signal is on while vhal_range is
+ // not provided.
+ std::string defaultTexture;
+
+ // Default animation time elapsed to finish the texture operation.
+ // Unit is milliseconds.
+ // If the animation time is specified, the vhal_property is assumed to be
+ // on/off type.
+ // It is ignored if it is equal or less than zero and vhal_property is
+ // assumed to provide continuous value.
+ int animationTime;
+
+ // texture range mapped to texture_ids[i].first.
+ Range textureRange;
+
+ // VHAL signal range.
+ // Un-supported types: STRING, BYTES and VEC
+ // Refer: hardware/interfaces/automotive/vehicle/2.0/types.hal
+ // VehiclePropertyType
+ Range vhalRange;
+
+ // Texture ids for switching textures.
+ // Applicable for animation types: kSwitchTextureOnce and
+ // kSwitchTextureRepeated
+ // 0 - n-1
+ std::vector<std::pair<float, std::string>> textureIds;
+};
+
+// Gamma operation.
+struct GammaOp {
+ // VHAL signal to trigger operation.
+ uint64_t vhalProperty;
+
+ // Texture operation type.
+ // Applicable for animation types: kAdjustGammaOnce and kAdjustGammaRepeat.
+ AnimationType type;
+
+ // Default animation time elapsed to finish the gamma operation.
+ // Unit is milliseconds.
+ // If the animation time is specified, the vhal_property is assumed to be
+ // on/off type.
+ // It is ignored if it is equal or less than zero and vhal_property is
+ // assumed to provide continuous value.
+ int animationTime;
+
+ // Gamma range with start mapped to vhal_range start and
+ // end mapped to vhal_range end.
+ Range gammaRange;
+
+ // VHAL signal range.
+ // Un-supported types: STRING, BYTES and VEC
+ // Refer: hardware/interfaces/automotive/vehicle/2.0/types.hal
+ // VehiclePropertyType
+ Range vhalRange;
+};
+
+// Animation info of a car part
+struct AnimationInfo {
+ // Car animation part id(name). It is a unique id.
+ std::string partId;
+
+ // Car part parent name.
+ std::string parentId;
+
+ // Car part pose w.r.t parent's coordinate.
+ Mat4x4 pose;
+
+ // VHAL priority from high [0] to low [n-1]. Only VHALs specified in the
+ // vector have priority.
+ std::vector<uint64_t> vhalPriority;
+
+ // TODO(b/158245554): simplify xxOpsMap data structs.
+ // Map of gamma operations. Key value is VHAL property.
+ std::map<uint64_t, std::vector<GammaOp>> gammaOpsMap;
+
+ // Map of texture operations. Key value is VHAL property.
+ std::map<uint64_t, std::vector<TextureOp>> textureOpsMap;
+
+ // Map of rotation operations. Key value is VHAL property.
+ // Multiple rotation ops are supported and will be simultaneously animated in
+ // order if their rotation axis are different and rotation points are the
+ // same.
+ std::map<uint64_t, std::vector<RotationOp>> rotationOpsMap;
+
+ // Map of translation operations. Key value is VHAL property.
+ std::map<uint64_t, std::vector<TranslationOp>> translationOpsMap;
+};
+
+// Car animation class. It is constructed with textures, animations, and
+// vhal_handler. It automatically updates animation params when
+// GetUpdatedAnimationParams() is called.
+class AnimationModule {
+public:
+ // Constructor.
+ // |parts| is from I/O module. The key value is part id.
+ // |textures| is from I/O module. The key value is texture id.
+ // |animations| is from I/O module.
+ AnimationModule(const std::map<std::string, CarPart>& partsMap,
+ const std::map<std::string, CarTexture>& texturesMap,
+ const std::vector<AnimationInfo>& animations);
+
+ // Gets Animation parameters with input of VehiclePropValue.
+ std::vector<AnimationParam> getUpdatedAnimationParams(
+ const std::vector<VehiclePropValue>& vehiclePropValue);
+
+private:
+ // Internal car part status.
+ struct CarPartStatus {
+ // Car part id.
+ std::string partId;
+
+ // Car part children ids.
+ std::vector<std::string> childIds;
+
+ // Parent model matrix.
+ Mat4x4 parentModel;
+
+ // Local model in local coordinate.
+ Mat4x4 localModel;
+
+ // Current status model matrix in global coordinate with
+ // animations combined.
+ // current_model = local_model * parent_model;
+ Mat4x4 currentModel;
+
+ // Gamma parameters.
+ float gamma;
+
+ // Texture id.
+ std::string textureId;
+
+ // Internal vhal percentage. Each car part maintain its own copy
+ // the vhal percentage.
+ // Key value is vhal property (combined with area id).
+ std::map<uint64_t, float> vhalProgressMap;
+
+ // Vhal off map. Key value is vhal property (combined with area id).
+ // Assume off status when vhal value is 0.
+ std::map<uint64_t, bool> vhalOffMap;
+ };
+
+ // Internal Vhal status.
+ struct VhalStatus {
+ float vhalValueFloat;
+ };
+
+ // Help function to get vhal to parts map.
+ void mapVhalToParts();
+
+ // Help function to init car part status for constructor.
+ void initCarPartStatus();
+
+ // Iteratively update children parts status if partent status is changed.
+ void updateChildrenParts(const std::string& partId, const Mat4x4& parentModel);
+
+ // Perform gamma opertion for the part with given vhal property.
+ void performGammaOp(const std::string& partId, uint64_t vhalProperty, const GammaOp& gammaOp);
+
+ // Perform translation opertion for the part with given vhal property.
+ void performTranslationOp(const std::string& partId, uint64_t vhalProperty,
+ const TranslationOp& translationOp);
+
+ // Perform texture opertion for the part with given vhal property.
+ // Not implemented yet.
+ void performTextureOp(const std::string& partId, uint64_t vhalProperty,
+ const TextureOp& textureOp);
+
+ // Perform rotation opertion for the part with given vhal property.
+ void performRotationOp(const std::string& partId, uint64_t vhalProperty,
+ const RotationOp& rotationOp);
+
+ // Last call time of GetUpdatedAnimationParams() in millisecond.
+ float mLastCallTime;
+
+ // Current call time of GetUpdatedAnimationParams() in millisecond.
+ float mCurrentCallTime;
+
+ // Flag indicating if GetUpdatedAnimationParams() was called before.
+ bool mIsCalled;
+
+ std::map<std::string, CarPart> mPartsMap;
+
+ std::map<std::string, CarTexture> mTexturesMap;
+
+ std::vector<AnimationInfo> mAnimations;
+
+ std::map<std::string, AnimationInfo> mPartsToAnimationMap;
+
+ std::map<uint64_t, VhalStatus> mVhalStatusMap;
+
+ std::map<uint64_t, std::set<std::string>> mVhalToPartsMap;
+
+ std::map<std::string, CarPartStatus> mCarPartsStatusMap;
+
+ std::map<std::string, AnimationParam> mUpdatedPartsMap;
+};
+
+} // namespace implementation
+} // namespace V1_0
+} // namespace sv
+} // namespace automotive
+} // namespace hardware
+} // namespace android
+
+#endif // SURROUND_VIEW_SERVICE_IMPL_ANIMATION_H_
diff --git a/surround_view/service-impl/AnimationModuleTests.cpp b/surround_view/service-impl/AnimationModuleTests.cpp
new file mode 100644
index 0000000..5d6e5d5
--- /dev/null
+++ b/surround_view/service-impl/AnimationModuleTests.cpp
@@ -0,0 +1,439 @@
+/*
+ * Copyright 2020 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.
+ */
+
+#define LOG_TAG "AnimationModuleTests"
+
+#include "AnimationModule.h"
+#include "MathHelp.h"
+
+#include <android/hardware/automotive/vehicle/2.0/IVehicle.h>
+#include <gtest/gtest.h>
+#include <map>
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace sv {
+namespace V1_0 {
+namespace implementation {
+namespace {
+
+std::map<std::string, CarPart> getSampleCarPartsMap() {
+ std::vector<std::string> carFrameChildPartIds{"front_left_door", "front_right_door",
+ "front_left_blinker", "front_right_blinker",
+ "sun_roof"};
+
+ android_auto::surround_view::CarPart frame(std::vector<CarVertex>(),
+ android_auto::surround_view::CarMaterial(),
+ gMat4Identity, "root", carFrameChildPartIds);
+
+ android_auto::surround_view::CarPart frameChild(std::vector<CarVertex>(),
+ android_auto::surround_view::CarMaterial(),
+ gMat4Identity, "frame",
+ std::vector<std::string>());
+
+ std::map<std::string, CarPart> sampleCarParts;
+ sampleCarParts.emplace(std::make_pair("frame", frame));
+ sampleCarParts.emplace(std::make_pair("front_left_door", frameChild));
+ sampleCarParts.emplace(std::make_pair("front_right_door", frameChild));
+ sampleCarParts.emplace(std::make_pair("front_left_blinker", frameChild));
+ sampleCarParts.emplace(std::make_pair("front_right_blinker", frameChild));
+ sampleCarParts.emplace(std::make_pair("sun_roof", frameChild));
+ return sampleCarParts;
+}
+
+std::vector<AnimationInfo> getSampleAnimations() {
+ AnimationInfo frameAnimation = AnimationInfo{
+ .partId = "frame",
+ .parentId = "root",
+ .pose = gMat4Identity,
+ };
+
+ RotationOp frontLeftDoorRotationOp =
+ RotationOp{.vhalProperty = (int64_t)(0x0200 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 | VehicleArea::DOOR)
+ << 32 |
+ (int64_t)(VehicleArea::DOOR),
+ .type = AnimationType::ROTATION_ANGLE,
+ .axis =
+ RotationAxis{
+ .axisVector = std::array<float, 3>{0.0f, 0.0f, 1.0f},
+ .rotationPoint = std::array<float, 3>{-1.0f, 0.5f, 0.0f},
+ },
+ .animationTime = 2000,
+ .rotationRange =
+ Range{
+ .start = 0.0f,
+ .end = 90.0f,
+ },
+ .vhalRange = Range{
+ .start = 0.0f,
+ .end = (float)INT32_MAX,
+ }};
+
+ std::map<uint64_t, std::vector<RotationOp>> frontLeftDoorRotationOpsMap;
+
+ frontLeftDoorRotationOpsMap.emplace(
+ std::make_pair(frontLeftDoorRotationOp.vhalProperty,
+ std::vector<RotationOp>{frontLeftDoorRotationOp}));
+
+ AnimationInfo frontLeftDoorAnimation = AnimationInfo{
+ .partId = "front_left_door",
+ .parentId = "frame",
+ .pose = gMat4Identity,
+ .rotationOpsMap = frontLeftDoorRotationOpsMap,
+ };
+
+ RotationOp frontRightDoorRotationOp =
+ RotationOp{.vhalProperty = (int64_t)(0x0201 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 | VehicleArea::DOOR)
+ << 32 |
+ (int64_t)(VehicleArea::DOOR),
+ .type = AnimationType::ROTATION_ANGLE,
+ .axis =
+ RotationAxis{
+ .axisVector = std::array<float, 3>{0.0f, 0.0f, 1.0f},
+ .rotationPoint = std::array<float, 3>{1.0f, 0.5f, 0.0f},
+ },
+ .animationTime = 2000,
+ .rotationRange =
+ Range{
+ .start = 0.0f,
+ .end = -90.0f,
+ },
+ .vhalRange = Range{
+ .start = 0.0f,
+ .end = (float)INT32_MAX,
+ }};
+
+ std::map<uint64_t, std::vector<RotationOp>> frontRightDoorRotationOpsMap;
+
+ frontRightDoorRotationOpsMap.emplace(
+ std::make_pair(frontRightDoorRotationOp.vhalProperty,
+ std::vector<RotationOp>{frontRightDoorRotationOp}));
+
+ AnimationInfo frontRightDoorAnimation = AnimationInfo{
+ .partId = "front_right_door",
+ .parentId = "frame",
+ .pose = gMat4Identity,
+ .rotationOpsMap = frontRightDoorRotationOpsMap,
+ };
+
+ GammaOp frontLeftBlinkerGammaOp = GammaOp{
+ .vhalProperty = (int64_t)(0x0300 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 | VehicleArea::GLOBAL)
+ << 32 |
+ (int64_t)(VehicleArea::GLOBAL),
+ .type = AnimationType::ADJUST_GAMMA_REPEAT,
+ .animationTime = 1000,
+ .gammaRange =
+ Range{
+ .start = 1.0f,
+ .end = 0.5f,
+ },
+ .vhalRange =
+ Range{
+ .start = 0.0f,
+ .end = (float)INT32_MAX,
+ },
+ };
+
+ std::map<uint64_t, std::vector<GammaOp>> frontLeftBlinkerGammaOpsMap;
+
+ frontLeftBlinkerGammaOpsMap.emplace(
+ std::make_pair(frontLeftBlinkerGammaOp.vhalProperty,
+ std::vector<GammaOp>{frontLeftBlinkerGammaOp}));
+
+ AnimationInfo frontLeftBlinkerAnimation = AnimationInfo{
+ .partId = "front_left_blinker",
+ .parentId = "frame",
+ .pose = gMat4Identity,
+ .gammaOpsMap = frontLeftBlinkerGammaOpsMap,
+ };
+
+ GammaOp frontRightBlinkerGammaOp = GammaOp{
+ .vhalProperty = (int64_t)(0x0301 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 | VehicleArea::GLOBAL)
+ << 32 |
+ (int64_t)(VehicleArea::GLOBAL),
+ .type = AnimationType::ADJUST_GAMMA_REPEAT,
+ .animationTime = 1000,
+ .gammaRange =
+ Range{
+ .start = 1.0f,
+ .end = 0.5f,
+ },
+ .vhalRange =
+ Range{
+ .start = 0.0f,
+ .end = (float)INT32_MAX,
+ },
+ };
+
+ std::map<uint64_t, std::vector<GammaOp>> frontRightBlinkerGammaOpsMap;
+
+ frontRightBlinkerGammaOpsMap.emplace(
+ std::make_pair(frontRightBlinkerGammaOp.vhalProperty,
+ std::vector<GammaOp>{frontRightBlinkerGammaOp}));
+
+ AnimationInfo frontRightBlinkerAnimation = AnimationInfo{
+ .partId = "front_right_blinker",
+ .parentId = "frame",
+ .pose = gMat4Identity,
+ .gammaOpsMap = frontRightBlinkerGammaOpsMap,
+ };
+
+ TranslationOp sunRoofTranslationOp = TranslationOp{
+ .vhalProperty = (int64_t)(0x0400 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 | VehicleArea::GLOBAL)
+ << 32 |
+ (int64_t)(VehicleArea::GLOBAL),
+ .type = AnimationType::TRANSLATION,
+ .direction = std::array<float, 3>{0.0f, -1.0f, 0.0f},
+ .animationTime = 3000,
+ .translationRange =
+ Range{
+ .start = 0.0f,
+ .end = 0.5f,
+ },
+ .vhalRange =
+ Range{
+ .start = 0.0f,
+ .end = (float)INT32_MAX,
+ },
+ };
+
+ std::map<uint64_t, std::vector<TranslationOp>> sunRoofRotationOpsMap;
+ sunRoofRotationOpsMap.emplace(std::make_pair(sunRoofTranslationOp.vhalProperty,
+ std::vector<TranslationOp>{sunRoofTranslationOp}));
+
+ AnimationInfo sunRoofAnimation = AnimationInfo{
+ .partId = "sun_roof",
+ .parentId = "frame",
+ .pose = gMat4Identity,
+ .translationOpsMap = sunRoofRotationOpsMap,
+ };
+
+ return std::vector<AnimationInfo>{frameAnimation,
+ frontLeftDoorAnimation,
+ frontRightDoorAnimation,
+ frontLeftBlinkerAnimation,
+ frontRightBlinkerAnimation,
+ sunRoofAnimation};
+}
+
+TEST(AnimationModuleTests, EmptyVhalSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result =
+ animationModule.getUpdatedAnimationParams(std::vector<VehiclePropValue>());
+ EXPECT_EQ(result.size(), 0);
+}
+
+TEST(AnimationModuleTests, LeftDoorAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0200 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+}
+
+TEST(AnimationModuleTests, LeftDoorAnimationTenTimesSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ for (int i = 0; i < 10; ++i) {
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0200 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+ }
+}
+
+TEST(AnimationModuleTests, RightDoorAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0201 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+}
+
+TEST(AnimationModuleTests, RightDoorAnimationTenTimesSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ for (int i = 0; i < 10; ++i) {
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0201 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+ }
+}
+
+TEST(AnimationModuleTests, LeftBlinkerAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0300 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+}
+
+TEST(AnimationModuleTests, LeftBlinkerAnimationTenTimesSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ for (int i = 0; i < 10; ++i) {
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0300 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+ }
+}
+
+TEST(AnimationModuleTests, RightBlinkerAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0301 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+}
+
+TEST(AnimationModuleTests, RightBlinkerAnimationTenTimesSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ for (int i = 0; i < 10; ++i) {
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0301 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+ }
+}
+
+TEST(AnimationModuleTests, SunRoofAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0400 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+}
+
+TEST(AnimationModuleTests, SunRoofAnimationTenTimesSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ for (int i = 0; i < 10; ++i) {
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0400 | VehiclePropertyGroup::SYSTEM | VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values = std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 1);
+ }
+}
+
+TEST(AnimationModuleTests, All5PartsAnimationOnceSuccess) {
+ AnimationModule animationModule(getSampleCarPartsMap(), std::map<std::string, CarTexture>(),
+ getSampleAnimations());
+ std::vector<AnimationParam> result = animationModule.getUpdatedAnimationParams(
+ std::vector<VehiclePropValue>{VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0200 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values =
+ std::vector<int32_t>(1, INT32_MAX),
+ },
+ VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::DOOR,
+ .prop = 0x0201 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 |
+ VehicleArea::DOOR,
+ .value.int32Values =
+ std::vector<int32_t>(1, INT32_MAX),
+ },
+ VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0300 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values =
+ std::vector<int32_t>(1, INT32_MAX),
+ },
+ VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0301 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values =
+ std::vector<int32_t>(1, INT32_MAX),
+ },
+ VehiclePropValue{
+ .areaId = (int32_t)VehicleArea::GLOBAL,
+ .prop = 0x0400 | VehiclePropertyGroup::SYSTEM |
+ VehiclePropertyType::INT32 |
+ VehicleArea::GLOBAL,
+ .value.int32Values =
+ std::vector<int32_t>(1, INT32_MAX),
+ }});
+ EXPECT_EQ(result.size(), 5);
+}
+
+} // namespace
+} // namespace implementation
+} // namespace V1_0
+} // namespace sv
+} // namespace automotive
+} // namespace hardware
+} // namespace android
diff --git a/surround_view/service-impl/MathHelp.h b/surround_view/service-impl/MathHelp.h
new file mode 100644
index 0000000..fa20af6
--- /dev/null
+++ b/surround_view/service-impl/MathHelp.h
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2020 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.
+ */
+
+#ifndef SURROUND_VIEW_SERVICE_IMPL_MATH_HELP_H_
+#define SURROUND_VIEW_SERVICE_IMPL_MATH_HELP_H_
+
+#include "Matrix4x4.h"
+#include "core_lib.h"
+
+#include <android-base/logging.h>
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace sv {
+namespace V1_0 {
+namespace implementation {
+
+const int gMat4Size = 4 * 4 * sizeof(float);
+
+const Mat4x4 gMat4Identity = {1, 0, 0, /*tx=*/0.0, 0, 1, 0, /*ty=*/0,
+ 0, 0, 1, /*tz=*/0.0, 0, 0, 0, 1};
+
+inline float degToRad(float angleInDegrees) {
+ return 1.0f * angleInDegrees / 180 * M_PI;
+}
+
+typedef std::array<float, 3> VectorT;
+typedef std::array<float, 4> HomVectorT;
+typedef Matrix4x4<float> HomMatrixT;
+
+// Create a Translation matrix.
+inline HomMatrixT translationMatrix(const VectorT& v) {
+ HomMatrixT m = HomMatrixT::identity();
+ m.setRow(3, HomVectorT{v[0], v[1], v[2], 1});
+ return m;
+}
+
+// Create a Rotation matrix.
+inline HomMatrixT rotationMatrix(const VectorT& v, float angle, int orientation) {
+ const float c = cos(angle);
+ const float s = orientation * sin(angle);
+ const float t = 1 - c;
+ const float tx = t * v[0];
+ const float ty = t * v[1];
+ const float tz = t * v[2];
+ return HomMatrixT(tx * v[0] + c, tx * v[1] + s * v[2], tx * v[2] - s * v[1], 0,
+ tx * v[1] - s * v[2], ty * v[1] + c, ty * v[2] + s * v[0], 0,
+ tx * v[2] + s * v[1], ty * v[2] - s * v[0], tz * v[2] + c, 0, 0, 0, 0, 1);
+}
+
+inline Mat4x4 toMat4x4(const Matrix4x4F& matrix4x4F) {
+ Mat4x4 mat4x4;
+ memcpy(&mat4x4[0], matrix4x4F.transpose().data(), gMat4Size);
+ return mat4x4;
+}
+
+inline Matrix4x4F toMatrix4x4F(const Mat4x4& mat4x4) {
+ Matrix4x4F matrix4x4F;
+ memcpy(matrix4x4F.data(), &mat4x4[0], gMat4Size);
+
+ for (int i = 0; i < 4; i++) {
+ for (int j = 0; j < 4; j++) {
+ if (matrix4x4F(i, j) != mat4x4[i * 4 + j]) {
+ LOG(ERROR) << "Matrix error";
+ }
+ }
+ }
+ return matrix4x4F.transpose();
+}
+
+// Create a Rotation Matrix, around a unit vector by a ccw angle.
+inline Mat4x4 rotationMatrix(float angleInDegrees, const VectorT& axis) {
+ return toMat4x4(rotationMatrix(axis, degToRad(angleInDegrees), 1));
+}
+
+inline Mat4x4 appendRotation(float angleInDegrees, const VectorT& axis, const Mat4x4& mat4) {
+ return toMat4x4(toMatrix4x4F(mat4) * rotationMatrix(axis, degToRad(angleInDegrees), 1));
+}
+
+// Append mat_l * mat_r;
+inline Mat4x4 appendMat(const Mat4x4& matL, const Mat4x4& matR) {
+ return toMat4x4(toMatrix4x4F(matL) * toMatrix4x4F(matR));
+}
+
+// Rotate about a point about a unit vector.
+inline Mat4x4 rotationAboutPoint(float angleInDegrees, const VectorT& point, const VectorT& axis) {
+ VectorT pointInv = point;
+ pointInv[0] *= -1;
+ pointInv[1] *= -1;
+ pointInv[2] *= -1;
+ return toMat4x4(translationMatrix(pointInv) *
+ rotationMatrix(axis, degToRad(angleInDegrees), 1) * translationMatrix(point));
+}
+
+inline Mat4x4 translationMatrixToMat4x4(const VectorT& translation) {
+ return toMat4x4(translationMatrix(translation));
+}
+
+inline Mat4x4 appendTranslation(const VectorT& translation, const Mat4x4& mat4) {
+ return toMat4x4(toMatrix4x4F(mat4) * translationMatrix(translation));
+}
+
+inline Mat4x4 appendMatrix(const Mat4x4& deltaMatrix, const Mat4x4& currentMatrix) {
+ return toMat4x4(toMatrix4x4F(deltaMatrix) * toMatrix4x4F(currentMatrix));
+}
+
+} // namespace implementation
+} // namespace V1_0
+} // namespace sv
+} // namespace automotive
+} // namespace hardware
+} // namespace android
+
+#endif // SURROUND_VIEW_SERVICE_IMPL_MATH_HELP_H_
diff --git a/surround_view/service-impl/Matrix4x4.h b/surround_view/service-impl/Matrix4x4.h
new file mode 100644
index 0000000..8854e69
--- /dev/null
+++ b/surround_view/service-impl/Matrix4x4.h
@@ -0,0 +1,397 @@
+/*
+ * Copyright 2020 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.
+ */
+
+#ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_
+#define SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_
+
+#include <array>
+#include <cassert>
+#include <cmath>
+#include <iosfwd>
+
+template <class VType>
+class Matrix4x4 {
+private:
+ VType m[4][4];
+
+public:
+ typedef Matrix4x4<VType> Self;
+ typedef VType BaseType;
+ typedef std::array<VType, 4> MVector;
+
+ // Initialize the matrix to 0
+ Matrix4x4() {
+ m[0][3] = m[0][2] = m[0][1] = m[0][0] = VType();
+ m[1][3] = m[1][2] = m[1][1] = m[1][0] = VType();
+ m[2][3] = m[2][2] = m[2][1] = m[2][0] = VType();
+ m[3][3] = m[3][2] = m[3][1] = m[3][0] = VType();
+ }
+
+ // Explicitly set every element on construction
+ Matrix4x4(const VType& m00, const VType& m01, const VType& m02, const VType& m03,
+ const VType& m10, const VType& m11, const VType& m12, const VType& m13,
+ const VType& m20, const VType& m21, const VType& m22, const VType& m23,
+ const VType& m30, const VType& m31, const VType& m32, const VType& m33) {
+ m[0][0] = m00;
+ m[0][1] = m01;
+ m[0][2] = m02;
+ m[0][3] = m03;
+
+ m[1][0] = m10;
+ m[1][1] = m11;
+ m[1][2] = m12;
+ m[1][3] = m13;
+
+ m[2][0] = m20;
+ m[2][1] = m21;
+ m[2][2] = m22;
+ m[2][3] = m23;
+
+ m[3][0] = m30;
+ m[3][1] = m31;
+ m[3][2] = m32;
+ m[3][3] = m33;
+ }
+
+ // Casting constructor
+ template <class VType2>
+ static Matrix4x4 cast(const Matrix4x4<VType2>& mb) {
+ return Matrix4x4(static_cast<VType>(mb(0, 0)), static_cast<VType>(mb(0, 1)),
+ static_cast<VType>(mb(0, 2)), static_cast<VType>(mb(0, 3)),
+ static_cast<VType>(mb(1, 0)), static_cast<VType>(mb(1, 1)),
+ static_cast<VType>(mb(1, 2)), static_cast<VType>(mb(1, 3)),
+ static_cast<VType>(mb(2, 0)), static_cast<VType>(mb(2, 1)),
+ static_cast<VType>(mb(2, 2)), static_cast<VType>(mb(2, 3)),
+ static_cast<VType>(mb(3, 0)), static_cast<VType>(mb(3, 1)),
+ static_cast<VType>(mb(3, 2)), static_cast<VType>(mb(3, 3)));
+ }
+
+ // Change the value of all the coefficients of the matrix
+ inline Matrix4x4& set(const VType& m00, const VType& m01, const VType& m02, const VType& m03,
+ const VType& m10, const VType& m11, const VType& m12, const VType& m13,
+ const VType& m20, const VType& m21, const VType& m22, const VType& m23,
+ const VType& m30, const VType& m31, const VType& m32, const VType& m33) {
+ m[0][0] = m00;
+ m[0][1] = m01;
+ m[0][2] = m02;
+ m[0][3] = m03;
+
+ m[1][0] = m10;
+ m[1][1] = m11;
+ m[1][2] = m12;
+ m[1][3] = m13;
+
+ m[2][0] = m20;
+ m[2][1] = m21;
+ m[2][2] = m22;
+ m[2][3] = m23;
+
+ m[3][0] = m30;
+ m[3][1] = m31;
+ m[3][2] = m32;
+ m[3][3] = m33;
+ return (*this);
+ }
+
+ // Matrix addition
+ inline Matrix4x4& operator+=(const Matrix4x4& addFrom) {
+ m[0][0] += addFrom.m[0][0];
+ m[0][1] += addFrom.m[0][1];
+ m[0][2] += addFrom.m[0][2];
+ m[0][3] += addFrom.m[0][3];
+
+ m[1][0] += addFrom.m[1][0];
+ m[1][1] += addFrom.m[1][1];
+ m[1][2] += addFrom.m[1][2];
+ m[1][3] += addFrom.m[1][3];
+
+ m[2][0] += addFrom.m[2][0];
+ m[2][1] += addFrom.m[2][1];
+ m[2][2] += addFrom.m[2][2];
+ m[2][3] += addFrom.m[2][3];
+
+ m[3][0] += addFrom.m[3][0];
+ m[3][1] += addFrom.m[3][1];
+ m[3][2] += addFrom.m[3][2];
+ m[3][3] += addFrom.m[3][3];
+ return (*this);
+ }
+
+ // Matrix subtration
+ inline Matrix4x4& operator-=(const Matrix4x4& subFrom) {
+ m[0][0] -= subFrom.m[0][0];
+ m[0][1] -= subFrom.m[0][1];
+ m[0][2] -= subFrom.m[0][2];
+ m[0][3] -= subFrom.m[0][3];
+
+ m[1][0] -= subFrom.m[1][0];
+ m[1][1] -= subFrom.m[1][1];
+ m[1][2] -= subFrom.m[1][2];
+ m[1][3] -= subFrom.m[1][3];
+
+ m[2][0] -= subFrom.m[2][0];
+ m[2][1] -= subFrom.m[2][1];
+ m[2][2] -= subFrom.m[2][2];
+ m[2][3] -= subFrom.m[2][3];
+
+ m[3][0] -= subFrom.m[3][0];
+ m[3][1] -= subFrom.m[3][1];
+ m[3][2] -= subFrom.m[3][2];
+ m[3][3] -= subFrom.m[3][3];
+ return (*this);
+ }
+
+ // Matrix multiplication by a scalar
+ inline Matrix4x4& operator*=(const VType& k) {
+ m[0][0] *= k;
+ m[0][1] *= k;
+ m[0][2] *= k;
+ m[0][3] *= k;
+
+ m[1][0] *= k;
+ m[1][1] *= k;
+ m[1][2] *= k;
+ m[1][3] *= k;
+
+ m[2][0] *= k;
+ m[2][1] *= k;
+ m[2][2] *= k;
+ m[2][3] *= k;
+
+ m[3][0] *= k;
+ m[3][1] *= k;
+ m[3][2] *= k;
+ m[3][3] *= k;
+ return (*this);
+ }
+
+ // Matrix addition
+ inline Matrix4x4 operator+(const Matrix4x4& mb) const { return Matrix4x4(*this) += mb; }
+
+ // Matrix subtraction
+ inline Matrix4x4 operator-(const Matrix4x4& mb) const { return Matrix4x4(*this) -= mb; }
+
+ // Change the sign of all the coefficients in the matrix
+ friend inline Matrix4x4 operator-(const Matrix4x4& vb) {
+ return Matrix4x4(-vb.m[0][0], -vb.m[0][1], -vb.m[0][2], -vb.m[0][3], -vb.m[1][0],
+ -vb.m[1][1], -vb.m[1][2], -vb.m[1][3], -vb.m[2][0], -vb.m[2][1],
+ -vb.m[2][2], -vb.m[2][3], -vb.m[3][0], -vb.m[3][1], -vb.m[3][2],
+ -vb.m[3][3]);
+ }
+
+ // Matrix multiplication by a scalar
+ inline Matrix4x4 operator*(const VType& k) const { return Matrix4x4(*this) *= k; }
+
+ // Multiplication by a scaler
+ friend inline Matrix4x4 operator*(const VType& k, const Matrix4x4& mb) {
+ return Matrix4x4(mb) * k;
+ }
+
+ // Matrix multiplication
+ friend Matrix4x4 operator*(const Matrix4x4& a, const Matrix4x4& b) {
+ return Matrix4x4::fromCols(a * b.col(0), a * b.col(1), a * b.col(2), a * b.col(3));
+ }
+
+ // Multiplication of a matrix by a vector
+ friend MVector operator*(const Matrix4x4& a, const MVector& b) {
+ return MVector{dotProd(a.row(0), b), dotProd(a.row(1), b), dotProd(a.row(2), b),
+ dotProd(a.row(3), b)};
+ }
+
+ // Return the trace of the matrix
+ inline VType trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; }
+
+ // Return a pointer to the data array for interface with other libraries
+ // like opencv
+ VType* data() { return reinterpret_cast<VType*>(m); }
+ const VType* data() const { return reinterpret_cast<const VType*>(m); }
+
+ // Return matrix element (i,j) with 0<=i<=3 0<=j<=3
+ inline VType& operator()(const int i, const int j) {
+ assert(i >= 0);
+ assert(i < 4);
+ assert(j >= 0);
+ assert(j < 4);
+ return m[i][j];
+ }
+
+ inline VType operator()(const int i, const int j) const {
+ assert(i >= 0);
+ assert(i < 4);
+ assert(j >= 0);
+ assert(j < 4);
+ return m[i][j];
+ }
+
+ // Return matrix element (i/4,i%4) with 0<=i<=15 (access concatenated rows).
+ inline VType& operator[](const int i) {
+ assert(i >= 0);
+ assert(i < 16);
+ return reinterpret_cast<VType*>(m)[i];
+ }
+ inline VType operator[](const int i) const {
+ assert(i >= 0);
+ assert(i < 16);
+ return reinterpret_cast<const VType*>(m)[i];
+ }
+
+ // Return the transposed matrix
+ inline Matrix4x4 transpose() const {
+ return Matrix4x4(m[0][0], m[1][0], m[2][0], m[3][0], m[0][1], m[1][1], m[2][1], m[3][1],
+ m[0][2], m[1][2], m[2][2], m[3][2], m[0][3], m[1][3], m[2][3], m[3][3]);
+ }
+
+ // Returns the transpose of the matrix of the cofactors.
+ // (Useful for inversion for example.)
+ inline Matrix4x4 comatrixTransposed() const {
+ const auto cof = [this](unsigned int row, unsigned int col) {
+ unsigned int r0 = (row + 1) % 4;
+ unsigned int r1 = (row + 2) % 4;
+ unsigned int r2 = (row + 3) % 4;
+ unsigned int c0 = (col + 1) % 4;
+ unsigned int c1 = (col + 2) % 4;
+ unsigned int c2 = (col + 3) % 4;
+
+ VType minor = m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
+ m[r1][c0] * (m[r0][c1] * m[r2][c2] - m[r2][c1] * m[r0][c2]) +
+ m[r2][c0] * (m[r0][c1] * m[r1][c2] - m[r1][c1] * m[r0][c2]);
+ return (row + col) & 1 ? -minor : minor;
+ };
+
+ // Transpose
+ return Matrix4x4(cof(0, 0), cof(1, 0), cof(2, 0), cof(3, 0), cof(0, 1), cof(1, 1),
+ cof(2, 1), cof(3, 1), cof(0, 2), cof(1, 2), cof(2, 2), cof(3, 2),
+ cof(0, 3), cof(1, 3), cof(2, 3), cof(3, 3));
+ }
+
+ // Return dot production of two the vectors
+ static inline VType dotProd(const MVector& lhs, const MVector& rhs) {
+ return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3];
+ }
+
+ // Return the 4D vector at row i
+ inline MVector row(const int i) const {
+ assert(i >= 0);
+ assert(i < 4);
+ return MVector{m[i][0], m[i][1], m[i][2], m[i][3]};
+ }
+
+ // Return the 4D vector at col i
+ inline MVector col(const int i) const {
+ assert(i >= 0);
+ assert(i < 4);
+ return MVector{m[0][i], m[1][i], m[2][i], m[3][i]};
+ }
+
+ // Create a matrix from 4 row vectors
+ static inline Matrix4x4 fromRows(const MVector& v1, const MVector& v2, const MVector& v3,
+ const MVector& v4) {
+ return Matrix4x4(v1[0], v1[1], v1[2], v1[3], v2[0], v2[1], v2[2], v2[3], v3[0], v3[1],
+ v3[2], v3[3], v4[0], v4[1], v4[2], v4[3]);
+ }
+
+ // Create a matrix from 3 column vectors
+ static inline Matrix4x4 fromCols(const MVector& v1, const MVector& v2, const MVector& v3,
+ const MVector& v4) {
+ return Matrix4x4(v1[0], v2[0], v3[0], v4[0], v1[1], v2[1], v3[1], v4[1], v1[2], v2[2],
+ v3[2], v4[2], v1[3], v2[3], v3[3], v4[3]);
+ }
+
+ // Set the vector in row i to be v1
+ void setRow(int i, const MVector& v1) {
+ assert(i >= 0);
+ assert(i < 4);
+ m[i][0] = v1[0];
+ m[i][1] = v1[1];
+ m[i][2] = v1[2];
+ m[i][3] = v1[3];
+ }
+
+ // Set the vector in column i to be v1
+ void setCol(int i, const MVector& v1) {
+ assert(i >= 0);
+ assert(i < 4);
+ m[0][i] = v1[0];
+ m[1][i] = v1[1];
+ m[2][i] = v1[2];
+ m[3][i] = v1[3];
+ }
+
+ // Return the identity matrix
+ static inline Matrix4x4 identity() {
+ return Matrix4x4(VType(1), VType(), VType(), VType(), VType(), VType(1), VType(), VType(),
+ VType(), VType(), VType(1), VType(), VType(), VType(), VType(), VType(1));
+ }
+
+ // Return a matrix full of zeros
+ static inline Matrix4x4 zero() { return Matrix4x4(); }
+
+ // Return a diagonal matrix with the coefficients in v
+ static inline Matrix4x4 diagonal(const MVector& v) {
+ return Matrix4x4(v[0], VType(), VType(), VType(), //
+ VType(), v[1], VType(), VType(), //
+ VType(), VType(), v[2], VType(), //
+ VType(), VType(), VType(), v[3]);
+ }
+
+ // Return the matrix vvT
+ static Matrix4x4 sym4(const MVector& v) {
+ return Matrix4x4(v[0] * v[0], v[0] * v[1], v[0] * v[2], v[0] * v[3], v[1] * v[0],
+ v[1] * v[1], v[1] * v[2], v[1] * v[3], v[2] * v[0], v[2] * v[1],
+ v[2] * v[2], v[2] * v[3], v[3] * v[0], v[3] * v[1], v[3] * v[2],
+ v[3] * v[3]);
+ }
+
+ // Return the Frobenius norm of the matrix: sqrt(sum(aij^2))
+ VType frobeniusNorm() const {
+ VType sum = VType();
+ for (int i = 0; i < 4; i++) {
+ for (int j = 0; j < 4; j++) {
+ sum += m[i][j] * m[i][j];
+ }
+ }
+ return std::sqrt(sum);
+ }
+
+ // Return true is one of the elements of the matrix is NaN
+ bool isNaN() const {
+ for (int i = 0; i < 4; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ if (isnan(m[i][j])) {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ friend bool operator==(const Matrix4x4& a, const Matrix4x4& b) {
+ return a.m[0][0] == b.m[0][0] && a.m[0][1] == b.m[0][1] && a.m[0][2] == b.m[0][2] &&
+ a.m[0][3] == b.m[0][3] && a.m[1][0] == b.m[1][0] && a.m[1][1] == b.m[1][1] &&
+ a.m[1][2] == b.m[1][2] && a.m[1][3] == b.m[1][3] && a.m[2][0] == b.m[2][0] &&
+ a.m[2][1] == b.m[2][1] && a.m[2][2] == b.m[2][2] && a.m[2][3] == b.m[2][3] &&
+ a.m[3][0] == b.m[3][0] && a.m[3][1] == b.m[3][1] && a.m[3][2] == b.m[3][2] &&
+ a.m[3][3] == b.m[3][3];
+ }
+
+ friend bool operator!=(const Matrix4x4& a, const Matrix4x4& b) { return !(a == b); }
+};
+
+typedef Matrix4x4<int> Matrix4x4I;
+typedef Matrix4x4<float> Matrix4x4F;
+typedef Matrix4x4<double> Matrix4x4D;
+
+#endif // #ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_