DTrackSDK  v2.9.0
SampleMath.h
1 /* DTrackSDK in C++: SampleMath.h
2  *
3  * Minimum math classes; to be replaced by your favourite library.
4  *
5  * Copyright (c) 2022 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 
31 #include <iostream>
32 #include <cmath>
33 
34 #ifndef M_PI
35 #define M_PI 3.14159265358979323846
36 #endif
37 
38 class SampleRot;
39 
43 class SampleLoc
44 {
45 public:
46 
47  SampleLoc()
48  {}
49 
50  SampleLoc( const double loc[ 3 ] )
51  {
52  for ( int i = 0; i < 3; i++ )
53  m_loc[ i ] = loc[ i ];
54  }
55 
56  SampleLoc( double loc0, double loc1, double loc2 )
57  {
58  m_loc[ 0 ] = loc0;
59  m_loc[ 1 ] = loc1;
60  m_loc[ 2 ] = loc2;
61  }
62 
63  double operator[]( int ind ) const
64  {
65  return m_loc[ ind ];
66  }
67 
68  SampleLoc operator+( const SampleLoc& other ) const
69  {
70  SampleLoc tmploc;
71 
72  for ( int i = 0; i < 3; i++ )
73  tmploc.m_loc[ i ] = m_loc[ i ] + other.m_loc[ i ];
74 
75  return tmploc;
76  }
77 
78  friend SampleRot;
79 
80 private:
81 
82  double m_loc[ 3 ];
83 };
84 
85 
89 class SampleRot
90 {
91 public:
92 
93  SampleRot()
94  {}
95 
96  SampleRot( const double rot[ 9 ] )
97  {
98  for ( int i = 0; i < 9; i++ )
99  m_rot[ i ] = rot[ i ];
100  }
101 
102  double operator[]( int ind ) const
103  {
104  return m_rot[ ind ];
105  }
106 
107  SampleRot operator*( const SampleRot& other ) const
108  {
109  SampleRot tmprot;
110 
111  for ( int i = 0; i < 3; i++ )
112  {
113  for ( int j = 0; j < 3; j++ )
114  {
115  double t = 0.0;
116  for ( int k = 0; k < 3; k++ )
117  t += m_rot[ i + k * 3 ] * other.m_rot[ k + j * 3 ];
118 
119  tmprot.m_rot[ i + j * 3 ] = t;
120  }
121  }
122 
123  return tmprot;
124  }
125 
126  SampleLoc operator*( const SampleLoc& other ) const
127  {
128  SampleLoc tmploc;
129 
130  for ( int i = 0; i < 3; i++ )
131  {
132  double t = 0.0;
133  for ( int k = 0; k < 3; k++ )
134  t += m_rot[ i + k * 3 ] * other.m_loc[ k ];
135 
136  tmploc.m_loc[ i ] = t;
137  }
138 
139  return tmploc;
140  }
141 
142  static SampleRot rotationY( double ang ) // rotation around Y-axis
143  {
144  SampleRot tmprot;
145 
146  double cosang = cos( ang * M_PI / 180 );
147  double sinang = sin( ang * M_PI / 180 );
148 
149  tmprot.m_rot[ 0 + 0 * 3 ] = tmprot.m_rot[ 2 + 2 * 3 ] = cosang;
150  tmprot.m_rot[ 2 + 0 * 3 ] = -sinang;
151  tmprot.m_rot[ 0 + 2 * 3 ] = sinang;
152  tmprot.m_rot[ 1 + 1 * 3 ] = 1.0;
153  tmprot.m_rot[ 0 + 1 * 3 ] = tmprot.m_rot[ 1 + 0 * 3 ] = tmprot.m_rot[ 2 + 1 * 3 ] = tmprot.m_rot[ 1 + 2 * 3 ] = 0.0;
154 
155  return tmprot;
156  }
157 
158 private:
159 
160  double m_rot[ 9 ];
161 };
162 
163 
167 std::ostream& operator<<( std::ostream& os, const SampleLoc& loc )
168 {
169  os << "loc " << loc[ 0 ] << " " << loc[ 1 ] << " " << loc[ 2 ];
170 
171  return os;
172 }
173 
177 std::ostream& operator<<( std::ostream& os, const SampleRot& rot )
178 {
179  os << "rot " << rot[ 0 ] << " " << rot[ 1 ] << " " << rot[ 2 ] << " " << rot[ 3 ] << " " << rot[ 4 ] << " "
180  << rot[ 5 ] << " " << rot[ 6 ] << " " << rot[ 7 ] << " " << rot[ 8 ];
181 
182  return os;
183 }
184 
Minimum class for a rotation.
Definition: SampleMath.h:89
Minimum class for a position.
Definition: SampleMath.h:43