OpenCOVER
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
176class OSGVRUIEXPORT coCoord
177{
178public:
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) const;
187
188private:
189 void initFromMatrix(const osg::Matrix &right);
190};
191
192// snap matrix 45 degrees in orientation
193void OSGVRUIEXPORT snapTo45Degrees(osg::Matrix *mat);
194// snap matrix 5 degrees in orientation
195void OSGVRUIEXPORT snapToDegrees(float degree, osg::Matrix *mat);
196// modulo for doubles
197double OSGVRUIEXPORT mod(double a, double b);
198
199#endif
snapTo45Degrees
void OSGVRUIEXPORT snapTo45Degrees(osg::Matrix *mat)
mod
double OSGVRUIEXPORT mod(double a, double b)
snapToDegrees
void OSGVRUIEXPORT snapToDegrees(float degree, osg::Matrix *mat)
coCoord
Definition: mathUtils.h:177
coCoord::coCoord
coCoord(const coCoord &c)
coCoord::coCoord
coCoord()
Definition: mathUtils.h:179
coCoord::makeMat
void makeMat(osg::Matrix &right) const
coCoord::hpr
osg::Vec3 hpr
Definition: mathUtils.h:184
coCoord::coCoord
coCoord(const osg::Matrix &right)
coCoord::xyz
osg::Vec3 xyz
Definition: mathUtils.h:183
coCoord::operator=
coCoord & operator=(const osg::Matrix &right)
Generated by doxygen 1.9.4