OpenCOVER
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
mathUtils.h
Go to the documentation of this file.
1 /* This file is part of COVISE.
2 
3  You can use it under the terms of the GNU Lesser General Public License
4  version 2.1 or later, see lgpl-2.1.txt.
5 
6  * License: LGPL 2+ */
7 
8 #ifndef CO_MATH_UTILS_H_
9 #define CO_MATH_UTILS_H_
10 #ifndef _USE_MATH_DEFINES
11 #define _USE_MATH_DEFINES
12 #endif
13 #include <util/coExport.h>
14 #include <osg/Vec3>
15 #include <osg/Matrix>
16 #include <math.h>
17 
18 #ifndef M_PI
19 #define M_PI 3.141592653
20 #endif
21 
22 //#define MAKE_EULER_MAT(m,h,p,r) m.makeRotate(h,osg::Y_AXIS, p,osg::X_AXIS, r,osg::Z_AXIS)
23 
24 #define MAKE_EULER_MAT(m, h, p, r) \
25  { \
26  double sr, sp, sh, cr, cp, ch; \
27  sr = sin(r / 180.0 * M_PI); \
28  sp = sin(p / 180.0 * M_PI); \
29  sh = sin(h / 180.0 * M_PI); \
30  cr = cos(r / 180.0 * M_PI); \
31  cp = cos(p / 180.0 * M_PI); \
32  ch = cos(h / 180.0 * M_PI); \
33  m(0, 0) = ch * cr - sh * sr * sp; \
34  m(0, 1) = cr * sh + ch * sr * sp; \
35  m(0, 2) = -sr * cp; \
36  m(0, 3) = 0; \
37  m(1, 0) = -sh * cp; \
38  m(1, 1) = ch * cp; \
39  m(1, 2) = sp; \
40  m(1, 3) = 0; \
41  m(2, 0) = sp * cr * sh + sr * ch; \
42  m(2, 1) = sr * sh - sp * cr * ch; \
43  m(2, 2) = cp * cr; \
44  m(2, 3) = 0; \
45  m(3, 0) = 0; \
46  m(3, 1) = 0; \
47  m(3, 2) = 0; \
48  m(3, 3) = 1; \
49  }
50 #define MAKE_EULER_MAT_VEC(m, hpr) MAKE_EULER_MAT(m, hpr[0], hpr[1], hpr[2])
51 
52 #if 0
53 //#define GET_HPR(m,h,p,r) { float cp; p= asin(m(1,2)); cp = cos(p); r = acos(m(2,2)/cp); h = -asin(m(1,0)/cp); }
54 #define GET_HPR(m, h, p, r) \
55  { \
56  float cp; \
57  osg::Vec3 v1(m(0, 0), m(0, 1), m(0, 2)); \
58  osg::Vec3 v2(m(1, 0), m(1, 1), m(1, 2)); \
59  osg::Vec3 v3(m(2, 0), m(2, 1), m(2, 2)); \
60  v1.normalize(); \
61  v2.normalize(); \
62  v3.normalize(); \
63  m(0, 0) = v1[0]; \
64  m(0, 1) = v1[1]; \
65  m(0, 2) = v1[2]; \
66  m(1, 0) = v2[0]; \
67  m(1, 1) = v2[1]; \
68  m(1, 2) = v2[2]; \
69  m(2, 0) = v3[0]; \
70  m(2, 1) = v3[1]; \
71  m(2, 2) = v3[2]; \
72  p = asin(m(1, 2)); \
73  cp = cos(p); \
74  float d = m(1, 0) / cp; \
75  if (d > 1.0) \
76  { \
77  h = -M_PI_2; \
78  } \
79  else if (d < -1.0) \
80  { \
81  h = M_PI_2; \
82  } \
83  else \
84  h = -asin(d); \
85  float diff = cos(h) * cp - m(1, 1); \
86  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other Heading angle */ \
87  { \
88  h = M_PI_2 - h; \
89  diff = cos(h) * cp - m(1, 1); \
90  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other pitch angle */ \
91  { \
92  p = M_PI - p; \
93  cp = cos(p); \
94  d = m(1, 0) / cp; \
95  if (d > 1.0) \
96  { \
97  h = -M_PI_2; \
98  } \
99  else if (d < -1.0) \
100  { \
101  h = M_PI_2; \
102  } \
103  else \
104  h = -asin(d); \
105  diff = cos(h) * cp - m(1, 1); \
106  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other Heading angle */ \
107  { \
108  h = M_PI - h; \
109  } \
110  } \
111  } \
112  d = m(2, 2) / cp; \
113  if (d > 1.0) \
114  r = 0; \
115  else if (d > 1.0) \
116  r = M_PI; \
117  else \
118  r = acos(d); \
119  diff = -sin(r) * cp - m(0, 2); \
120  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other roll angle */ \
121  r = -r; \
122  h = h / M_PI * 180.0; \
123  p = p / M_PI * 180.0; \
124  r = r / M_PI * 180.0; \
125  }
126 
127 #define GET_HPR_VEC(mx, hpr) \
128  { \
129  float cp; \
130  osg::Matrix m; \
131  osg::Vec3 v1(mx(0, 0), mx(0, 1), mx(0, 2)); \
132  osg::Vec3 v2(mx(1, 0), mx(1, 1), mx(1, 2)); \
133  osg::Vec3 v3(mx(2, 0), mx(2, 1), mx(2, 2)); \
134  v1.normalize(); \
135  v2.normalize(); \
136  v3.normalize(); \
137  m(0, 0) = v1[0]; \
138  m(0, 1) = v1[1]; \
139  m(0, 2) = v1[2]; \
140  m(1, 0) = v2[0]; \
141  m(1, 1) = v2[1]; \
142  m(1, 2) = v2[2]; \
143  m(2, 0) = v3[0]; \
144  m(2, 1) = v3[1]; \
145  m(2, 2) = v3[2]; \
146  hpr[1] = asin(m(1, 2)); \
147  cp = cos(hpr[1]); \
148  hpr[0] = -asin(m(1, 0) / cp); \
149  float diff = cos(hpr[0]) * cp - m(1, 1); \
150  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other Heading angle */ \
151  { \
152  hpr[0] = M_PI - hpr[0]; \
153  diff = cos(hpr[0]) * cp - m(1, 1); \
154  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other pitch angle */ \
155  { \
156  hpr[1] = M_PI - hpr[1]; \
157  cp = cos(hpr[1]); \
158  hpr[0] = -asin(m(1, 0) / cp); \
159  diff = cos(hpr[0]) * cp - m(1, 1); \
160  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other Heading angle */ \
161  { \
162  hpr[0] = M_PI - hpr[0]; \
163  } \
164  } \
165  } \
166  hpr[2] = acos(m(2, 2) / cp); \
167  diff = -sin(hpr[2]) * cp - m(0, 2); \
168  if (diff < -0.01 || diff > 0.01) /* oops, not correct, use other roll angle */ \
169  hpr[2] = -hpr[2]; \
170  hpr[0] = hpr[0] / M_PI * 180.0; \
171  hpr[1] = hpr[1] / M_PI * 180.0; \
172  hpr[2] = hpr[2] / M_PI * 180.0; \
173  }
174 #endif
175 
176 class OSGVRUIEXPORT coCoord
177 {
178 public:
179  coCoord(){};
180  coCoord(const osg::Matrix &right);
181  ~coCoord();
182  coCoord(const coCoord &c);
183  osg::Vec3 xyz;
184  osg::Vec3 hpr;
185  coCoord &operator=(const osg::Matrix &right);
186  void makeMat(osg::Matrix &right);
187 
188 private:
189  void initFromMatrix(const osg::Matrix &right);
190 };
191 
192 // snap matrix 45 degrees in orientation
193 void OSGVRUIEXPORT snapTo45Degrees(osg::Matrix *mat);
194 // snap matrix 5 degrees in orientation
195 void OSGVRUIEXPORT snapToDegrees(float degree, osg::Matrix *mat);
196 // modulo for doubles
197 double OSGVRUIEXPORT mod(double a, double b);
198 
199 #endif
coCoord::xyz
osg::Vec3 xyz
Definition: mathUtils.h:183
snapTo45Degrees
void OSGVRUIEXPORT snapTo45Degrees(osg::Matrix *mat)
mod
double OSGVRUIEXPORT mod(double a, double b)
coCoord::coCoord
coCoord()
Definition: mathUtils.h:179
coCoord::hpr
osg::Vec3 hpr
Definition: mathUtils.h:184
snapToDegrees
void OSGVRUIEXPORT snapToDegrees(float degree, osg::Matrix *mat)
coCoord
Definition: mathUtils.h:176
Generated by   doxygen 1.8.5