xsquaternion.h
Go to the documentation of this file.
1 
2 // Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without modification,
6 // are permitted provided that the following conditions are met:
7 //
8 // 1. Redistributions of source code must retain the above copyright notice,
9 // this list of conditions, and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright notice,
12 // this list of conditions, and the following disclaimer in the documentation
13 // and/or other materials provided with the distribution.
14 //
15 // 3. Neither the names of the copyright holders nor the names of their contributors
16 // may be used to endorse or promote products derived from this software without
17 // specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
20 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
22 // THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
24 // OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
26 // TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
27 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS
28 // SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES
29 // OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE
30 // ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
31 //
32 
33 
34 // Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
35 // All rights reserved.
36 //
37 // Redistribution and use in source and binary forms, with or without modification,
38 // are permitted provided that the following conditions are met:
39 //
40 // 1. Redistributions of source code must retain the above copyright notice,
41 // this list of conditions, and the following disclaimer.
42 //
43 // 2. Redistributions in binary form must reproduce the above copyright notice,
44 // this list of conditions, and the following disclaimer in the documentation
45 // and/or other materials provided with the distribution.
46 //
47 // 3. Neither the names of the copyright holders nor the names of their contributors
48 // may be used to endorse or promote products derived from this software without
49 // specific prior written permission.
50 //
51 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
52 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
53 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
54 // THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
55 // SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
56 // OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
57 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
58 // TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
59 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS
60 // SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES
61 // OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE
62 // ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
63 //
64 
65 #ifndef XSQUATERNION_H
66 #define XSQUATERNION_H
67 
68 #include "xsmath.h"
69 
70 struct XsEuler;
71 struct XsMatrix;
72 struct XsVector;
73 struct XsQuaternion;
74 
75 #ifdef __cplusplus
76 extern "C" {
77 #endif
78 #ifndef __cplusplus
79 #define XSQUATERNION_INITIALIZER { { { XsMath_zero, XsMath_zero, XsMath_zero, XsMath_zero } } }
80 typedef struct XsQuaternion XsQuaternion;
81 #endif
82 
83 XSTYPES_DLL_API void XsQuaternion_destruct(XsQuaternion* thisPtr);
84 XSTYPES_DLL_API int XsQuaternion_empty(const XsQuaternion* thisPtr);
85 XSTYPES_DLL_API void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest);
86 XSTYPES_DLL_API XsReal XsQuaternion_normalized(const XsQuaternion* thisPtr, XsQuaternion* dest);
87 XSTYPES_DLL_API XsReal XsQuaternion_normalize(XsQuaternion* thisPtr);
88 XSTYPES_DLL_API void XsQuaternion_fromEulerAngles(XsQuaternion* thisPtr, const struct XsEuler* src);
89 XSTYPES_DLL_API void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const struct XsMatrix* ori);
90 XSTYPES_DLL_API const XsQuaternion* XsQuaternion_identity(void);
91 XSTYPES_DLL_API void XsQuaternion_multiply(const XsQuaternion* left, const XsQuaternion* right, XsQuaternion* dest);
92 XSTYPES_DLL_API void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b);
93 XSTYPES_DLL_API void XsQuaternion_copy(XsQuaternion* copy, XsQuaternion const* src);
94 XSTYPES_DLL_API int XsQuaternion_equal(XsQuaternion const* a, XsQuaternion const* b);
95 XSTYPES_DLL_API int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal tolerance);
96 XSTYPES_DLL_API XsReal XsQuaternion_dotProduct(XsQuaternion const* thisPtr, XsQuaternion const* other);
97 
98 #ifdef __cplusplus
99 } // extern "C"
100 #endif
101 
102 struct XsQuaternion
103 {
104  XSCPPPROTECTED
105  union
106  {
107  struct
108  {
109  XsReal m_w;
110  XsReal m_x;
111  XsReal m_y;
112  XsReal m_z;
113  };
114  XsReal m_data[4];
115  };
116 #ifdef __cplusplus
117 public:
119  inline explicit XsQuaternion(XsReal w_ = XsMath_zero, XsReal x_ = XsMath_zero, XsReal y_ = XsMath_zero, XsReal z_ = XsMath_zero)
120  : m_w(w_)
121  , m_x(x_)
122  , m_y(y_)
123  , m_z(z_)
124  {
125  }
126 
128  inline explicit XsQuaternion(XsReal w_, XsReal x_, XsReal y_, XsReal z_, bool normalize_)
129  : m_w(w_)
130  , m_x(x_)
131  , m_y(y_)
132  , m_z(z_)
133  {
134  if (normalize_)
135  normalize();
136  }
137 
139  inline XsQuaternion(const XsQuaternion& other)
140  : m_w(other.m_w)
141  , m_x(other.m_x)
142  , m_y(other.m_y)
143  , m_z(other.m_z)
144  {}
145 
146 #if !defined(SWIG) && !defined(__ADSP21000__)
147  inline XsQuaternion(XsQuaternion&& other)
149  : m_w(other.m_w)
150  , m_x(other.m_x)
151  , m_y(other.m_y)
152  , m_z(other.m_z)
153  {}
154 #endif
155 
157  inline explicit XsQuaternion(const XsEuler& euler)
158  {
159  XsQuaternion_fromEulerAngles(this, &euler);
160  }
161 
163  inline explicit XsQuaternion(const XsMatrix& ori)
164  {
165  XsQuaternion_fromRotationMatrix(this, &ori);
166  }
167 
169  inline ~XsQuaternion()
170  {
171  }
172 
174  inline XsQuaternion& operator=(const XsQuaternion& other)
175  {
176  if (this != &other)
177  {
178  m_w = other.m_w;
179  m_x = other.m_x;
180  m_y = other.m_y;
181  m_z = other.m_z;
182  }
183  return *this;
184  }
185 
188  inline void assign(XsReal w_, XsReal x_, XsReal y_, XsReal z_)
189  {
190  m_w = w_;
191  m_x = x_;
192  m_y = y_;
193  m_z = z_;
194  }
195 
199  inline void assign(const XsReal* values)
200  {
201  for (int i = 0; i < 4; ++i)
202  m_data[i] = values[i];
203  }
204 
206  inline XsReal& operator[](XsSize index)
207  {
208  assert(index < 4);
209  return m_data[index];
210  }
211 
213  inline XsReal operator[](XsSize index) const
214  {
215  assert(index < 4);
216  return m_data[index];
217  }
218 
220  inline const XsReal* data() const
221  {
222  return m_data;
223  }
224 
226  inline XsQuaternion inverse() const
227  {
228  XsQuaternion tmp;
229  XsQuaternion_inverse(this, &tmp);
230  return tmp;
231  }
232 
234  inline XsQuaternion conjugate() const
235  {
236  XsQuaternion tmp;
237  XsQuaternion_inverse(this, &tmp);
238  return tmp;
239  }
240 
242  XsQuaternion normalized() const
243  {
244  XsQuaternion tmp;
245  XsQuaternion_normalized(this, &tmp);
246  return tmp;
247  }
248 
250  inline XsReal normalize()
251  {
252  return XsQuaternion_normalized(this, this);
253  }
254 
256  inline bool empty() const
257  {
258  return 0 != XsQuaternion_empty(this);
259  }
260 
262  inline XsQuaternion& fromEulerAngles(const XsEuler& src)
263  {
264  XsQuaternion_fromEulerAngles(this, &src);
265  return *this;
266  }
267 
269  inline XsQuaternion& fromRotationMatrix(const XsMatrix& ori)
270  {
271  XsQuaternion_fromRotationMatrix(this, &ori);
272  return *this;
273  }
274 
276  inline static const XsQuaternion& identity()
277  {
278  return *XsQuaternion_identity();
279  }
280 
285  inline void operator*=(const XsQuaternion& other)
286  {
287  XsQuaternion_multiply(this, &other, this);
288  }
289 
297  inline void multiply(const XsQuaternion& a, const XsQuaternion& b)
298  {
299  XsQuaternion_multiply(&a, &b, this);
300  }
301 
303  inline XsReal w() const
304  {
305  return m_w;
306  }
308  inline XsReal x() const
309  {
310  return m_x;
311  }
313  inline XsReal y() const
314  {
315  return m_y;
316  }
318  inline XsReal z() const
319  {
320  return m_z;
321  }
323  inline XsReal& w()
324  {
325  return m_w;
326  }
328  inline XsReal& x()
329  {
330  return m_x;
331  }
333  inline XsReal& y()
334  {
335  return m_y;
336  }
338  inline XsReal& z()
339  {
340  return m_z;
341  }
342 
344  inline void swap(XsQuaternion& other)
345  {
346  XsQuaternion_swap(this, &other);
347  }
348 
350  friend void swap(XsQuaternion& first, XsQuaternion& second)
351  {
352  first.swap(second);
353  }
354 
356  inline bool operator ==(const XsQuaternion& other) const
357  {
358  return m_w == other.m_w &&
359  m_x == other.m_x &&
360  m_y == other.m_y &&
361  m_z == other.m_z;
362  }
363 
365  inline bool isEqual(const XsQuaternion& other, XsReal tolerance) const
366  {
367  return !!XsQuaternion_compare(this, &other, tolerance);
368  }
369 
371  inline XsReal dotProduct(const XsQuaternion& other) const
372  {
373  return XsQuaternion_dotProduct(this, &other);
374  }
375 #endif
376 };
377 
378 #ifdef __cplusplus
379 inline XsQuaternion operator-(const XsQuaternion& q)
381 {
382  return XsQuaternion(q.w(), -q.x(), -q.y(), -q.z());
383 }
384 
386 inline XsQuaternion operator *(const XsQuaternion& lhs, const XsQuaternion& rhs)
387 {
388  XsQuaternion tmp(lhs);
389  tmp *= rhs;
390  return tmp;
391 }
392 #endif
393 
394 #endif
XsQuaternion::XsQuaternion_fromRotationMatrix
void XsQuaternion_fromRotationMatrix(XsQuaternion *thisPtr, const XsMatrix *ori)
Create a quaternion representation of orientation matrix ori.
Definition: xsquaternion.c:176
XsMatrix
A class that represents a matrix of real numbers.
Definition: xsmatrix.h:107
XsVector
A class that represents a vector of real numbers.
Definition: xsvector.h:113
inverse
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
XsQuaternion::XsQuaternion_normalized
XsReal XsQuaternion_normalized(const XsQuaternion *thisPtr, XsQuaternion *dest)
Create a normalized version of this quaternion.
Definition: xsquaternion.c:123
XsQuaternion::XsQuaternion_copy
void XsQuaternion_copy(XsQuaternion *copy, XsQuaternion const *src)
Copies the contents of thisPtr into copy.
Definition: xsquaternion.c:291
XsQuaternion
A class that implements a quaternion.
Definition: xsquaternion.h:102
data
data
XsQuaternion::XsQuaternion_inverse
void XsQuaternion_inverse(const XsQuaternion *thisPtr, XsQuaternion *dest)
Compute the inverse/conjugate of this quaternion.
Definition: xsquaternion.c:113
XsQuaternion::XsQuaternion_empty
int XsQuaternion_empty(const XsQuaternion *thisPtr)
Test if this is a null object.
Definition: xsquaternion.c:94
XSCPPPROTECTED
#define XSCPPPROTECTED
Definition: xstypesconfig.h:116
operator==
bool operator==(const XsFilterProfile &lhs, const XsFilterProfile &rhs)
Returns true if lhs has the same type as rhs.
Definition: scenariomatchpred.h:81
XsQuaternion::XsQuaternion_dotProduct
XsReal XsQuaternion_dotProduct(XsQuaternion const *thisPtr, XsQuaternion const *other)
Returns the dot product of the thisPtr with other.
Definition: xsquaternion.c:343
XsQuaternion
struct XsQuaternion XsQuaternion
Definition: xsquaternion.h:80
XsReal
double XsReal
Defines the floating point type used by the Xsens libraries.
Definition: xstypedefs.h:73
XsQuaternion::XsQuaternion_equal
int XsQuaternion_equal(XsQuaternion const *a, XsQuaternion const *b)
returns non-zero if a and b are numerically equal
Definition: xsquaternion.c:301
XsQuaternion::XsQuaternion_multiply
void XsQuaternion_multiply(const XsQuaternion *left, const XsQuaternion *right, XsQuaternion *dest)
Multiply left quaternion with right quaternion and put the result in dest. The parameters may point t...
Definition: xsquaternion.c:257
XsSize
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:74
XsQuaternion::XsQuaternion_identity
const XsQuaternion * XsQuaternion_identity(void)
Returns an XsQuaternion that represents the identity quaternion.
Definition: xsquaternion.c:250
XsQuaternion::m_x
XsReal m_x
Stores the x component of the quaternion.
Definition: xsquaternion.h:110
XsQuaternion::m_data
XsReal m_data[4]
Stores the quaternion in an array of four elements.
Definition: xsquaternion.h:114
xsmath.h
XsMath_zero
XSMATHCONST XsReal XsMath_zero
0
Definition: xsmath.h:150
XsQuaternion::XsQuaternion_swap
void XsQuaternion_swap(XsQuaternion *a, XsQuaternion *b)
Swap the contents of a and b.
Definition: xsquaternion.c:277
XSTYPES_DLL_API
#define XSTYPES_DLL_API
Definition: xstypesconfig.h:65
XsEuler
Contains Euler Angle data and conversion from Quaternion.
Definition: xseuler.h:92
XsQuaternion::m_y
XsReal m_y
Stores the y component of the quaternion.
Definition: xsquaternion.h:111
XsQuaternion::XsQuaternion_destruct
void XsQuaternion_destruct(XsQuaternion *thisPtr)
Sets the contents to 0, which is an invalid XsQuaternion.
Definition: xsquaternion.c:85
XsQuaternion::m_w
XsReal m_w
Stores the w component of the quaternion.
Definition: xsquaternion.h:109
operator-
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q)
XsQuaternion::XsQuaternion_compare
int XsQuaternion_compare(XsQuaternion const *thisPtr, XsQuaternion const *other, XsReal tolerance)
Returns non-zero if the values at thisPtr and other are within tolerance of each other.
Definition: xsquaternion.c:323
XsQuaternion::m_z
XsReal m_z
Stores the z component of the quaternion.
Definition: xsquaternion.h:112
XsQuaternion::XsQuaternion_fromEulerAngles
void XsQuaternion_fromEulerAngles(XsQuaternion *thisPtr, const XsEuler *src)
Create a quaternion representation from euler angles.
Definition: xsquaternion.c:146
XsQuaternion::XsQuaternion_normalize
XsReal XsQuaternion_normalize(XsQuaternion *thisPtr)
Normalize this quaternion.
Definition: xsquaternion.c:140

xsens_mti_driver
Author(s):
autogenerated on Sun Sep 3 2023 02:43:20