src/sensor/fusion_util.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #include <fusion_util.h>
  21 #include <math.h>
  22 #include <stdlib.h>
  23
  24 int quat_to_matrix(const float *quat, float *R)
  25 {
  26         if(quat == NULL || R == NULL)
  27                 return -1;
  28
  29         float q0 = quat[3];
  30         float q1 = quat[0];
  31         float q2 = quat[1];
  32         float q3 = quat[2];
  33
  34         float sq_q1 = 2 * q1 * q1;
  35         float sq_q2 = 2 * q2 * q2;
  36         float sq_q3 = 2 * q3 * q3;
  37         float q1_q2 = 2 * q1 * q2;
  38         float q3_q0 = 2 * q3 * q0;
  39         float q1_q3 = 2 * q1 * q3;
  40         float q2_q0 = 2 * q2 * q0;
  41         float q2_q3 = 2 * q2 * q3;
  42         float q1_q0 = 2 * q1 * q0;
  43
  44         R[0] = 1 - sq_q2 - sq_q3;
  45         R[1] = q1_q2 - q3_q0;
  46         R[2] = q1_q3 + q2_q0;
  47         R[3] = q1_q2 + q3_q0;
  48         R[4] = 1 - sq_q1 - sq_q3;
  49         R[5] = q2_q3 - q1_q0;
  50         R[6] = q1_q3 - q2_q0;
  51         R[7] = q2_q3 + q1_q0;
  52         R[8] = 1 - sq_q1 - sq_q2;
  53
  54         return 0;
  55 }