src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.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 "../../../rotation_matrix.h"
  21
  22 int main()
  23 {
  24         float arr1[3][3] = {{20.2, 40.5, 10.0}, {3.6, 52.0, 5.5}, {1.0, 45.5, 66.6}};
  25         float arr2[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}};
  26         float arr3[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}};
  27
  28         matrix<float> m1(3, 3, (float *) arr1);
  29         matrix<float> m2(3, 3, (float *) arr2);
  30         matrix<float> m3(3, 3, (float *) arr3);
  31
  32         rotation_matrix<float> rm0, rm5;
  33         rotation_matrix<float> rm1(m1);
  34         rotation_matrix<float> rm2(m2);
  35         rotation_matrix<float> rm3(m3);
  36         rotation_matrix<float> rm4(3, 3, (float *) arr1);
  37
  38         quaternion<float> q0(-0.612372, 0.456436, 0.456436, 0.456436);
  39         quaternion<float> q1;
  40
  41         cout << "Constructor tests\n";
  42         cout << "input\n" << m1 << "\n";
  43         cout << "output\n" << rm1.m_rot_mat << "\n\n";
  44         cout << "input\n" << m2 << "\n";
  45         cout << "output\n" << rm2.m_rot_mat << "\n\n";
  46         cout << "input\n" << m3 << "\n";
  47         cout << "output\n" << rm3.m_rot_mat << "\n\n";
  48         cout << "input\n" << m1 << "\n";
  49         cout << "output\n" << rm4.m_rot_mat << "\n\n";
  50         cout << "default constructor\n";
  51         cout << "output\n" << rm0.m_rot_mat << "\n\n";
  52
  53         cout << "Quaternion to Rotation Matrix\n";
  54         cout << "input\n" << q0.m_quat << "\n";
  55         rm0 = quat2rot_mat(q0);
  56         cout << "output\n" << rm0.m_rot_mat << "\n\n";
  57
  58         cout << "Rotation Matrix to Quaternion\n";
  59         cout << "input\n" << rm0.m_rot_mat << "\n";
  60         q1 = rot_mat2quat(rm0);
  61         cout << "output\n" << q1.m_quat << "\n\n";
  62 }