1 /*
2 * Copyright (c) 2021 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15 #include "sensor_algorithm.h"
16
17 #include <cmath>
18 #include <vector>
19
20 #include "sensors_errors.h"
21
22 using OHOS::HiviewDFX::HiLog;
23 using OHOS::HiviewDFX::HiLogLabel;
24
25 static constexpr HiLogLabel LABEL = {LOG_CORE, OHOS::Sensors::SENSOR_LOG_DOMAIN, "SensorAlgorithmAPI"};
26
CreateQuaternion(std::vector<float> rotationVector,std::vector<float> & quaternion)27 int32_t SensorAlgorithm::CreateQuaternion(std::vector<float> rotationVector, std::vector<float> &quaternion)
28 {
29 if (static_cast<int32_t>(rotationVector.size()) < ROTATION_VECTOR_LENGTH
30 || static_cast<int32_t>(rotationVector.size()) > QUATERNION_LENGTH) {
31 SEN_HILOGE("Invalid input rotationVector parameter");
32 return OHOS::Sensors::PARAMETER_ERROR;
33 }
34 if (static_cast<int32_t>(quaternion.size()) < QUATERNION_LENGTH) {
35 SEN_HILOGE("Invalid input quaternion parameter");
36 return OHOS::Sensors::PARAMETER_ERROR;
37 }
38 if (static_cast<int32_t>(rotationVector.size()) == ROTATION_VECTOR_LENGTH) {
39 quaternion[0] = 1 - static_cast<float>((pow(rotationVector[0], 2) + pow(rotationVector[1], 2)
40 + pow(rotationVector[2], 2)));
41 quaternion[0] = (quaternion[0] > 0) ? static_cast<float>(std::sqrt(quaternion[0])) : 0;
42 } else {
43 quaternion[0] = rotationVector[3];
44 }
45 quaternion[1] = rotationVector[0];
46 quaternion[2] = rotationVector[1];
47 quaternion[3] = rotationVector[2];
48 return OHOS::Sensors::SUCCESS;
49 }
50
TransformCoordinateSystemImpl(std::vector<float> inRotationMatrix,int32_t axisX,int32_t axisY,std::vector<float> & outRotationMatrix)51 int32_t SensorAlgorithm::TransformCoordinateSystemImpl(std::vector<float> inRotationMatrix, int32_t axisX,
52 int32_t axisY, std::vector<float> &outRotationMatrix)
53 {
54 if ((axisX & 0x7C) != 0 || (axisX & 0x3) == 0) {
55 SEN_HILOGE("axisX is invalid parameter");
56 return OHOS::Sensors::PARAMETER_ERROR;
57 }
58 if ((axisY & 0x7C) != 0 || (axisY & 0x3) == 0 || (axisX & 0x3) == (axisY & 0x3)) {
59 SEN_HILOGE("axisY is invalid parameter");
60 return OHOS::Sensors::PARAMETER_ERROR;
61 }
62 int32_t axisZ = axisX ^ axisY;
63 int32_t x = (axisX & 0x3) - 1;
64 int32_t y = (axisY & 0x3) - 1;
65 int32_t z = (axisZ & 0x3) - 1;
66 if (((x ^ ((z + 1) % 3)) | ( y ^ ((z + 2) % 3))) != 0) {
67 axisZ ^= 0x80;
68 }
69 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
70 int32_t matrixDimension = ((inRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
71 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
72 for (int32_t j = 0; j < ROTATION_VECTOR_LENGTH; j++) {
73 int32_t offset = j * matrixDimension;
74 for (int32_t i = 0; i < 3; i++) {
75 if (x == i) {
76 outRotationMatrix[offset + i] = (axisX >= 0x80) ? -inRotationMatrix[offset + 0] : inRotationMatrix[offset + 0];
77 }
78 if (y == i) {
79 outRotationMatrix[offset + i] = (axisY >= 0x80) ? -inRotationMatrix[offset + 1] : inRotationMatrix[offset + 1];
80 }
81 if (z == i) {
82 outRotationMatrix[offset + i] = (axisZ >= 0x80) ? -inRotationMatrix[offset + 2] : inRotationMatrix[offset + 2];
83 }
84 }
85 }
86 if (inRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
87 outRotationMatrix[3] = outRotationMatrix[7] = outRotationMatrix[11] =
88 outRotationMatrix[12] = outRotationMatrix[13] = outRotationMatrix[14] = 0;
89 outRotationMatrix[15] = 1;
90 }
91 return OHOS::Sensors::SUCCESS;
92 }
93
TransformCoordinateSystem(std::vector<float> inRotationMatrix,int32_t axisX,int32_t axisY,std::vector<float> & outRotationMatrix)94 int32_t SensorAlgorithm::TransformCoordinateSystem(std::vector<float> inRotationMatrix, int32_t axisX, int32_t axisY,
95 std::vector<float> &outRotationMatrix)
96 {
97 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
98 if (((inRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH) && (inRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH))
99 || (inRotationMatrixLength != static_cast<int32_t>(outRotationMatrix.size()))) {
100 SEN_HILOGE("Invalid input parameter");
101 return OHOS::Sensors::PARAMETER_ERROR;
102 }
103 if (inRotationMatrix == outRotationMatrix) {
104 std::vector<float> tempRotationMatrix(inRotationMatrixLength);
105 if (TransformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, tempRotationMatrix) != OHOS::Sensors::SUCCESS) {
106 SEN_HILOGE("TransformCoordinateSystemImpl failed");
107 return OHOS::Sensors::PARAMETER_ERROR;
108 }
109 for (int32_t i = 0; i < inRotationMatrixLength; i++) {
110 outRotationMatrix[i] = tempRotationMatrix[i];
111 }
112 return OHOS::Sensors::SUCCESS;
113 }
114 return TransformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, outRotationMatrix);
115 }
116
GetAltitude(float seaPressure,float currentPressure,float * altitude)117 int32_t SensorAlgorithm::GetAltitude(float seaPressure, float currentPressure, float *altitude)
118 {
119 if (altitude == nullptr) {
120 SEN_HILOGE("invalid parameter");
121 return OHOS::Sensors::PARAMETER_ERROR;
122 }
123 float coef = 1.0f / RECIPROCAL_COEFFICIENT;
124 float rationOfStandardPressure = currentPressure / seaPressure;
125 float difference = pow(rationOfStandardPressure, coef);
126 *altitude = ZERO_PRESSURE_ALTITUDE * (1.0f - difference);
127 return OHOS::Sensors::SUCCESS;
128 }
129
GetGeomagneticDip(std::vector<float> inclinationMatrix,float * geomagneticDip)130 int32_t SensorAlgorithm::GetGeomagneticDip(std::vector<float> inclinationMatrix, float *geomagneticDip)
131 {
132 if (geomagneticDip == nullptr) {
133 SEN_HILOGE("invalid parameter");
134 return OHOS::Sensors::PARAMETER_ERROR;
135 }
136 int32_t matrixLength = static_cast<int32_t>(inclinationMatrix.size());
137 if (matrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH && matrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH) {
138 SEN_HILOGE("Invalid input parameter");
139 return OHOS::Sensors::PARAMETER_ERROR;
140 }
141 if (matrixLength == THREE_DIMENSIONAL_MATRIX_LENGTH) {
142 *geomagneticDip = std::atan2(inclinationMatrix[5], inclinationMatrix[4]);
143 } else {
144 *geomagneticDip = std::atan2(inclinationMatrix[6], inclinationMatrix[5]);
145 }
146 return OHOS::Sensors::SUCCESS;
147 }
148
GetAngleModify(std::vector<float> curRotationMatrix,std::vector<float> preRotationMatrix,std::vector<float> & angleChange)149 int32_t SensorAlgorithm::GetAngleModify(std::vector<float> curRotationMatrix, std::vector<float> preRotationMatrix,
150 std::vector<float> &angleChange)
151 {
152 if (static_cast<int32_t>(angleChange.size()) < ROTATION_VECTOR_LENGTH) {
153 SEN_HILOGE("invalid parameter");
154 return OHOS::Sensors::PARAMETER_ERROR;
155 }
156 int32_t curRotationMatrixLength = static_cast<int32_t>(curRotationMatrix.size());
157 int32_t preRotationMatrixLength = static_cast<int32_t>(preRotationMatrix.size());
158 if ((curRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
159 && (curRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
160 SEN_HILOGE("Invalid input curRotationMatrix parameter");
161 return OHOS::Sensors::PARAMETER_ERROR;
162 }
163 if ((preRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
164 && (preRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
165 SEN_HILOGE("Invalid input currotationMatrix parameter");
166 return OHOS::Sensors::PARAMETER_ERROR;
167 }
168 float curMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
169 float preMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
170 int32_t curmatrixDimension = ((curRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
171 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
172 int32_t prematrixDimension = ((preRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
173 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
174 for (int32_t i = 0; i < THREE_DIMENSIONAL_MATRIX_LENGTH; i++) {
175 int32_t curMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * curmatrixDimension;
176 curMatrix[i] = curRotationMatrix[curMatrixIndex];
177 int32_t preMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * prematrixDimension;
178 preMatrix[i] = preRotationMatrix[preMatrixIndex];
179 }
180 float radian[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
181 radian[1] = preMatrix[0] * curMatrix[1] + preMatrix[3] * curMatrix[4] + preMatrix[6] * curMatrix[7];
182 radian[4] = preMatrix[1] * curMatrix[1] + preMatrix[4] * curMatrix[4] + preMatrix[7] * curMatrix[7];
183 radian[6] = preMatrix[2] * curMatrix[0] + preMatrix[5] * curMatrix[3] + preMatrix[8] * curMatrix[6];
184 radian[7] = preMatrix[2] * curMatrix[1] + preMatrix[5] * curMatrix[4] + preMatrix[8] * curMatrix[7];
185 radian[8] = preMatrix[2] * curMatrix[2] + preMatrix[5] * curMatrix[5] + preMatrix[8] * curMatrix[8];
186 angleChange[0] = static_cast<float>(std::atan2(radian[1], radian[4]));
187 angleChange[1] = static_cast<float>(std::asin(-radian[7]));
188 angleChange[2] = static_cast<float>(std::atan2(-radian[6], radian[8]));
189 return OHOS::Sensors::SUCCESS;
190 }
191
GetDirection(std::vector<float> rotationMatrix,std::vector<float> & rotationAngle)192 int32_t SensorAlgorithm::GetDirection(std::vector<float> rotationMatrix, std::vector<float> &rotationAngle)
193 {
194 if (static_cast<int32_t>(rotationAngle.size()) < ROTATION_VECTOR_LENGTH) {
195 SEN_HILOGE("invalid parameter");
196 return OHOS::Sensors::PARAMETER_ERROR;
197 }
198 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
199 if ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
200 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
201 SEN_HILOGE("Invalid input rotationMatrix parameter");
202 return OHOS::Sensors::PARAMETER_ERROR;
203 }
204 int32_t dimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
205 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
206 rotationAngle[0] = static_cast<float>(std::atan2(rotationMatrix[1],
207 rotationMatrix[dimension * 1 + 1]));
208 rotationAngle[1] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension + 1],
209 std::sqrt(pow(rotationMatrix[1], 2) + pow(rotationMatrix[dimension + 1], 2))));
210 rotationAngle[2] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension],
211 rotationMatrix[2 * dimension + 2]));
212 return OHOS::Sensors::SUCCESS;
213 }
214
CreateRotationMatrix(std::vector<float> rotationVector,std::vector<float> & rotationMatrix)215 int32_t SensorAlgorithm::CreateRotationMatrix(std::vector<float> rotationVector, std::vector<float> &rotationMatrix)
216 {
217 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
218 if ((static_cast<int32_t>(rotationVector.size()) < ROTATION_VECTOR_LENGTH)
219 || ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
220 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH))) {
221 SEN_HILOGE("Invalid input rotationMatrix parameter");
222 return OHOS::Sensors::PARAMETER_ERROR;
223 }
224 std::vector<float> quaternion(4);
225 int32_t ret = CreateQuaternion(rotationVector, quaternion);
226 if (ret != OHOS::Sensors::SUCCESS) {
227 SEN_HILOGE("create quaternion failed");
228 return ret;
229 }
230 float squareOfX = 2 * static_cast<float>(pow(quaternion[1], 2));
231 float squareOfY = 2 * static_cast<float>(pow(quaternion[2], 2));
232 float squareOfZ = 2 * static_cast<float>(pow(quaternion[3], 2));
233 float productOfWZ = 2 * quaternion[0] * quaternion[3];
234 float productOfXY = 2 * quaternion[1] * quaternion[2];
235 float productOfWY = 2 * quaternion[0] * quaternion[2];
236 float productOfXZ = 2 * quaternion[1] * quaternion[3];
237 float productOfWX = 2 * quaternion[0] * quaternion[1];
238 float productOfYZ = 2 * quaternion[2] * quaternion[3];
239 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
240 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
241 rotationMatrix[0] = 1 - squareOfY - squareOfZ;
242 rotationMatrix[1] = productOfXY - productOfWZ;
243 rotationMatrix[2] = productOfXZ + productOfWY;
244 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
245 = productOfXY + productOfWZ;
246 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
247 = 1 - squareOfX - squareOfZ;
248 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
249 = productOfYZ - productOfWX;
250 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
251 = productOfXZ - productOfWY;
252 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
253 = productOfYZ + productOfWX;
254 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
255 = 1 - squareOfX - squareOfY;
256 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
257 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12] = rotationMatrix[13]
258 = rotationMatrix[14] = 0.0f;
259 rotationMatrix[15] = 1.0f;
260 }
261 return OHOS::Sensors::SUCCESS;
262 }
263
CreateRotationAndInclination(std::vector<float> gravity,std::vector<float> geomagnetic,std::vector<float> & rotationMatrix,std::vector<float> & inclinationMatrix)264 int32_t SensorAlgorithm::CreateRotationAndInclination(std::vector<float> gravity, std::vector<float> geomagnetic,
265 std::vector<float> &rotationMatrix, std::vector<float> &inclinationMatrix)
266 {
267 if (static_cast<int32_t>(gravity.size()) < ROTATION_VECTOR_LENGTH
268 || static_cast<int32_t>(geomagnetic.size()) < ROTATION_VECTOR_LENGTH) {
269 SEN_HILOGE("Invalid input parameter");
270 return OHOS::Sensors::PARAMETER_ERROR;
271 }
272 float totalGravity = pow(gravity[0], 2) + pow(gravity[1], 2) + pow(gravity[2], 2);
273 if (totalGravity < (0.01f * pow(GRAVITATIONAL_ACCELERATION, 2))) {
274 SEN_HILOGE("Invalid input gravity parameter");
275 return OHOS::Sensors::PARAMETER_ERROR;
276 }
277 std::vector<float> componentH(3);
278 componentH[0] = geomagnetic[1] * gravity[2] - geomagnetic[2] * gravity[1];
279 componentH[1] = geomagnetic[2] * gravity[0] - geomagnetic[0] * gravity[2];
280 componentH[2] = geomagnetic[0] * gravity[1] - geomagnetic[1] * gravity[0];
281 float totalH = static_cast<float>(std::sqrt(pow(componentH[0], 2) + pow(componentH[1], 2)
282 + pow(componentH[2], 2)));
283 if (totalH < 0.1f) {
284 SEN_HILOGE("The total strength of H is less than 0.1");
285 return OHOS::Sensors::PARAMETER_ERROR;
286 }
287 float reciprocalH = 1.0f / totalH;
288 componentH[0] *= reciprocalH;
289 componentH[1] *= reciprocalH;
290 componentH[2] *= reciprocalH;
291 float reciprocalA = 1.0f / static_cast<float>(std::sqrt(totalGravity));
292 gravity[0] *= reciprocalA;
293 gravity[1] *= reciprocalA;
294 gravity[2] *= reciprocalA;
295
296 std::vector<float> measuredValue(3);
297 measuredValue[0] = gravity[1] * componentH[2] - gravity[2] * componentH[1];
298 measuredValue[1] = gravity[2] * componentH[0] - gravity[0] * componentH[2];
299 measuredValue[2] = gravity[0] * componentH[1] - gravity[1] * componentH[0];
300 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
301 int32_t inclinationMatrixLength = static_cast<int32_t>(inclinationMatrix.size());
302 if ((rotationMatrixLength != 9 && rotationMatrixLength != 16) || (inclinationMatrixLength != 9
303 && inclinationMatrixLength != 16)) {
304 SEN_HILOGE("Invalid input parameter");
305 return OHOS::Sensors::PARAMETER_ERROR;
306 }
307 float reciprocalE = 1.0f / static_cast<float>(std::sqrt(pow(geomagnetic[0], 2) + pow(geomagnetic[1], 2)
308 + pow(geomagnetic[2], 2)));
309 float c = (geomagnetic[0] * measuredValue[0] + geomagnetic[1] * measuredValue[1]
310 + geomagnetic[2] * measuredValue[2]) * reciprocalE;
311 float s = (geomagnetic[0] * gravity[0] + geomagnetic[1] * gravity[1] + geomagnetic[2] * gravity[2]) * reciprocalE;
312
313 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
314 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
315 int32_t inclinationMatrixDimension = ((inclinationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
316 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
317 rotationMatrix[0] = componentH[0];
318 rotationMatrix[1] = componentH[1];
319 rotationMatrix[2] = componentH[2];
320 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[0];
321 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[1];
322 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[2];
323 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[0];
324 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[1];
325 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[2];
326 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
327 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12]
328 = rotationMatrix[13] = rotationMatrix[14] = 0.0f;
329 rotationMatrix[15] = 1.0f;
330 }
331 inclinationMatrix[0] = 1;
332 inclinationMatrix[1] = 0;
333 inclinationMatrix[2] = 0;
334 inclinationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
335 inclinationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
336 inclinationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = s;
337 inclinationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
338 inclinationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = -s;
339 inclinationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
340 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
341 inclinationMatrix[3] = inclinationMatrix[7] = inclinationMatrix[11] = inclinationMatrix[12]
342 = inclinationMatrix[13] = inclinationMatrix[14] = 0.0f;
343 inclinationMatrix[15] = 1.0f;
344 }
345 return OHOS::Sensors::SUCCESS;
346 }