DTrackSDK  v2.9.0
DTrackData.cpp
1 /* DTrackData: C++ source file
2  *
3  * DTrackSDK: data helper routines.
4  *
5  * Copyright 2020-2021, Advanced Realtime Tracking GmbH & Co. KG
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * 3. Neither the name of copyright holder nor the names of its contributors
16  * may be used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *
30  * Version v2.7.0
31  *
32  */
33 
34 #include <cmath>
35 
36 #include "DTrackDataTypes.hpp"
37 
38 namespace DTrackSDK_Datatypes {
39 
40 // -----------------------------------------------------------------------------------------------------
41 
42 /*
43  * Helper to convert a rotation matrix into a quaternion.
44  */
45 DTrackQuaternion rot2quat( const double rot[ 9 ] )
46 {
47  DTrackQuaternion quat;
48  double tr, s;
49 
50  tr = rot[ 0 ] + rot[ 4 ] + rot[ 8 ];
51 
52  if ( tr > 0 )
53  {
54  s = std::sqrt( 1.0 + tr );
55 
56  quat.w = 0.5 * s;
57 
58  s = 0.5 / s; // = 1 / (4 * w)
59 
60  quat.x = ( rot[ 5 ] - rot[ 7 ] ) * s ;
61  quat.y = ( rot[ 6 ] - rot[ 2 ] ) * s ;
62  quat.z = ( rot[ 1 ] - rot[ 3 ] ) * s ;
63  }
64  else
65  {
66  if ( ( rot[ 0 ] > rot[ 4 ] ) && ( rot[ 0 ] > rot[ 8 ] ) )
67  {
68  s = std::sqrt( 1.0 + rot[ 0 ] - rot[ 4 ] - rot[ 8 ] );
69 
70  quat.x = 0.5 * s;
71 
72  s = 0.5 / s; // = 1 / (4 * x)
73 
74  quat.y = ( rot[ 1 ] + rot[ 3 ] ) * s ;
75  quat.z = ( rot[ 2 ] + rot[ 6 ] ) * s ;
76  quat.w = ( rot[ 5 ] - rot[ 7 ] ) * s ;
77  }
78  else if ( rot[ 4 ] > rot[ 8 ] )
79  {
80  s = std::sqrt( 1.0 - rot[ 0 ] + rot[ 4 ] - rot[ 8 ] );
81 
82  quat.y = 0.5 * s;
83 
84  s = 0.5 / s; // = 1 / (4 * y)
85 
86  quat.x = ( rot[ 1 ] + rot[ 3 ] ) * s ;
87  quat.z = ( rot[ 5 ] + rot[ 7 ] ) * s ;
88  quat.w = ( rot[ 6 ] - rot[ 2 ] ) * s ;
89  }
90  else
91  {
92  s = std::sqrt( 1.0 - rot[ 0 ] - rot[ 4 ] + rot[ 8 ] );
93 
94  quat.z = 0.5 * s;
95 
96  s = 0.5 / s; // = 1 / (4 * z)
97 
98  quat.x = ( rot[ 2 ] + rot[ 6 ] ) * s ;
99  quat.y = ( rot[ 5 ] + rot[ 7 ] ) * s ;
100  quat.w = ( rot[ 1 ] - rot[ 3 ] ) * s ;
101  }
102  }
103 
104  return quat;
105 }
106 
107 
108 } // namespace DTrackSDK_Datatypes
109