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