1 /*
2  * Copyright (C) 2023 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 
17 package com.google.uwb.support.fira;
18 
19 import android.os.PersistableBundle;
20 
21 import com.google.common.primitives.Doubles;
22 import com.google.common.primitives.Floats;
23 
24 /**
25  * Parameters passed to update the device pose information for the UWB distance and azimuth filter.
26  *
27  * <p>This is passed as a bundle to the service's session API
28  * {@link android.uwb.RangingSession#updatePose(PersistableBundle)}</p>
29  */
30 public class FiraPoseUpdateParams extends FiraParams {
31     private static final int BUNDLE_VERSION_1 = 1;
32     private static final int BUNDLE_VERSION_CURRENT = BUNDLE_VERSION_1;
33     private final double[] mPoseInfo;
34 
35     private static final String KEY_POSE_VQ = "pose_vq";
36 
37     @Override
getBundleVersion()38     protected int getBundleVersion() {
39         return BUNDLE_VERSION_CURRENT;
40     }
41 
FiraPoseUpdateParams(double[] poseInfo)42     private FiraPoseUpdateParams(double[] poseInfo) {
43         mPoseInfo = poseInfo;
44     }
45 
46     /** Converts the pose params to a bundle. */
47     @Override
toBundle()48     public PersistableBundle toBundle() {
49         PersistableBundle bundle = super.toBundle();
50         bundle.putDoubleArray(KEY_POSE_VQ, mPoseInfo);
51 
52         return bundle;
53     }
54 
55     /** Converts a PersistableBundle to pose params */
fromBundle(PersistableBundle bundle)56     public static FiraPoseUpdateParams fromBundle(PersistableBundle bundle) {
57         switch (getBundleVersion(bundle)) {
58             case BUNDLE_VERSION_1:
59                 return parseBundleVersion1(bundle);
60 
61             default:
62                 throw new IllegalArgumentException("unknown bundle version");
63         }
64     }
65 
parseBundleVersion1(PersistableBundle bundle)66     private static FiraPoseUpdateParams parseBundleVersion1(PersistableBundle bundle) {
67         return new Builder().setPose(bundle.getDoubleArray(KEY_POSE_VQ)).build();
68     }
69 
getPoseInfo()70     public double[] getPoseInfo() {
71         return mPoseInfo;
72     }
73 
74     /** Builder */
75     public static final class Builder {
76         public double[] mPoseInfo;
77 
78         /**
79          * Sets the pose. This must be either vX, vY, vZ, qX, qY, qZ, qW from a vector and
80          * quaternion OR a 16-element affine transformation matrix.
81          *
82          * @param poseInfo The 7 vector and quaternion values, or a 16-element matrix transform
83          */
setPose(float[] poseInfo)84         public FiraPoseUpdateParams.Builder setPose(float[] poseInfo) {
85             return setPose(Doubles.toArray(Floats.asList(poseInfo)));
86         }
87 
88         /**
89          * Sets the pose. This must be either vX, vY, vZ, qX, qY, qZ, qW from a vector and
90          * quaternion OR a 16-element affine transformation matrix.
91          *
92          * @param poseInfo The 7 vector and quaternion values, or a 16-element matrix transform
93          */
setPose(double[] poseInfo)94         public FiraPoseUpdateParams.Builder setPose(double[] poseInfo) {
95             if (poseInfo.length != 7 && poseInfo.length != 16) {
96                 throw new IllegalArgumentException("Pose must be 7 elements (vector3 xyz and"
97                         + " quaternion xyzw) or 16 elements (4x4 transformation matrix).");
98             }
99 
100             this.mPoseInfo = poseInfo;
101 
102             return this;
103         }
104 
105         /** Builds the {@link FiraPoseUpdateParams} */
build()106         public FiraPoseUpdateParams build() {
107             for (double k :
108                     mPoseInfo) {
109                 if (!Double.isFinite(k)) {
110                     throw new IllegalArgumentException("Cannot set pose; non-real numbers.");
111                 }
112             }
113 
114             return new FiraPoseUpdateParams(mPoseInfo);
115         }
116     }
117 }
118