SEGS  0.4.2
Super Entity Game Server
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 - 2018 SEGS Team (see Authors.txt)
5  * This software is licensed! (See License.txt 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 constexpr float F_PI = float(M_PI);
16 
17 // All conversion will use YPR order of rotations,
18 // but the used values are passed/returned in the PYR order
19 inline glm::vec3 toCoH_YPR(const glm::quat &q)
20 {
21  float qxx(q.x * q.x);
22  float qyy(q.y * q.y);
23  float qzz(q.z * q.z);
24  float qxz(q.x * q.z);
25  float qxy(q.x * q.y);
26  float qyz(q.y * q.z);
27  float qwx(q.w * q.x);
28  float qwy(q.w * q.y);
29  float qwz(q.w * q.z);
30  glm::mat3 mat(mat3_cast(q));
31  //float mat00 = 1 - 2 * (qyy + qzz);
32  float mat01 = 2 * (qxy + qwz);
33  //float mat02 = 2 * (qxz - qwy);
34  float mat10 = 2 * (qxy - qwz);
35  float mat11 = 1 - 2 * (qxx + qzz);
36  float mat12 = 2 * (qyz + qwx);
37  //float mat20 = 2 * (qxz + qwy);
38  float mat21 = 2 * (qyz - qwx);
39  //float mat22 = 1 - 2 * (qxx + qyy);
40  float pitchVal;
41  float yawVal;
42  float rollVal;
43 
44  if (std::abs(1.0f - std::abs(mat21)) >= 0.00001f)
45  {
46  rollVal = std::atan2(-mat01, mat11);
47  const float cos_roll = std::cos(rollVal);
48  if (cos_roll == 0.0f)
49  {
50  if (rollVal <= 0.0f)
51  {
52  pitchVal = std::atan2(mat21, mat01);
53  yawVal = std::atan2(-mat12, mat10);
54  }
55  else
56  {
57  pitchVal = std::atan2(mat21, -mat01);
58  yawVal = std::atan2(mat12, -mat10);
59  }
60  }
61  else
62  {
63  float v11 = mat11 / cos_roll;
64  pitchVal = std::atan2(mat21, v11);
65  yawVal = std::atan2(2 * (qxz + qwy) / v11, (1 - 2 * (qxx + qyy)) / v11);
66  }
67  }
68  else
69  {
70  yawVal = std::atan2(-(2 * (qxz - qwy)), 1 - 2 * (qyy + qzz));
71  if (mat21 <= 0.0f)
72  pitchVal = -glm::radians(90.0f);
73  else
74  pitchVal = glm::radians(90.0f);
75  rollVal = 0.0;
76  }
77 
78  assert(pitchVal > -16.0f);
79  assert(yawVal > -16.0f);
80  assert(rollVal > -16.0f);
81  return {pitchVal,yawVal,rollVal};
82 }
83 
84 inline glm::vec3 CoHYprFromMat(const glm::mat3 &mat)
85 {
86  float pitchVal;
87  float yawVal;
88  float rollVal;
89 
90  if (std::abs(1.0f - std::abs(mat[2][1])) >= 0.00001f)
91  {
92  rollVal = std::atan2(-mat[0][1], mat[1][1]);
93  const float cos_roll = std::cos(rollVal);
94  if (cos_roll == 0.0f)
95  {
96  if (rollVal <= 0.0f)
97  {
98  pitchVal = std::atan2(mat[2][1], mat[0][1]);
99  yawVal = std::atan2(-mat[1][2], mat[1][0]);
100  }
101  else
102  {
103  pitchVal = std::atan2(mat[2][1], -mat[0][1]);
104  yawVal = std::atan2(mat[1][2], -mat[1][0]);
105  }
106  }
107  else
108  {
109  float v11 = mat[1][1] / cos_roll;
110  pitchVal = std::atan2(mat[2][1], v11);
111  yawVal = std::atan2(mat[2][0] / v11, mat[2][2] / v11);
112  }
113  }
114  else
115  {
116  yawVal = std::atan2(-mat[2][0],mat[0][0]);
117  if (mat[2][1] <= 0.0f)
118  pitchVal = -1.570796326794897f;
119  else
120  pitchVal = 1.570796326794897f;
121  rollVal = 0.0;
122  }
123 
124  assert(pitchVal > -16.0f);
125  assert(yawVal > -16.0f);
126  assert(rollVal > -16.0f);
127  return {pitchVal,yawVal,rollVal};
128 }
129 
130 inline glm::quat fromCoHYpr(glm::vec3 pyr)
131 {
132  const float cp = std::cos(pyr.x);
133  const float sp = std::sin(pyr.x);
134  const float cy = std::cos(pyr.y);
135  const float sy = std::sin(pyr.y);
136  const float cr = std::cos(pyr.z);
137  const float sr = std::sin(pyr.z);
138  const float m00 = cr * cy + sy * sp * sr;
139  const float m11 = cr * cp;
140  const float m22 = cy * cp;
141  const float m21 = sp;
142  const float m12 = - cy * sp * cr - sr * sy;
143  const float m02 = cy * sp * sr - cr * sy;
144  const float m20 = sy * cp;
145  const float m01 = - sr * cp;
146  const float m10 = sr * cy - sy * sp * cr;
147  glm::quat zc;
148  zc.w = std::sqrt(std::max(0.f, 1 + m00 + m11 + m22)) / 2;
149  zc.x = std::sqrt(std::max(0.f, 1 + m00 - m11 - m22)) / 2;
150  zc.y = std::sqrt(std::max(0.f, 1 - m00 + m11 - m22)) / 2;
151  zc.z = std::sqrt(std::max(0.f, 1 - m00 - m11 + m22)) / 2;
152  zc.x = std::copysign(zc.x, m12 - m21);
153  zc.y = std::copysign(zc.y, m20 - m02);
154  zc.z = std::copysign(zc.z, m01 - m10);
155  return zc;
156 }
157 
158 inline glm::mat3 CoHYprToMat3(glm::vec3 pyr)
159 {
160  const float cp = std::cos(pyr.x);
161  const float sp = std::sin(pyr.x);
162  const float cy = std::cos(pyr.y);
163  const float sy = std::sin(pyr.y);
164  const float cr = std::cos(pyr.z);
165  const float sr = std::sin(pyr.z);
166  glm::mat3 mat;
167  mat[0][0] = cr * cy + sy * sp * sr;
168  mat[1][0] = sr * cy - sy * sp * cr;
169  mat[2][0] = sy * cp;
170  mat[0][1] = - sr * cp;
171  mat[1][1] = cr * cp;
172  mat[2][1] = sp;
173  mat[0][2] = cy * sp * sr - cr * sy;
174  mat[1][2] = - cy * sp * cr - sr * sy;
175  mat[2][2] = cy * cp;
176 
177  {
178  float sp = -std::sin(pyr.x);
179  float sy = -std::sin(pyr.y);
180  float sr = -std::sin(pyr.z);
181  mat[0][0] = cr * cy - sy * sp * sr;
182  mat[1][0] = -(sr * cy) - sy * sp * cr;
183  mat[2][0] = -(sy * cp);
184  mat[0][1] = sr * cp;
185  mat[1][1] = cr * cp;
186  mat[2][1] = -sp;
187  mat[0][2] = cy * sp * sr + cr * sy;
188  mat[1][2] = cy * sp * cr - sr * sy;
189  mat[2][2] = cy * cp;
190  }
191  return mat;
192 }
193 
194 inline float normalizeRadAngle(float ang)
195 {
196  float res = ang;
197  if ( ang > F_PI )
198  res -= 2*F_PI;
199  if ( res <= -F_PI )
200  res += 2*F_PI;
201  return res;
202 }
203 
204 inline uint32_t countBits(uint32_t val)
205 {
206  uint32_t r = 0;
207  while (val >>= 1)
208  r++;
209 
210  return r; // log2(v)
211 }
212 
213 inline static float AngleDequantize(uint32_t val,int numb_bits) {
214  float v = val;
215  v = v/(1<<numb_bits);
216  v *= (2*F_PI);
217  v -= F_PI;
218  return v;
219 }
220 
221 inline uint32_t AngleQuantize(float val,int numb_bits)
222 {
223  int max_val = 1<<numb_bits;
224 
225  float v = normalizeRadAngle(val); // ensure v falls within -pi..pi
226  v = (v+F_PI)/(2*F_PI);
227  v *= max_val;
228 // assert(v<=max_val);
229  return uint32_t(v);
230 }
glm::vec3 toCoH_YPR(const glm::quat &q)
Definition: CoHMath.h:19
float normalizeRadAngle(float ang)
Definition: CoHMath.h:194
constexpr float F_PI
Definition: CoHMath.h:15
uint32_t AngleQuantize(float val, int numb_bits)
Definition: CoHMath.h:221
glm::quat fromCoHYpr(glm::vec3 pyr)
Definition: CoHMath.h:130
glm::mat3 CoHYprToMat3(glm::vec3 pyr)
Definition: CoHMath.h:158
uint32_t countBits(uint32_t val)
Definition: CoHMath.h:204
glm::vec3 CoHYprFromMat(const glm::mat3 &mat)
Definition: CoHMath.h:84