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 <mutex>
19 #include "sensors_errors.h"
20 #include "sensors_log_domain.h"
21
22 using OHOS::HiviewDFX::HiLog;
23 using OHOS::HiviewDFX::HiLogLabel;
24
25 static constexpr HiLogLabel LABEL = {LOG_CORE, OHOS::SensorsLogDomain::SENSORS_INTERFACE, "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 HiLog::Error(LABEL, "%{public}s Invalid input rotationVector parameter", __func__);
32 return OHOS::Sensors::ERROR;
33 }
34 if (static_cast<int32_t>(quaternion.size()) < QUATERNION_LENGTH) {
35 HiLog::Error(LABEL, "%{public}s Invalid input quaternion parameter", __func__);
36 return OHOS::Sensors::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 HiLog::Error(LABEL, "%{public}s axisX is invalid parameter", __func__);
56 return OHOS::Sensors::ERROR;
57 }
58 if ((axisY & 0x7C) != 0 || (axisY & 0x3) == 0 || (axisX & 0x3) == (axisY & 0x3)) {
59 HiLog::Error(LABEL, "%{public}s axisY is invalid parameter", __func__);
60 return OHOS::Sensors::ERROR;
61 }
62 int32_t axisZ = axisX ^ axisY;
63
64 int32_t x = (axisX & 0x3) - 1;
65 int32_t y = (axisY & 0x3) - 1;
66 int32_t z = (axisZ & 0x3) - 1;
67 if (((x ^ ((z + 1) % 3)) | ( y ^ ((z + 2) % 3))) != 0) {
68 axisZ ^= 0x80;
69 }
70 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
71 int32_t matrixDimension = ((inRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
72 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
73 for (int32_t j = 0; j < ROTATION_VECTOR_LENGTH; j++) {
74 int32_t offset = j * matrixDimension;
75 for (int32_t i = 0; i < 3; i++) {
76 if (x == i) {
77 outRotationMatrix[offset + i] = (axisX >= 0x80) ? -inRotationMatrix[offset + 0] : inRotationMatrix[offset + 0];
78 }
79 if (y == i) {
80 outRotationMatrix[offset + i] = (axisY >= 0x80) ? -inRotationMatrix[offset + 1] : inRotationMatrix[offset + 1];
81 }
82 if (z == i) {
83 outRotationMatrix[offset + i] = (axisZ >= 0x80) ? -inRotationMatrix[offset + 2] : inRotationMatrix[offset + 2];
84 }
85 }
86 }
87
88 if (inRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
89 outRotationMatrix[3] = outRotationMatrix[7] = outRotationMatrix[11] =
90 outRotationMatrix[12] = outRotationMatrix[13] = outRotationMatrix[14] = 0;
91 outRotationMatrix[15] = 1;
92 }
93 return OHOS::Sensors::SUCCESS;
94 }
95
transformCoordinateSystem(std::vector<float> inRotationMatrix,int32_t axisX,int32_t axisY,std::vector<float> & outRotationMatrix)96 int32_t SensorAlgorithm::transformCoordinateSystem(std::vector<float> inRotationMatrix, int32_t axisX, int32_t axisY,
97 std::vector<float> &outRotationMatrix)
98 {
99 HiLog::Info(LABEL, "%{public}s inRotationMatrix start", __func__);
100 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
101 if (((inRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH) && (inRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH))
102 || (inRotationMatrixLength != static_cast<int32_t>(outRotationMatrix.size()))) {
103 HiLog::Error(LABEL, "%{public}s Invalid input parameter", __func__);
104 return OHOS::Sensors::ERROR;
105 }
106 if (inRotationMatrix == outRotationMatrix) {
107 std::vector<float> tempRotationMatrix(inRotationMatrixLength);
108 if (transformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, tempRotationMatrix) != OHOS::Sensors::SUCCESS) {
109 HiLog::Error(LABEL, "%{public}s transformCoordinateSystemImpl failed", __func__);
110 return OHOS::Sensors::ERROR;
111 }
112 for (int32_t i = 0; i < inRotationMatrixLength; i++) {
113 outRotationMatrix[i] = tempRotationMatrix[i];
114 }
115 return OHOS::Sensors::SUCCESS;
116 }
117 return transformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, outRotationMatrix);
118 }
119
getAltitude(float seaPressure,float currentPressure,float * altitude)120 int32_t SensorAlgorithm::getAltitude(float seaPressure, float currentPressure, float *altitude)
121 {
122 if (altitude == nullptr) {
123 HiLog::Error(LABEL, "%{public}s invalid parameter", __func__);
124 return OHOS::Sensors::ERROR;
125 }
126 float coef = 1.0f / RECIPROCAL_COEFFICIENT;
127 float rationOfStandardPressure = currentPressure / seaPressure;
128 float difference = pow(rationOfStandardPressure, coef);
129 *altitude = ZERO_PRESSURE_ALTITUDE * (1.0f - difference);
130 return OHOS::Sensors::SUCCESS;
131 }
132
getGeomagneticDip(std::vector<float> inclinationMatrix,float * geomagneticDip)133 int32_t SensorAlgorithm::getGeomagneticDip(std::vector<float> inclinationMatrix, float *geomagneticDip)
134 {
135 if (geomagneticDip == nullptr) {
136 HiLog::Error(LABEL, "%{public}s invalid parameter", __func__);
137 return OHOS::Sensors::ERROR;
138 }
139 int32_t matrixLength = static_cast<int32_t>(inclinationMatrix.size());
140 if (matrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH && matrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH) {
141 HiLog::Error(LABEL, "%{public}s Invalid input parameter", __func__);
142 return OHOS::Sensors::ERROR;
143 }
144 if (matrixLength == THREE_DIMENSIONAL_MATRIX_LENGTH) {
145 *geomagneticDip = std::atan2(inclinationMatrix[5], inclinationMatrix[4]);
146 } else {
147 *geomagneticDip = std::atan2(inclinationMatrix[6], inclinationMatrix[5]);
148 }
149 return OHOS::Sensors::SUCCESS;
150 }
151
getAngleModify(std::vector<float> curRotationMatrix,std::vector<float> preRotationMatrix,std::vector<float> & angleChange)152 int32_t SensorAlgorithm::getAngleModify(std::vector<float> curRotationMatrix, std::vector<float> preRotationMatrix,
153 std::vector<float> &angleChange)
154 {
155 if (static_cast<int32_t>(angleChange.size()) < ROTATION_VECTOR_LENGTH) {
156 HiLog::Error(LABEL, "%{public}s invalid parameter", __func__);
157 return OHOS::Sensors::ERROR;
158 }
159 int32_t curRotationMatrixLength = static_cast<int32_t>(curRotationMatrix.size());
160 int32_t preRotationMatrixLength = static_cast<int32_t>(preRotationMatrix.size());
161 if ((curRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
162 && (curRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
163 HiLog::Error(LABEL, "%{public}s Invalid input curRotationMatrix parameter", __func__);
164 return OHOS::Sensors::ERROR;
165 }
166 if ((preRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
167 && (preRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
168 HiLog::Error(LABEL, "%{public}s Invalid input currotationMatrix parameter", __func__);
169 return OHOS::Sensors::ERROR;
170 }
171
172 float curMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
173 float preMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
174 int32_t curmatrixDimension = ((curRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
175 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
176 int32_t prematrixDimension = ((preRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
177 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
178 for (int32_t i = 0; i < THREE_DIMENSIONAL_MATRIX_LENGTH; i++) {
179 int32_t curMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * curmatrixDimension;
180 curMatrix[i] = curRotationMatrix[curMatrixIndex];
181 int32_t preMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * prematrixDimension;
182 preMatrix[i] = preRotationMatrix[preMatrixIndex];
183 }
184
185 float radian[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
186 radian[1] = preMatrix[0] * curMatrix[1] + preMatrix[3] * curMatrix[4] + preMatrix[6] * curMatrix[7];
187 radian[4] = preMatrix[1] * curMatrix[1] + preMatrix[4] * curMatrix[4] + preMatrix[7] * curMatrix[7];
188 radian[6] = preMatrix[2] * curMatrix[0] + preMatrix[5] * curMatrix[3] + preMatrix[8] * curMatrix[6];
189 radian[7] = preMatrix[2] * curMatrix[1] + preMatrix[5] * curMatrix[4] + preMatrix[8] * curMatrix[7];
190 radian[8] = preMatrix[2] * curMatrix[2] + preMatrix[5] * curMatrix[5] + preMatrix[8] * curMatrix[8];
191 angleChange[0] = static_cast<float>(std::atan2(radian[1], radian[4]));
192 angleChange[1] = static_cast<float>(std::asin(-radian[7]));
193 angleChange[2] = static_cast<float>(std::atan2(-radian[6], radian[8]));
194 return OHOS::Sensors::SUCCESS;
195 }
196
getDirection(std::vector<float> rotationMatrix,std::vector<float> & rotationAngle)197 int32_t SensorAlgorithm::getDirection(std::vector<float> rotationMatrix, std::vector<float> &rotationAngle)
198 {
199 if (static_cast<int32_t>(rotationAngle.size()) < ROTATION_VECTOR_LENGTH) {
200 HiLog::Error(LABEL, "%{public}s invalid parameter", __func__);
201 return OHOS::Sensors::ERROR;
202 }
203 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
204 if ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
205 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
206 HiLog::Error(LABEL, "%{public}s Invalid input rotationMatrix parameter", __func__);
207 return OHOS::Sensors::ERROR;
208 }
209 int32_t dimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
210 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
211 rotationAngle[0] = static_cast<float>(std::atan2(rotationMatrix[1],
212 rotationMatrix[dimension * 1 + 1]));
213 rotationAngle[1] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension + 1],
214 std::sqrt(pow(rotationMatrix[1], 2) + pow(rotationMatrix[dimension + 1], 2))));
215 rotationAngle[2] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension],
216 rotationMatrix[2 * dimension + 2]));
217 return OHOS::Sensors::SUCCESS;
218 }
219
createRotationMatrix(std::vector<float> rotationVector,std::vector<float> & rotationMatrix)220 int32_t SensorAlgorithm::createRotationMatrix(std::vector<float> rotationVector, std::vector<float> &rotationMatrix)
221 {
222 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
223 if ((static_cast<int32_t>(rotationVector.size()) < ROTATION_VECTOR_LENGTH)
224 || ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
225 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH))) {
226 HiLog::Error(LABEL, "%{public}s Invalid input rotationMatrix parameter", __func__);
227 return OHOS::Sensors::ERROR;
228 }
229 std::vector<float> quaternion(4);
230 int32_t ret = createQuaternion(rotationVector, quaternion);
231 if (ret != OHOS::Sensors::SUCCESS) {
232 HiLog::Error(LABEL, "%{public}s create quaternion failed", __func__);
233 return OHOS::Sensors::ERROR;
234 }
235 float squareOfX = 2 * static_cast<float>(pow(quaternion[1], 2));
236 float squareOfY = 2 * static_cast<float>(pow(quaternion[2], 2));
237 float squareOfZ = 2 * static_cast<float>(pow(quaternion[3], 2));
238 float productOfWZ = 2 * quaternion[0] * quaternion[3];
239 float productOfXY = 2 * quaternion[1] * quaternion[2];
240 float productOfWY = 2 * quaternion[0] * quaternion[2];
241 float productOfXZ = 2 * quaternion[1] * quaternion[3];
242 float productOfWX = 2 * quaternion[0] * quaternion[1];
243 float productOfYZ = 2 * quaternion[2] * quaternion[3];
244 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
245 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
246 rotationMatrix[0] = 1 - squareOfY - squareOfZ;
247 rotationMatrix[1] = productOfXY - productOfWZ;
248 rotationMatrix[2] = productOfXZ + productOfWY;
249 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
250 = productOfXY + productOfWZ;
251 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
252 = 1 - squareOfX - squareOfZ;
253 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
254 = productOfYZ - productOfWX;
255 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
256 = productOfXZ - productOfWY;
257 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
258 = productOfYZ + productOfWX;
259 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
260 = 1 - squareOfX - squareOfY;
261 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
262 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12] = rotationMatrix[13]
263 = rotationMatrix[14] = 0.0f;
264 rotationMatrix[15] = 1.0f;
265 }
266 return OHOS::Sensors::SUCCESS;
267 }
268
createRotationAndInclination(std::vector<float> gravity,std::vector<float> geomagnetic,std::vector<float> & rotationMatrix,std::vector<float> & inclinationMatrix)269 int32_t SensorAlgorithm::createRotationAndInclination(std::vector<float> gravity, std::vector<float> geomagnetic,
270 std::vector<float> &rotationMatrix, std::vector<float> &inclinationMatrix)
271 {
272 if (static_cast<int32_t>(gravity.size()) < ROTATION_VECTOR_LENGTH
273 || static_cast<int32_t>(geomagnetic.size()) < ROTATION_VECTOR_LENGTH) {
274 HiLog::Error(LABEL, "%{public}s Invalid input parameter", __func__);
275 return OHOS::Sensors::ERROR;
276 }
277 float totalGravity = pow(gravity[0], 2) + pow(gravity[1], 2) + pow(gravity[2], 2);
278 if (totalGravity < (0.01f * pow(GRAVITATIONAL_ACCELERATION, 2))) {
279 HiLog::Error(LABEL, "%{public}s Invalid input gravity parameter", __func__);
280 return OHOS::Sensors::ERROR;
281 }
282
283 std::vector<float> componentH(3);
284 componentH[0] = geomagnetic[1] * gravity[2] - geomagnetic[2] * gravity[1];
285 componentH[1] = geomagnetic[2] * gravity[0] - geomagnetic[0] * gravity[2];
286 componentH[2] = geomagnetic[0] * gravity[1] - geomagnetic[1] * gravity[0];
287
288 float totalH = static_cast<float>(std::sqrt(pow(componentH[0], 2) + pow(componentH[1], 2)
289 + pow(componentH[2], 2)));
290 if (totalH < 0.1f) {
291 HiLog::Error(LABEL, "%{public}s The total strength of H is less than 0.1", __func__);
292 return OHOS::Sensors::ERROR;
293 }
294 float reciprocalH = 1.0f / totalH;
295 componentH[0] *= reciprocalH;
296 componentH[1] *= reciprocalH;
297 componentH[2] *= reciprocalH;
298 float reciprocalA = 1.0f / static_cast<float>(std::sqrt(totalGravity));
299 gravity[0] *= reciprocalA;
300 gravity[1] *= reciprocalA;
301 gravity[2] *= reciprocalA;
302
303 std::vector<float> measuredValue(3);
304 measuredValue[0] = gravity[1] * componentH[2] - gravity[2] * componentH[1];
305 measuredValue[1] = gravity[2] * componentH[0] - gravity[0] * componentH[2];
306 measuredValue[2] = gravity[0] * componentH[1] - gravity[1] * componentH[0];
307
308 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
309 int32_t inclinationMatrixLength = static_cast<int32_t>(inclinationMatrix.size());
310 if ((rotationMatrixLength != 9 && rotationMatrixLength != 16) || (inclinationMatrixLength != 9
311 && inclinationMatrixLength != 16)) {
312 HiLog::Error(LABEL, "%{public}s Invalid input parameter", __func__);
313 return OHOS::Sensors::ERROR;
314 }
315 float reciprocalE = 1.0f / static_cast<float>(std::sqrt(pow(geomagnetic[0], 2) + pow(geomagnetic[1], 2)
316 + pow(geomagnetic[2], 2)));
317 float c = (geomagnetic[0] * measuredValue[0] + geomagnetic[1] * measuredValue[1]
318 + geomagnetic[2] * measuredValue[2]) * reciprocalE;
319 float s = (geomagnetic[0] * gravity[0] + geomagnetic[1] * gravity[1] + geomagnetic[2] * gravity[2]) * reciprocalE;
320
321 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
322 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
323 int32_t inclinationMatrixDimension = ((inclinationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
324 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
325 rotationMatrix[0] = componentH[0];
326 rotationMatrix[1] = componentH[1];
327 rotationMatrix[2] = componentH[2];
328 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[0];
329 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[1];
330 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = measuredValue[2];
331 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[0];
332 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[1];
333 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[2];
334 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
335 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12]
336 = rotationMatrix[13] = rotationMatrix[14] = 0.0f;
337 rotationMatrix[15] = 1.0f;
338 }
339 inclinationMatrix[0] = 1;
340 inclinationMatrix[1] = 0;
341 inclinationMatrix[2] = 0;
342 inclinationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
343 inclinationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
344 inclinationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = s;
345 inclinationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
346 inclinationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = -s;
347 inclinationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
348 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
349 inclinationMatrix[3] = inclinationMatrix[7] = inclinationMatrix[11] = inclinationMatrix[12]
350 = inclinationMatrix[13] = inclinationMatrix[14] = 0.0f;
351 inclinationMatrix[15] = 1.0f;
352 }
353 return OHOS::Sensors::SUCCESS;
354 }