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