Overte C++ Documentation
GLMHelpers.h
1 //
2 // GLMHelpers.h
3 // libraries/shared/src
4 //
5 // Created by Stephen Birarda on 2014-08-07.
6 // Copyright 2014 High Fidelity, Inc.
7 //
8 // Distributed under the Apache License, Version 2.0.
9 // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
10 //
11 
12 #ifndef hifi_GLMHelpers_h
13 #define hifi_GLMHelpers_h
14 
15 #include <stdint.h>
16 
17 #include <array>
18 
19 #include <glm/glm.hpp>
20 #include <glm/gtc/quaternion.hpp>
21 #include <glm/gtx/quaternion.hpp>
22 #include <glm/gtx/component_wise.hpp>
23 
24 // Bring the most commonly used GLM types into the default namespace
25 using glm::ivec2;
26 using glm::ivec3;
27 using glm::ivec4;
28 using glm::uvec2;
29 using glm::u8vec3;
30 using glm::uvec3;
31 using glm::uvec4;
32 using glm::mat3;
33 using glm::mat4;
34 using glm::vec2;
35 using glm::vec3;
36 using glm::vec4;
37 using glm::quat;
38 
39 #if defined(__GNUC__) && !defined(__clang__)
40 #pragma GCC diagnostic push
41 #pragma GCC diagnostic ignored "-Wdouble-promotion"
42 #endif
43 
44 #include <QtCore/QByteArray>
45 #include <QtGui/QMatrix4x4>
46 #include <QtGui/QColor>
47 
48 #if defined(__GNUC__) && !defined(__clang__)
49 #pragma GCC diagnostic pop
50 #endif
51 
52 #include "SharedUtil.h"
53 
54 // this is where the coordinate system is represented
55 const glm::vec3 IDENTITY_RIGHT = glm::vec3( 1.0f, 0.0f, 0.0f);
56 const glm::vec3 IDENTITY_UP = glm::vec3( 0.0f, 1.0f, 0.0f);
57 const glm::vec3 IDENTITY_FORWARD = glm::vec3( 0.0f, 0.0f,-1.0f);
58 
59 glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha);
60 
61 class Matrices {
62 public:
63  static const mat4 IDENTITY;
64  static const mat4 X_180;
65  static const mat4 Y_180;
66  static const mat4 Z_180;
67 };
68 
69 class Quaternions {
70  public:
71  static const quat IDENTITY;
72  static const quat X_180;
73  static const quat Y_180;
74  static const quat Z_180;
75 };
76 
77 class Vectors {
78 public:
79  static const vec3 UNIT_X;
80  static const vec3 UNIT_Y;
81  static const vec3 UNIT_Z;
82  static const vec3 UNIT_NEG_X;
83  static const vec3 UNIT_NEG_Y;
84  static const vec3 UNIT_NEG_Z;
85  static const vec3 UNIT_XY;
86  static const vec3 UNIT_XZ;
87  static const vec3 UNIT_YZ;
88  static const vec3 UNIT_XYZ;
89  static const vec3 MAX;
90  static const vec3 MIN;
91  static const vec3 ZERO;
92  static const vec3 ONE;
93  static const vec3 TWO;
94  static const vec3 HALF;
95  static const vec3& RIGHT;
96  static const vec3& UP;
97  static const vec3& FRONT;
98  static const vec3 ZERO4;
99 };
100 
101 // These pack/unpack functions are designed to start specific known types in as efficient a manner
102 // as possible. Taking advantage of the known characteristics of the semantic types.
103 
104 // Angles are known to be between 0 and 360 degrees, this allows us to encode in 16bits with great accuracy
105 int packFloatAngleToTwoByte(unsigned char* buffer, float degrees);
106 int unpackFloatAngleFromTwoByte(const uint16_t* byteAnglePointer, float* destinationPointer);
107 
108 // Orientation Quats are known to have 4 normalized components be between -1.0 and 1.0
109 // this allows us to encode each component in 16bits with great accuracy
110 int packOrientationQuatToBytes(unsigned char* buffer, const glm::quat& quatInput);
111 int unpackOrientationQuatFromBytes(const unsigned char* buffer, glm::quat& quatOutput);
112 
113 // alternate compression method that picks the smallest three quaternion components.
114 // and packs them into 15 bits each. An additional 2 bits are used to encode which component
115 // was omitted. Also because the components are encoded from the -1/sqrt(2) to 1/sqrt(2) which
116 // gives us some extra precision over the -1 to 1 range. The final result will have a maximum
117 // error of +- 4.3e-5 error per compoenent.
118 int packOrientationQuatToSixBytes(unsigned char* buffer, const glm::quat& quatInput);
119 int unpackOrientationQuatFromSixBytes(const unsigned char* buffer, glm::quat& quatOutput);
120 
121 // Ratios need the be highly accurate when less than 10, but not very accurate above 10, and they
122 // are never greater than 1000 to 1, this allows us to encode each component in 16bits
123 int packFloatRatioToTwoByte(unsigned char* buffer, float ratio);
124 int unpackFloatRatioFromTwoByte(const unsigned char* buffer, float& ratio);
125 
126 // Near/Far Clip values need the be highly accurate when less than 10, but only integer accuracy above 10 and
127 // they are never greater than 16,000, this allows us to encode each component in 16bits
128 int packClipValueToTwoByte(unsigned char* buffer, float clipValue);
129 int unpackClipValueFromTwoByte(const unsigned char* buffer, float& clipValue);
130 
131 // Positive floats that don't need to be very precise
132 int packFloatToByte(unsigned char* buffer, float value, float scaleBy);
133 int unpackFloatFromByte(const unsigned char* buffer, float& value, float scaleBy);
134 
135 // Allows sending of fixed-point numbers: radix 1 makes 15.1 number, radix 8 makes 8.8 number, etc
136 int packFloatScalarToSignedTwoByteFixed(unsigned char* buffer, float scalar, int radix);
137 int unpackFloatScalarFromSignedTwoByteFixed(const int16_t* byteFixedPointer, float* destinationPointer, int radix);
138 
139 // A convenience for sending vec3's as fixed-point floats
140 int packFloatVec3ToSignedTwoByteFixed(unsigned char* destBuffer, const glm::vec3& srcVector, int radix);
141 int unpackFloatVec3FromSignedTwoByteFixed(const unsigned char* sourceBuffer, glm::vec3& destination, int radix);
142 
143 bool closeEnough(float a, float b, float relativeError);
144 
146 glm::vec3 safeEulerAngles(const glm::quat& q);
147 
148 float angleBetween(const glm::vec3& v1, const glm::vec3& v2);
149 
150 glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2);
151 
152 bool isPointBehindTrianglesPlane(glm::vec3 point, glm::vec3 p0, glm::vec3 p1, glm::vec3 p2);
153 
154 glm::vec3 extractTranslation(const glm::mat4& matrix);
155 
156 void setTranslation(glm::mat4& matrix, const glm::vec3& translation);
157 
158 glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal = false);
159 glm::quat glmExtractRotation(const glm::mat4& matrix);
160 
161 glm::vec3 extractScale(const glm::mat4& matrix);
162 
163 float extractUniformScale(const glm::mat4& matrix);
164 
165 float extractUniformScale(const glm::vec3& scale);
166 
167 QByteArray createByteArray(const glm::vec3& vector);
168 QByteArray createByteArray(const glm::quat& quat);
169 
171 const float ORIENTATION_SIMILAR_ENOUGH = 5.0f; // 10 degrees in any direction
172 bool isSimilarOrientation(const glm::quat& orientionA, const glm::quat& orientionB,
173  float similarEnough = ORIENTATION_SIMILAR_ENOUGH);
174 const float POSITION_SIMILAR_ENOUGH = 0.1f; // 0.1 meter
175 bool isSimilarPosition(const glm::vec3& positionA, const glm::vec3& positionB, float similarEnough = POSITION_SIMILAR_ENOUGH);
176 
177 uvec2 toGlm(const QSize& size);
178 ivec2 toGlm(const QPoint& pt);
179 vec2 toGlm(const QPointF& pt);
180 vec3 toGlm(const glm::u8vec3& color);
181 vec4 toGlm(const QColor& color);
182 ivec4 toGlm(const QRect& rect);
183 vec4 toGlm(const glm::u8vec3& color, float alpha);
184 
185 QSize fromGlm(const glm::ivec2 & v);
186 QMatrix4x4 fromGlm(const glm::mat4 & m);
187 
188 QRectF glmToRect(const glm::vec2 & pos, const glm::vec2 & size);
189 
190 template <typename T>
191 float aspect(const T& t) {
192  return (float)t.x / (float)t.y;
193 }
194 
195 // Take values in an arbitrary range [0, size] and convert them to the range [0, 1]
196 template <typename T>
197 T toUnitScale(const T& value, const T& size) {
198  return value / size;
199 }
200 
201 // Take values in an arbitrary range [0, size] and convert them to the range [0, 1]
202 template <typename T>
203 T toNormalizedDeviceScale(const T& value, const T& size) {
204  vec2 result = toUnitScale(value, size);
205  result *= 2.0f;
206  result -= 1.0f;
207  return result;
208 }
209 
210 #define YAW(euler) euler.y
211 #define PITCH(euler) euler.x
212 #define ROLL(euler) euler.z
213 
214 // float - linear interpolate
215 inline float lerp(float x, float y, float a) {
216  return x * (1.0f - a) + (y * a);
217 }
218 
219 // vec2 lerp - linear interpolate
220 template<typename T, glm::precision P>
221 glm::tvec2<T, P> lerp(const glm::tvec2<T, P>& x, const glm::tvec2<T, P>& y, T a) {
222  return x * (T(1) - a) + (y * a);
223 }
224 
225 // vec3 lerp - linear interpolate
226 template<typename T, glm::precision P>
227 glm::tvec3<T, P> lerp(const glm::tvec3<T, P>& x, const glm::tvec3<T, P>& y, T a) {
228  return x * (T(1) - a) + (y * a);
229 }
230 
231 // vec4 lerp - linear interpolate
232 template<typename T, glm::precision P>
233 glm::tvec4<T, P> lerp(const glm::tvec4<T, P>& x, const glm::tvec4<T, P>& y, T a) {
234  return x * (T(1) - a) + (y * a);
235 }
236 
237 glm::mat4 createMatFromQuatAndPos(const glm::quat& q, const glm::vec3& p);
238 glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat& rot, const glm::vec3& trans);
239 glm::mat4 createMatFromScale(const glm::vec3& scale);
240 glm::quat cancelOutRoll(const glm::quat& q);
241 glm::quat cancelOutRollAndPitch(const glm::quat& q);
242 glm::mat4 cancelOutRollAndPitch(const glm::mat4& m);
243 glm::vec3 transformPoint(const glm::mat4& m, const glm::vec3& p);
244 glm::vec3 transformVectorFast(const glm::mat4& m, const glm::vec3& v);
245 glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v);
246 
247 // Calculate an orthogonal basis from a primary and secondary axis.
248 // The uAxis, vAxis & wAxis will form an orthognal basis.
249 // The primary axis will be the uAxis.
250 // The vAxis will be as close as possible to to the secondary axis.
251 void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& secondaryAxis,
252  glm::vec3& uAxisOut, glm::vec3& vAxisOut, glm::vec3& wAxisOut);
253 
254 // assumes z-forward and y-up
255 glm::vec2 getFacingDir2D(const glm::quat& rot);
256 
257 // assumes z-forward and y-up
258 glm::vec2 getFacingDir2D(const glm::mat4& m);
259 
260 inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
261 inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
262 inline bool isNaN(const glm::mat3& value) { return isNaN(value * glm::vec3(1.0f)); }
263 
264 glm::mat4 orthoInverse(const glm::mat4& m);
265 
266 // Return a random vector of average length 1
267 glm::vec3 randVector();
268 
269 bool isNonUniformScale(const glm::vec3& scale);
270 
271 //
272 // Safe replacement of glm_mat4_mul() for unaligned arguments instead of __m128
273 //
274 inline void glm_mat4u_mul(const glm::mat4& m1, const glm::mat4& m2, glm::mat4& r) {
275 
276 #if GLM_ARCH & GLM_ARCH_SSE2_BIT
277  __m128 u0 = _mm_loadu_ps((float*)&m1[0][0]);
278  __m128 u1 = _mm_loadu_ps((float*)&m1[1][0]);
279  __m128 u2 = _mm_loadu_ps((float*)&m1[2][0]);
280  __m128 u3 = _mm_loadu_ps((float*)&m1[3][0]);
281 
282  __m128 v0 = _mm_loadu_ps((float*)&m2[0][0]);
283  __m128 v1 = _mm_loadu_ps((float*)&m2[1][0]);
284  __m128 v2 = _mm_loadu_ps((float*)&m2[2][0]);
285  __m128 v3 = _mm_loadu_ps((float*)&m2[3][0]);
286 
287  __m128 t0 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(0,0,0,0)), u0);
288  __m128 t1 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(1,1,1,1)), u1);
289  __m128 t2 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(2,2,2,2)), u2);
290  __m128 t3 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(3,3,3,3)), u3);
291  v0 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));
292 
293  t0 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(0,0,0,0)), u0);
294  t1 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(1,1,1,1)), u1);
295  t2 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(2,2,2,2)), u2);
296  t3 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(3,3,3,3)), u3);
297  v1 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));
298 
299  t0 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(0,0,0,0)), u0);
300  t1 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(1,1,1,1)), u1);
301  t2 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(2,2,2,2)), u2);
302  t3 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(3,3,3,3)), u3);
303  v2 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));
304 
305  t0 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(0,0,0,0)), u0);
306  t1 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(1,1,1,1)), u1);
307  t2 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(2,2,2,2)), u2);
308  t3 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(3,3,3,3)), u3);
309  v3 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));
310 
311  _mm_storeu_ps((float*)&r[0][0], v0);
312  _mm_storeu_ps((float*)&r[1][0], v1);
313  _mm_storeu_ps((float*)&r[2][0], v2);
314  _mm_storeu_ps((float*)&r[3][0], v3);
315 #else
316  r = m1 * m2;
317 #endif
318 }
319 
320 //
321 // Fast replacement of glm::packSnorm3x10_1x2()
322 // The SSE2 version quantizes using round to nearest even.
323 // The glm version quantizes using round away from zero.
324 //
325 inline uint32_t glm_packSnorm3x10_1x2(vec4 const& v) {
326 
327  union i10i10i10i2 {
328  struct {
329  int x : 10;
330  int y : 10;
331  int z : 10;
332  int w : 2;
333  } data;
334  uint32_t pack;
335  } Result;
336 
337 #if GLM_ARCH & GLM_ARCH_SSE2_BIT
338  __m128 vclamp = _mm_min_ps(_mm_max_ps(_mm_loadu_ps((float*)&v[0]), _mm_set1_ps(-1.0f)), _mm_set1_ps(1.0f));
339  __m128i vpack = _mm_cvtps_epi32(_mm_mul_ps(vclamp, _mm_setr_ps(511.f, 511.f, 511.f, 1.f)));
340 
341  Result.data.x = _mm_cvtsi128_si32(vpack);
342  Result.data.y = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(1,1,1,1)));
343  Result.data.z = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(2,2,2,2)));
344  Result.data.w = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(3,3,3,3)));
345 #else
346  ivec4 const Pack(round(clamp(v, -1.0f, 1.0f) * vec4(511.f, 511.f, 511.f, 1.f)));
347 
348  Result.data.x = Pack.x;
349  Result.data.y = Pack.y;
350  Result.data.z = Pack.z;
351  Result.data.w = Pack.w;
352 #endif
353  return Result.pack;
354 }
355 
356 // convert float to int, using round-to-nearest-even (undefined on overflow)
357 inline int fastLrintf(float x) {
358 #if GLM_ARCH & GLM_ARCH_SSE2_BIT
359  return _mm_cvt_ss2si(_mm_set_ss(x));
360 #else
361  // return lrintf(x);
362  static_assert(std::numeric_limits<double>::is_iec559, "Requires IEEE-754 double precision format");
363  union { double d; int64_t i; } bits = { (double)x };
364  bits.d += (3ULL << 51);
365  return (int)bits.i;
366 #endif
367 }
368 
369 // returns the FOV from the projection matrix
370 inline glm::vec4 extractFov( const glm::mat4& m) {
371  static const std::array<glm::vec4, 4> CLIPS{ {
372  { 1, 0, 0, 1 },
373  { -1, 0, 0, 1 },
374  { 0, 1, 0, 1 },
375  { 0, -1, 0, 1 }
376  } };
377 
378  glm::mat4 mt = glm::transpose(m);
379  glm::vec4 v, result;
380  // Left
381  v = mt * CLIPS[0];
382  result.x = -atanf(v.z / v.x);
383  // Right
384  v = mt * CLIPS[1];
385  result.y = atanf(v.z / v.x);
386  // Down
387  v = mt * CLIPS[2];
388  result.z = -atanf(v.z / v.y);
389  // Up
390  v = mt * CLIPS[3];
391  result.w = atanf(v.z / v.y);
392  return result;
393 }
394 
395 #endif // hifi_GLMHelpers_h