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 "sensor_errors.h"
18 #include "sensor_utils.h"
19
20 #undef LOG_TAG
21 #define LOG_TAG "SensorAlgorithmAPI"
22 using namespace OHOS::Sensors;
23
CreateQuaternion(std::vector<float> rotationVector,std::vector<float> & quaternion)24 int32_t SensorAlgorithm::CreateQuaternion(std::vector<float> rotationVector, std::vector<float> &quaternion)
25 {
26 if (static_cast<int32_t>(rotationVector.size()) < ROTATION_VECTOR_LENGTH
27 || static_cast<int32_t>(rotationVector.size()) > QUATERNION_LENGTH) {
28 SEN_HILOGE("Invalid input rotationVector parameter");
29 return OHOS::Sensors::PARAMETER_ERROR;
30 }
31 if (static_cast<int32_t>(quaternion.size()) < QUATERNION_LENGTH) {
32 SEN_HILOGE("Invalid input quaternion parameter");
33 return OHOS::Sensors::PARAMETER_ERROR;
34 }
35 if (static_cast<int32_t>(rotationVector.size()) == ROTATION_VECTOR_LENGTH) {
36 quaternion[0] = 1 - static_cast<float>((pow(rotationVector[0], 2) + pow(rotationVector[1], 2)
37 + pow(rotationVector[2], 2)));
38 quaternion[0] = (quaternion[0] > 0) ? static_cast<float>(std::sqrt(quaternion[0])) : 0;
39 } else {
40 quaternion[0] = rotationVector[3];
41 }
42 quaternion[1] = rotationVector[0];
43 quaternion[2] = rotationVector[1];
44 quaternion[3] = rotationVector[2];
45 return OHOS::Sensors::SUCCESS;
46 }
47
TransformCoordinateSystemImpl(std::vector<float> inRotationMatrix,int32_t axisX,int32_t axisY,std::vector<float> & outRotationMatrix)48 int32_t SensorAlgorithm::TransformCoordinateSystemImpl(std::vector<float> inRotationMatrix, int32_t axisX,
49 int32_t axisY, std::vector<float> &outRotationMatrix)
50 {
51 if ((axisX & 0x7C) != 0 || (axisX & 0x3) == 0) {
52 SEN_HILOGE("axisX is invalid parameter");
53 return OHOS::Sensors::PARAMETER_ERROR;
54 }
55 if ((axisY & 0x7C) != 0 || (axisY & 0x3) == 0 || (axisX & 0x3) == (axisY & 0x3)) {
56 SEN_HILOGE("axisY is invalid parameter");
57 return OHOS::Sensors::PARAMETER_ERROR;
58 }
59 int32_t axisZ = axisX ^ axisY;
60 int32_t x = (axisX & 0x3) - 1;
61 int32_t y = (axisY & 0x3) - 1;
62 int32_t z = (axisZ & 0x3) - 1;
63 if (((x ^ ((z + 1) % 3)) | (y ^ ((z + 2) % 3))) != 0) {
64 axisZ ^= 0x80;
65 }
66 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
67 int32_t matrixDimension = ((inRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
68 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
69 for (int32_t j = 0; j < ROTATION_VECTOR_LENGTH; j++) {
70 int32_t offset = j * matrixDimension;
71 for (int32_t i = 0; i < 3; i++) {
72 if (x == i) {
73 outRotationMatrix[offset + i] = (axisX >= 0x80) ? -inRotationMatrix[offset + 0] :
74 inRotationMatrix[offset + 0];
75 }
76 if (y == i) {
77 outRotationMatrix[offset + i] = (axisY >= 0x80) ? -inRotationMatrix[offset + 1] :
78 inRotationMatrix[offset + 1];
79 }
80 if (z == i) {
81 outRotationMatrix[offset + i] = (axisZ >= 0x80) ? -inRotationMatrix[offset + 2] :
82 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 if (axisX < 0 || axisY < 0) {
98 SEN_HILOGE("Invalid axisX or axisY");
99 return OHOS::Sensors::PARAMETER_ERROR;
100 }
101 int32_t inRotationMatrixLength = static_cast<int32_t>(inRotationMatrix.size());
102 if (((inRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH) &&
103 (inRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)) ||
104 (inRotationMatrixLength != static_cast<int32_t>(outRotationMatrix.size()))) {
105 SEN_HILOGE("Invalid input parameter");
106 return OHOS::Sensors::PARAMETER_ERROR;
107 }
108 if (inRotationMatrix == outRotationMatrix) {
109 std::vector<float> tempRotationMatrix(inRotationMatrixLength);
110 if (TransformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, tempRotationMatrix)
111 != OHOS::Sensors::SUCCESS) {
112 SEN_HILOGE("TransformCoordinateSystemImpl failed");
113 return OHOS::Sensors::PARAMETER_ERROR;
114 }
115 for (int32_t i = 0; i < inRotationMatrixLength; i++) {
116 outRotationMatrix[i] = tempRotationMatrix[i];
117 }
118 return OHOS::Sensors::SUCCESS;
119 }
120 return TransformCoordinateSystemImpl(inRotationMatrix, axisX, axisY, outRotationMatrix);
121 }
122
GetAltitude(float seaPressure,float currentPressure,float * altitude)123 int32_t SensorAlgorithm::GetAltitude(float seaPressure, float currentPressure, float *altitude)
124 {
125 if (altitude == nullptr) {
126 SEN_HILOGE("Invalid parameter");
127 return OHOS::Sensors::PARAMETER_ERROR;
128 }
129 float coef = 1.0f / RECIPROCAL_COEFFICIENT;
130 float rationOfStandardPressure = IsEqual(seaPressure, 0.0f) ?
131 std::numeric_limits<float>::max() : currentPressure / seaPressure;
132 float difference = pow(rationOfStandardPressure, coef);
133 *altitude = ZERO_PRESSURE_ALTITUDE * (1.0f - difference);
134 return OHOS::Sensors::SUCCESS;
135 }
136
GetGeomagneticDip(std::vector<float> inclinationMatrix,float * geomagneticDip)137 int32_t SensorAlgorithm::GetGeomagneticDip(std::vector<float> inclinationMatrix, float *geomagneticDip)
138 {
139 if (geomagneticDip == nullptr) {
140 SEN_HILOGE("Invalid parameter");
141 return OHOS::Sensors::PARAMETER_ERROR;
142 }
143 int32_t matrixLength = static_cast<int32_t>(inclinationMatrix.size());
144 if (matrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH && matrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH) {
145 SEN_HILOGE("Invalid input parameter");
146 return OHOS::Sensors::PARAMETER_ERROR;
147 }
148 if (matrixLength == THREE_DIMENSIONAL_MATRIX_LENGTH) {
149 *geomagneticDip = std::atan2(inclinationMatrix[5], inclinationMatrix[4]);
150 } else {
151 *geomagneticDip = std::atan2(inclinationMatrix[6], inclinationMatrix[5]);
152 }
153 return OHOS::Sensors::SUCCESS;
154 }
155
GetAngleModify(std::vector<float> curRotationMatrix,std::vector<float> preRotationMatrix,std::vector<float> & angleChange)156 int32_t SensorAlgorithm::GetAngleModify(std::vector<float> curRotationMatrix, std::vector<float> preRotationMatrix,
157 std::vector<float> &angleChange)
158 {
159 if (static_cast<int32_t>(angleChange.size()) < ROTATION_VECTOR_LENGTH) {
160 SEN_HILOGE("Invalid parameter");
161 return OHOS::Sensors::PARAMETER_ERROR;
162 }
163 int32_t curRotationMatrixLength = static_cast<int32_t>(curRotationMatrix.size());
164 int32_t preRotationMatrixLength = static_cast<int32_t>(preRotationMatrix.size());
165 if ((curRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
166 && (curRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
167 SEN_HILOGE("Invalid input curRotationMatrix parameter");
168 return OHOS::Sensors::PARAMETER_ERROR;
169 }
170 if ((preRotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
171 && (preRotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
172 SEN_HILOGE("Invalid input currotationMatrix parameter");
173 return OHOS::Sensors::PARAMETER_ERROR;
174 }
175 float curMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
176 float preMatrix[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
177 int32_t curmatrixDimension = ((curRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
178 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
179 int32_t prematrixDimension = ((preRotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
180 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
181 for (int32_t i = 0; i < THREE_DIMENSIONAL_MATRIX_LENGTH; i++) {
182 int32_t curMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * curmatrixDimension;
183 curMatrix[i] = curRotationMatrix[curMatrixIndex];
184 int32_t preMatrixIndex = i % ROTATION_VECTOR_LENGTH + (i / ROTATION_VECTOR_LENGTH) * prematrixDimension;
185 preMatrix[i] = preRotationMatrix[preMatrixIndex];
186 }
187 float radian[THREE_DIMENSIONAL_MATRIX_LENGTH] = {0};
188 radian[1] = preMatrix[0] * curMatrix[1] + preMatrix[3] * curMatrix[4] + preMatrix[6] * curMatrix[7];
189 radian[4] = preMatrix[1] * curMatrix[1] + preMatrix[4] * curMatrix[4] + preMatrix[7] * curMatrix[7];
190 radian[6] = preMatrix[2] * curMatrix[0] + preMatrix[5] * curMatrix[3] + preMatrix[8] * curMatrix[6];
191 radian[7] = preMatrix[2] * curMatrix[1] + preMatrix[5] * curMatrix[4] + preMatrix[8] * curMatrix[7];
192 radian[8] = preMatrix[2] * curMatrix[2] + preMatrix[5] * curMatrix[5] + preMatrix[8] * curMatrix[8];
193 angleChange[0] = static_cast<float>(std::atan2(radian[1], radian[4]));
194 angleChange[1] = static_cast<float>(std::asin(-radian[7]));
195 angleChange[2] = static_cast<float>(std::atan2(-radian[6], radian[8]));
196 return OHOS::Sensors::SUCCESS;
197 }
198
GetDirection(std::vector<float> rotationMatrix,std::vector<float> & rotationAngle)199 int32_t SensorAlgorithm::GetDirection(std::vector<float> rotationMatrix, std::vector<float> &rotationAngle)
200 {
201 if (static_cast<int32_t>(rotationAngle.size()) < ROTATION_VECTOR_LENGTH) {
202 SEN_HILOGE("Invalid parameter");
203 return OHOS::Sensors::PARAMETER_ERROR;
204 }
205 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
206 if ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
207 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH)) {
208 SEN_HILOGE("Invalid input rotationMatrix parameter");
209 return OHOS::Sensors::PARAMETER_ERROR;
210 }
211 int32_t dimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
212 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
213 rotationAngle[0] = static_cast<float>(std::atan2(rotationMatrix[1],
214 rotationMatrix[dimension * 1 + 1]));
215 rotationAngle[1] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension + 1],
216 std::sqrt(pow(rotationMatrix[1], 2) + pow(rotationMatrix[dimension + 1], 2))));
217 rotationAngle[2] = static_cast<float>(std::atan2(-rotationMatrix[2 * dimension],
218 rotationMatrix[2 * dimension + 2]));
219 return OHOS::Sensors::SUCCESS;
220 }
221
CreateRotationMatrix(std::vector<float> rotationVector,std::vector<float> & rotationMatrix)222 int32_t SensorAlgorithm::CreateRotationMatrix(std::vector<float> rotationVector, std::vector<float> &rotationMatrix)
223 {
224 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
225 if ((static_cast<int32_t>(rotationVector.size()) < ROTATION_VECTOR_LENGTH)
226 || ((rotationMatrixLength != FOUR_DIMENSIONAL_MATRIX_LENGTH)
227 && (rotationMatrixLength != THREE_DIMENSIONAL_MATRIX_LENGTH))) {
228 SEN_HILOGE("Invalid input rotationMatrix parameter");
229 return OHOS::Sensors::PARAMETER_ERROR;
230 }
231 std::vector<float> quaternion(4);
232 int32_t ret = CreateQuaternion(rotationVector, quaternion);
233 if (ret != OHOS::Sensors::SUCCESS) {
234 SEN_HILOGE("Create quaternion failed");
235 return ret;
236 }
237 float squareOfX = 2 * static_cast<float>(pow(quaternion[1], 2));
238 float squareOfY = 2 * static_cast<float>(pow(quaternion[2], 2));
239 float squareOfZ = 2 * static_cast<float>(pow(quaternion[3], 2));
240 float productOfWZ = 2 * quaternion[0] * quaternion[3];
241 float productOfXY = 2 * quaternion[1] * quaternion[2];
242 float productOfWY = 2 * quaternion[0] * quaternion[2];
243 float productOfXZ = 2 * quaternion[1] * quaternion[3];
244 float productOfWX = 2 * quaternion[0] * quaternion[1];
245 float productOfYZ = 2 * quaternion[2] * quaternion[3];
246 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
247 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
248 rotationMatrix[0] = 1 - squareOfY - squareOfZ;
249 rotationMatrix[1] = productOfXY - productOfWZ;
250 rotationMatrix[2] = productOfXZ + productOfWY;
251 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
252 = productOfXY + productOfWZ;
253 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
254 = 1 - squareOfX - squareOfZ;
255 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
256 = productOfYZ - productOfWX;
257 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
258 = productOfXZ - productOfWY;
259 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
260 = productOfYZ + productOfWX;
261 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension]
262 = 1 - squareOfX - squareOfY;
263 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
264 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12] = rotationMatrix[13]
265 = rotationMatrix[14] = 0.0f;
266 rotationMatrix[15] = 1.0f;
267 }
268 return OHOS::Sensors::SUCCESS;
269 }
270
CreateRotationAndInclination(std::vector<float> gravity,std::vector<float> geomagnetic,std::vector<float> & rotationMatrix,std::vector<float> & inclinationMatrix)271 int32_t SensorAlgorithm::CreateRotationAndInclination(std::vector<float> gravity, std::vector<float> geomagnetic,
272 std::vector<float> &rotationMatrix, std::vector<float> &inclinationMatrix)
273 {
274 if (static_cast<int32_t>(gravity.size()) < ROTATION_VECTOR_LENGTH
275 || static_cast<int32_t>(geomagnetic.size()) < ROTATION_VECTOR_LENGTH) {
276 SEN_HILOGE("Invalid input parameter");
277 return OHOS::Sensors::PARAMETER_ERROR;
278 }
279 float totalGravity = pow(gravity[0], 2) + pow(gravity[1], 2) + pow(gravity[2], 2);
280 if (totalGravity < (0.01f * pow(GRAVITATIONAL_ACCELERATION, 2))) {
281 SEN_HILOGE("Invalid input gravity parameter");
282 return OHOS::Sensors::PARAMETER_ERROR;
283 }
284 std::vector<float> componentH(3);
285 componentH[0] = geomagnetic[1] * gravity[2] - geomagnetic[2] * gravity[1];
286 componentH[1] = geomagnetic[2] * gravity[0] - geomagnetic[0] * gravity[2];
287 componentH[2] = geomagnetic[0] * gravity[1] - geomagnetic[1] * gravity[0];
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 SEN_HILOGE("The total strength of H is less than 0.1");
292 return OHOS::Sensors::PARAMETER_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 int32_t rotationMatrixLength = static_cast<int32_t>(rotationMatrix.size());
308 int32_t inclinationMatrixLength = static_cast<int32_t>(inclinationMatrix.size());
309 if ((rotationMatrixLength != 9 && rotationMatrixLength != 16) || (inclinationMatrixLength != 9
310 && inclinationMatrixLength != 16)) {
311 SEN_HILOGE("Invalid input parameter");
312 return OHOS::Sensors::PARAMETER_ERROR;
313 }
314 float e = static_cast<float>(std::sqrt(pow(geomagnetic[0], 2) + pow(geomagnetic[1], 2) + pow(geomagnetic[2], 2)));
315 float reciprocalE = IsEqual(e, 0.0f) ? std::numeric_limits<float>::max() : 1.0f / e;
316 float c = (geomagnetic[0] * measuredValue[0] + geomagnetic[1] * measuredValue[1]
317 + geomagnetic[2] * measuredValue[2]) * reciprocalE;
318 float s = (geomagnetic[0] * gravity[0] + geomagnetic[1] * gravity[1] + geomagnetic[2] * gravity[2]) * reciprocalE;
319
320 int32_t rotationMatrixDimension = ((rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
321 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
322 int32_t inclinationMatrixDimension = ((inclinationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH)
323 ? QUATERNION_LENGTH : ROTATION_VECTOR_LENGTH);
324 rotationMatrix[0] = componentH[0];
325 rotationMatrix[1] = componentH[1];
326 rotationMatrix[2] = componentH[2];
327 rotationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] =
328 measuredValue[0];
329 rotationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] =
330 measuredValue[1];
331 rotationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] =
332 measuredValue[2];
333 rotationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[0];
334 rotationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[1];
335 rotationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * rotationMatrixDimension] = gravity[2];
336 if (rotationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
337 rotationMatrix[3] = rotationMatrix[7] = rotationMatrix[11] = rotationMatrix[12]
338 = rotationMatrix[13] = rotationMatrix[14] = 0.0f;
339 rotationMatrix[15] = 1.0f;
340 }
341 inclinationMatrix[0] = 1;
342 inclinationMatrix[1] = 0;
343 inclinationMatrix[2] = 0;
344 inclinationMatrix[3 % ROTATION_VECTOR_LENGTH + (3 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
345 inclinationMatrix[4 % ROTATION_VECTOR_LENGTH + (4 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
346 inclinationMatrix[5 % ROTATION_VECTOR_LENGTH + (5 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = s;
347 inclinationMatrix[6 % ROTATION_VECTOR_LENGTH + (6 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = 0;
348 inclinationMatrix[7 % ROTATION_VECTOR_LENGTH + (7 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = -s;
349 inclinationMatrix[8 % ROTATION_VECTOR_LENGTH + (8 / ROTATION_VECTOR_LENGTH) * inclinationMatrixDimension] = c;
350 if (inclinationMatrixLength == FOUR_DIMENSIONAL_MATRIX_LENGTH) {
351 inclinationMatrix[3] = inclinationMatrix[7] = inclinationMatrix[11] = inclinationMatrix[12]
352 = inclinationMatrix[13] = inclinationMatrix[14] = 0.0f;
353 inclinationMatrix[15] = 1.0f;
354 }
355 return OHOS::Sensors::SUCCESS;
356 }