SEGS  0.6.1-develop [d2cac982]
Super Entity Game Server
CoHMath.h
Go to the documentation of this file.
1 /*
2  * SEGS - Super Entity Game Server
3  * http://www.segs.io/
4  * Copyright (c) 2006 - 2019 SEGS Team (see AUTHORS.md)
5  * This software is licensed under the terms of the 3-clause BSD License. See LICENSE.md for details.
6  */
7 
8 #pragma once
9 #include <glm/vec3.hpp>
10 #include <glm/mat3x3.hpp>
11 #include <glm/gtc/quaternion.hpp>
12 #include <glm/gtc/constants.hpp>
13 #include <algorithm>
14 
15 // AngleRadians for use in PosUpdate etc
16 // TODO: we can replace this with glm::vec and other methods
18 {
19  static AngleRadians fromDeg(float deg) { return AngleRadians(deg*glm::pi<float>()/180.0f);}
20  float toDeg() { return AngleRadians((v*180.0f)/ glm::pi<float>()).v;}
21  explicit AngleRadians(float x=0.0f) : v(x) {}
23  {
24  AngleRadians result(v);
25  return result-=ot;
26  }
28  return AngleRadians(-v);
29  }
30  float operator/(AngleRadians &other) const {
31  return v/other.v;
32  }
34  {
35  AngleRadians result(v);
36  result+=ot;
37  result.fixup();
38  return result;
39  }
40  AngleRadians operator*(float scale) const
41  {
42  return AngleRadians(v*scale);
43  }
44  AngleRadians &operator*=(float scale)
45  {
46  v*=scale;
47  return *this;
48  }
50  {
51  v += ot.v;
52  fixup();
53  return *this;
54  }
56  {
57  v -= ot.v;
58  fixup();
59  return *this;
60  }
61  bool operator==(float other) const { return v == other; }
62  bool operator==(const AngleRadians &other) const { return v == other.v; }
63  bool operator!=(const AngleRadians &other) const { return v != other.v; }
65  if( v > glm::pi<float>())
66  v -= glm::two_pi<float>();
67  if( v <= -glm::pi<float>())
68  v += glm::two_pi<float>();
69  return *this;
70  }
71  bool operator<( const AngleRadians &o) const {
72  return v<o.v;
73  }
74  bool operator>( const AngleRadians &o) const {
75  return v>o.v;
76  }
77  AngleRadians lerp(AngleRadians towards,float factor) const {
78 
79  float v3(towards.v - v);
80  if( v3 > glm::pi<float>())
81  v3 = v3 - glm::two_pi<float>();
82  if( v3 <= -glm::pi<float>())
83  v3 = v3 + glm::two_pi<float>();
84  return AngleRadians(v3 * factor + v);
85 
86  }
87  // operator float()
88  // { return v;}
89  float v;
90  int toIntegerForm() const
91  {
92  return int((v + glm::pi<float>()) * 2048.0f / (glm::two_pi<float>()));
93  }
94  float fromIntegerForm(/*int v*/) const
95  {
96  return (float(v)/2048.0f)*glm::two_pi<float>() - glm::pi<float>();
97  }
98  explicit operator float() const {
99  return v;
100  }
101 };
102 
103 // All conversion will use YPR order of rotations,
104 // but the used values are passed/returned in the PYR order
105 inline glm::vec3 toCoH_YPR(const glm::quat &q)
106 {
107  float qxx(q.x * q.x);
108  float qyy(q.y * q.y);
109  float qzz(q.z * q.z);
110  float qxz(q.x * q.z);
111  float qxy(q.x * q.y);
112  float qyz(q.y * q.z);
113  float qwx(q.w * q.x);
114  float qwy(q.w * q.y);
115  float qwz(q.w * q.z);
116  //glm::mat3 mat(mat3_cast(q));
117  //float mat00 = 1 - 2 * (qyy + qzz);
118  float mat01 = 2 * (qxy + qwz);
119  //float mat02 = 2 * (qxz - qwy);
120  float mat10 = 2 * (qxy - qwz);
121  float mat11 = 1 - 2 * (qxx + qzz);
122  float mat12 = 2 * (qyz + qwx);
123  //float mat20 = 2 * (qxz + qwy);
124  float mat21 = 2 * (qyz - qwx);
125  //float mat22 = 1 - 2 * (qxx + qyy);
126  float pitchVal;
127  float yawVal;
128  float rollVal;
129 
130  if(std::abs(1.0f - std::abs(mat21)) >= 0.00001f)
131  {
132  rollVal = std::atan2(-mat01, mat11);
133  const float cos_roll = std::cos(rollVal);
134  if(cos_roll == 0.0f)
135  {
136  if(rollVal <= 0.0f)
137  {
138  pitchVal = std::atan2(mat21, mat01);
139  yawVal = std::atan2(-mat12, mat10);
140  }
141  else
142  {
143  pitchVal = std::atan2(mat21, -mat01);
144  yawVal = std::atan2(mat12, -mat10);
145  }
146  }
147  else
148  {
149  float v11 = mat11 / cos_roll;
150  pitchVal = std::atan2(mat21, v11);
151  yawVal = std::atan2(2 * (qxz + qwy) / v11, (1 - 2 * (qxx + qyy)) / v11);
152  }
153  }
154  else
155  {
156  yawVal = std::atan2(-(2 * (qxz - qwy)), 1 - 2 * (qyy + qzz));
157  if(mat21 <= 0.0f)
158  pitchVal = -glm::radians(90.0f);
159  else
160  pitchVal = glm::radians(90.0f);
161  rollVal = 0.0;
162  }
163 
164  assert(pitchVal > -16.0f);
165  assert(yawVal > -16.0f);
166  assert(rollVal > -16.0f);
167  return {pitchVal,yawVal,rollVal};
168 }
169 
170 inline glm::vec3 CoHYprFromMat(const glm::mat3 &mat)
171 {
172  float pitchVal;
173  float yawVal;
174  float rollVal;
175 
176  if(std::abs(1.0f - std::abs(mat[2][1])) >= 0.00001f)
177  {
178  rollVal = std::atan2(-mat[0][1], mat[1][1]);
179  const float cos_roll = std::cos(rollVal);
180  if(cos_roll == 0.0f)
181  {
182  if(rollVal <= 0.0f)
183  {
184  pitchVal = std::atan2(mat[2][1], mat[0][1]);
185  yawVal = std::atan2(-mat[1][2], mat[1][0]);
186  }
187  else
188  {
189  pitchVal = std::atan2(mat[2][1], -mat[0][1]);
190  yawVal = std::atan2(mat[1][2], -mat[1][0]);
191  }
192  }
193  else
194  {
195  float v11 = mat[1][1] / cos_roll;
196  pitchVal = std::atan2(mat[2][1], v11);
197  yawVal = std::atan2(mat[2][0] / v11, mat[2][2] / v11);
198  }
199  }
200  else
201  {
202  yawVal = std::atan2(-mat[2][0],mat[0][0]);
203  if(mat[2][1] <= 0.0f)
204  pitchVal = -1.570796326794897f;
205  else
206  pitchVal = 1.570796326794897f;
207  rollVal = 0.0;
208  }
209 
210  assert(pitchVal > -16.0f);
211  assert(yawVal > -16.0f);
212  assert(rollVal > -16.0f);
213  return {pitchVal,yawVal,rollVal};
214 }
215 
216 inline glm::quat fromCoHYpr(glm::vec3 pyr)
217 {
218  const float cp = std::cos(pyr.x);
219  const float sp = std::sin(pyr.x);
220  const float cy = std::cos(pyr.y);
221  const float sy = std::sin(pyr.y);
222  const float cr = std::cos(pyr.z);
223  const float sr = std::sin(pyr.z);
224  const float m00 = cr * cy + sy * sp * sr;
225  const float m11 = cr * cp;
226  const float m22 = cy * cp;
227  const float m21 = sp;
228  const float m12 = - cy * sp * cr - sr * sy;
229  const float m02 = cy * sp * sr - cr * sy;
230  const float m20 = sy * cp;
231  const float m01 = - sr * cp;
232  const float m10 = sr * cy - sy * sp * cr;
233  glm::quat zc;
234  zc.w = std::sqrt(std::max(0.f, 1 + m00 + m11 + m22)) / 2;
235  zc.x = std::sqrt(std::max(0.f, 1 + m00 - m11 - m22)) / 2;
236  zc.y = std::sqrt(std::max(0.f, 1 - m00 + m11 - m22)) / 2;
237  zc.z = std::sqrt(std::max(0.f, 1 - m00 - m11 + m22)) / 2;
238  zc.x = std::copysign(zc.x, m12 - m21);
239  zc.y = std::copysign(zc.y, m20 - m02);
240  zc.z = std::copysign(zc.z, m01 - m10);
241  return zc;
242 }
243 
244 inline void transformFromYPRandTranslation(glm::mat4 & mat, glm::vec3 pyr,glm::vec3 translation)
245 {
246  float cos_p = std::cos(pyr.x);
247  float neg_sin_p = -std::sin(pyr.x);
248  float cos_y = std::cos(pyr.y);
249  float neg_sin_y = -std::sin(pyr.y);
250  float cos_r = std::cos(pyr.z);
251  float neg_sin_r = -std::sin(pyr.z);
252  float tmp = - cos_y * neg_sin_p;
253  glm::mat3 rotmat;
254  rotmat[0][0] = cos_r * cos_y - neg_sin_y * neg_sin_p * neg_sin_r;
255  rotmat[0][1] = neg_sin_r * cos_p;
256  rotmat[0][2] = tmp * neg_sin_r + cos_r * neg_sin_y;
257  rotmat[1][0] = -(neg_sin_r * cos_y) - neg_sin_y * neg_sin_p * cos_r;
258  rotmat[1][1] = cos_r * cos_p;
259  rotmat[1][2] = tmp * cos_r - neg_sin_r * neg_sin_y;
260  rotmat[2][0] = -(neg_sin_y * cos_p);
261  rotmat[2][1] = -neg_sin_p;
262  rotmat[2][2] = cos_y * cos_p;
263 
264  mat[0]= glm::vec4(rotmat[0],0);
265  mat[1]= glm::vec4(rotmat[1],0);
266  mat[2]= glm::vec4(rotmat[2],0);
267  mat[3]= glm::vec4(translation,1);
268 }
269 
270 // XYZ
271 inline void createMat3RYP(glm::mat3 *mat, const glm::vec3 *vec)
272 {
273  float cx = std::cos(vec->x);
274  float nsx = -std::sin(vec->x);
275  float cy = std::cos(vec->y);
276  float nsy = -std::sin(vec->y);
277  float cz = std::cos(vec->z);
278  float nsz = -std::sin(vec->z);
279  float tmp = -(cz * nsy);
280  float tmp2 = -(nsz * nsy);
281  mat[0][0].x = cz * cy;
282  mat[0][1].y = nsz * cy;
283  mat[0][2].z = nsy;
284  mat[1][0].x = tmp * nsx - nsz * cx;
285  mat[1][1].y = tmp2 * nsx + cz * cx;
286  mat[1][2].z = cy * nsx;
287  mat[2][0].x = nsz * nsx + tmp * cx;
288  mat[2][1].y = tmp2 * cx - cz * nsx;
289  mat[2][2].z = cy * cx;
290 }
291 
292 inline void camLookAt(glm::vec3 *vec, glm::mat3 *mat)
293 {
294  glm::vec3 result;
295 
296  result.x = std::atan2(vec->y,sqrt(vec->z * vec->z + vec->x * vec->x));
297  result.y = std::atan2(vec->x,vec->z);
298  result.z = 0;
299  createMat3RYP(mat, &result);
300 }
301 
302 inline float normalizeRadAngle(float ang)
303 {
304  float res = ang;
305  if( ang > glm::pi<float>() )
306  res -= glm::two_pi<float>();
307  if( res <= -glm::pi<float>() )
308  res += glm::two_pi<float>();
309  return res;
310 }
311 
312 inline uint32_t countBits(uint32_t val)
313 {
314  uint32_t r = 0;
315  while (val >>= 1)
316  r++;
317 
318  return r; // log2(v)
319 }
320 
321 inline static float AngleDequantize(uint32_t val,int numb_bits) {
322  float v = val;
323  v = v/(1<<numb_bits);
324  v *= glm::two_pi<float>();
325  v -= glm::pi<float>();
326  return v;
327 }
328 
329 inline uint32_t AngleQuantize(float val,int numb_bits)
330 {
331  int max_val = 1<<numb_bits;
332 
333  float v = normalizeRadAngle(val); // ensure v falls within -pi..pi
334  v = (v+glm::pi<float>())/glm::two_pi<float>(); // 0..1
335  v *= max_val; // 0..max_val
336  return uint32_t(v);
337 }
Definition: CoHMath.h:17
glm::vec3 toCoH_YPR(const glm::quat &q)
Definition: CoHMath.h:105
void createMat3RYP(glm::mat3 *mat, const glm::vec3 *vec)
Definition: CoHMath.h:271
bool operator==(float other) const
Definition: CoHMath.h:61
bool operator==(const AngleRadians &other) const
Definition: CoHMath.h:62
static AngleRadians fromDeg(float deg)
Definition: CoHMath.h:19
AngleRadians & operator*=(float scale)
Definition: CoHMath.h:44
AngleRadians operator+(const AngleRadians &ot) const
Definition: CoHMath.h:33
float normalizeRadAngle(float ang)
Definition: CoHMath.h:302
int toIntegerForm() const
Definition: CoHMath.h:90
float v
Definition: CoHMath.h:89
AngleRadians operator-(const AngleRadians &ot) const
Definition: CoHMath.h:22
uint32_t AngleQuantize(float val, int numb_bits)
Definition: CoHMath.h:329
glm::quat fromCoHYpr(glm::vec3 pyr)
Definition: CoHMath.h:216
void camLookAt(glm::vec3 *vec, glm::mat3 *mat)
Definition: CoHMath.h:292
float fromIntegerForm() const
Definition: CoHMath.h:94
void transformFromYPRandTranslation(glm::mat4 &mat, glm::vec3 pyr, glm::vec3 translation)
Definition: CoHMath.h:244
AngleRadians operator-() const
Definition: CoHMath.h:27
AngleRadians & operator+=(const AngleRadians &ot)
Definition: CoHMath.h:49
AngleRadians operator*(float scale) const
Definition: CoHMath.h:40
float toDeg()
Definition: CoHMath.h:20
AngleRadians & fixup()
Definition: CoHMath.h:64
AngleRadians(float x=0.0f)
Definition: CoHMath.h:21
bool operator>(const AngleRadians &o) const
Definition: CoHMath.h:74
uint32_t countBits(uint32_t val)
Definition: CoHMath.h:312
float operator/(AngleRadians &other) const
Definition: CoHMath.h:30
bool operator!=(const AngleRadians &other) const
Definition: CoHMath.h:63
AngleRadians lerp(AngleRadians towards, float factor) const
Definition: CoHMath.h:77
bool operator<(const AngleRadians &o) const
Definition: CoHMath.h:71
AngleRadians & operator-=(const AngleRadians &ot)
Definition: CoHMath.h:55
glm::vec3 CoHYprFromMat(const glm::mat3 &mat)
Definition: CoHMath.h:170