////  GLMHelpers.h//  libraries/shared/src////  Created by Stephen Birarda on 2014-08-07.//  Copyright 2014 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #ifndef hifi_GLMHelpers_h#define hifi_GLMHelpers_h #include <stdint.h> #include <array> #include <glm/glm.hpp>#include <glm/gtc/quaternion.hpp>#include <glm/gtx/quaternion.hpp>#include <glm/gtx/component_wise.hpp> // Bring the most commonly used GLM types into the default namespaceusing glm::ivec2;using glm::ivec3;using glm::ivec4;using glm::uvec2;using glm::u8vec3;using glm::uvec3;using glm::uvec4;using glm::mat3;using glm::mat4;using glm::vec2;using glm::vec3;using glm::vec4;using glm::quat; #if defined(__GNUC__) && !defined(__clang__)#pragma GCC diagnostic push#pragma GCC diagnostic ignored "-Wdouble-promotion"#endif #include <QtCore/QByteArray>#include <QtGui/QMatrix4x4>#include <QtGui/QColor> #if defined(__GNUC__) && !defined(__clang__)#pragma GCC diagnostic pop#endif #include "SharedUtil.h" // this is where the coordinate system is representedconst glm::vec3 IDENTITY_RIGHT = glm::vec3( 1.0f, 0.0f, 0.0f);const glm::vec3 IDENTITY_UP    = glm::vec3( 0.0f, 1.0f, 0.0f);const glm::vec3 IDENTITY_FORWARD = glm::vec3( 0.0f, 0.0f,-1.0f); glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha); class Matrices {public:    static const mat4 IDENTITY;    static const mat4 X_180;    static const mat4 Y_180;    static const mat4 Z_180;}; class Quaternions { public:    static const quat IDENTITY;    static const quat X_180;    static const quat Y_180;    static const quat Z_180;}; class Vectors {public:    static const vec3 UNIT_X;    static const vec3 UNIT_Y;    static const vec3 UNIT_Z;    static const vec3 UNIT_NEG_X;    static const vec3 UNIT_NEG_Y;    static const vec3 UNIT_NEG_Z;    static const vec3 UNIT_XY;    static const vec3 UNIT_XZ;    static const vec3 UNIT_YZ;    static const vec3 UNIT_XYZ;    static const vec3 MAX;    static const vec3 MIN;    static const vec3 ZERO;    static const vec3 ONE;    static const vec3 TWO;    static const vec3 HALF;    static const vec3& RIGHT;    static const vec3& UP;    static const vec3& FRONT;    static const vec3 ZERO4;}; // These pack/unpack functions are designed to start specific known types in as efficient a manner// as possible. Taking advantage of the known characteristics of the semantic types. // Angles are known to be between 0 and 360 degrees, this allows us to encode in 16bits with great accuracyint packFloatAngleToTwoByte(unsigned char* buffer, float degrees);int unpackFloatAngleFromTwoByte(const uint16_t* byteAnglePointer, float* destinationPointer); // Orientation Quats are known to have 4 normalized components be between -1.0 and 1.0// this allows us to encode each component in 16bits with great accuracyint packOrientationQuatToBytes(unsigned char* buffer, const glm::quat& quatInput);int unpackOrientationQuatFromBytes(const unsigned char* buffer, glm::quat& quatOutput); // alternate compression method that picks the smallest three quaternion components.// and packs them into 15 bits each. An additional 2 bits are used to encode which component// was omitted.  Also because the components are encoded from the -1/sqrt(2) to 1/sqrt(2) which// gives us some extra precision over the -1 to 1 range.  The final result will have a maximum// error of +- 4.3e-5 error per compoenent.int packOrientationQuatToSixBytes(unsigned char* buffer, const glm::quat& quatInput);int unpackOrientationQuatFromSixBytes(const unsigned char* buffer, glm::quat& quatOutput); // Ratios need the be highly accurate when less than 10, but not very accurate above 10, and they// are never greater than 1000 to 1, this allows us to encode each component in 16bitsint packFloatRatioToTwoByte(unsigned char* buffer, float ratio);int unpackFloatRatioFromTwoByte(const unsigned char* buffer, float& ratio); // Near/Far Clip values need the be highly accurate when less than 10, but only integer accuracy above 10 and// they are never greater than 16,000, this allows us to encode each component in 16bitsint packClipValueToTwoByte(unsigned char* buffer, float clipValue);int unpackClipValueFromTwoByte(const unsigned char* buffer, float& clipValue); // Positive floats that don't need to be very preciseint packFloatToByte(unsigned char* buffer, float value, float scaleBy);int unpackFloatFromByte(const unsigned char* buffer, float& value, float scaleBy); // Allows sending of fixed-point numbers: radix 1 makes 15.1 number, radix 8 makes 8.8 number, etcint packFloatScalarToSignedTwoByteFixed(unsigned char* buffer, float scalar, int radix);int unpackFloatScalarFromSignedTwoByteFixed(const int16_t* byteFixedPointer, float* destinationPointer, int radix); // A convenience for sending vec3's as fixed-point floatsint packFloatVec3ToSignedTwoByteFixed(unsigned char* destBuffer, const glm::vec3& srcVector, int radix);int unpackFloatVec3FromSignedTwoByteFixed(const unsigned char* sourceBuffer, glm::vec3& destination, int radix); bool closeEnough(float a, float b, float relativeError); /// \return vec3 with euler angles in radiansglm::vec3 safeEulerAngles(const glm::quat& q); float angleBetween(const glm::vec3& v1, const glm::vec3& v2); glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2); bool isPointBehindTrianglesPlane(glm::vec3 point, glm::vec3 p0, glm::vec3 p1, glm::vec3 p2); glm::vec3 extractTranslation(const glm::mat4& matrix); void setTranslation(glm::mat4& matrix, const glm::vec3& translation); glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal = false);glm::quat glmExtractRotation(const glm::mat4& matrix); glm::vec3 extractScale(const glm::mat4& matrix); float extractUniformScale(const glm::mat4& matrix); float extractUniformScale(const glm::vec3& scale); QByteArray createByteArray(const glm::vec3& vector);QByteArray createByteArray(const glm::quat& quat); /// \return bool are two orientations similar to each otherconst float ORIENTATION_SIMILAR_ENOUGH = 5.0f; // 10 degrees in any directionbool isSimilarOrientation(const glm::quat& orientionA, const glm::quat& orientionB,                          float similarEnough = ORIENTATION_SIMILAR_ENOUGH);const float POSITION_SIMILAR_ENOUGH = 0.1f; // 0.1 meterbool isSimilarPosition(const glm::vec3& positionA, const glm::vec3& positionB, float similarEnough = POSITION_SIMILAR_ENOUGH); uvec2 toGlm(const QSize& size);ivec2 toGlm(const QPoint& pt);vec2 toGlm(const QPointF& pt);vec3 toGlm(const glm::u8vec3& color);vec4 toGlm(const QColor& color);ivec4 toGlm(const QRect& rect);vec4 toGlm(const glm::u8vec3& color, float alpha); QSize fromGlm(const glm::ivec2 & v);QMatrix4x4 fromGlm(const glm::mat4 & m); QRectF glmToRect(const glm::vec2 & pos, const glm::vec2 & size); template <typename T>float aspect(const T& t) {    return (float)t.x / (float)t.y;} // Take values in an arbitrary range [0, size] and convert them to the range [0, 1]template <typename T>T toUnitScale(const T& value, const T& size) {    return value / size;} // Take values in an arbitrary range [0, size] and convert them to the range [0, 1]template <typename T>T toNormalizedDeviceScale(const T& value, const T& size) {    vec2 result = toUnitScale(value, size);    result *= 2.0f;    result -= 1.0f;    return result;} #define YAW(euler) euler.y#define PITCH(euler) euler.x#define ROLL(euler) euler.z // vec2 lerp - linear interpolatetemplate<typename T, glm::precision P>glm::tvec2<T, P> lerp(const glm::tvec2<T, P>& x, const glm::tvec2<T, P>& y, T a) {    return x * (T(1) - a) + (y * a);} // vec3 lerp - linear interpolatetemplate<typename T, glm::precision P>glm::tvec3<T, P> lerp(const glm::tvec3<T, P>& x, const glm::tvec3<T, P>& y, T a) {    return x * (T(1) - a) + (y * a);} // vec4 lerp - linear interpolatetemplate<typename T, glm::precision P>glm::tvec4<T, P> lerp(const glm::tvec4<T, P>& x, const glm::tvec4<T, P>& y, T a) {    return x * (T(1) - a) + (y * a);} glm::mat4 createMatFromQuatAndPos(const glm::quat& q, const glm::vec3& p);glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat& rot, const glm::vec3& trans);glm::mat4 createMatFromScale(const glm::vec3& scale);glm::quat cancelOutRoll(const glm::quat& q);glm::quat cancelOutRollAndPitch(const glm::quat& q);glm::mat4 cancelOutRollAndPitch(const glm::mat4& m);glm::vec3 transformPoint(const glm::mat4& m, const glm::vec3& p);glm::vec3 transformVectorFast(const glm::mat4& m, const glm::vec3& v);glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v); // Calculate an orthogonal basis from a primary and secondary axis.// The uAxis, vAxis & wAxis will form an orthognal basis.// The primary axis will be the uAxis.// The vAxis will be as close as possible to to the secondary axis.void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& secondaryAxis,                          glm::vec3& uAxisOut, glm::vec3& vAxisOut, glm::vec3& wAxisOut); // assumes z-forward and y-upglm::vec2 getFacingDir2D(const glm::quat& rot); // assumes z-forward and y-upglm::vec2 getFacingDir2D(const glm::mat4& m); inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }inline bool isNaN(const glm::mat3& value) { return isNaN(value * glm::vec3(1.0f)); } glm::mat4 orthoInverse(const glm::mat4& m); //  Return a random vector of average length 1glm::vec3 randVector(); bool isNonUniformScale(const glm::vec3& scale); //// Safe replacement of glm_mat4_mul() for unaligned arguments instead of __m128//inline void glm_mat4u_mul(const glm::mat4& m1, const glm::mat4& m2, glm::mat4& r) { #if GLM_ARCH & GLM_ARCH_SSE2_BIT    __m128 u0 = _mm_loadu_ps((float*)&m1[0][0]);    __m128 u1 = _mm_loadu_ps((float*)&m1[1][0]);    __m128 u2 = _mm_loadu_ps((float*)&m1[2][0]);    __m128 u3 = _mm_loadu_ps((float*)&m1[3][0]);     __m128 v0 = _mm_loadu_ps((float*)&m2[0][0]);    __m128 v1 = _mm_loadu_ps((float*)&m2[1][0]);    __m128 v2 = _mm_loadu_ps((float*)&m2[2][0]);    __m128 v3 = _mm_loadu_ps((float*)&m2[3][0]);     __m128 t0 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(0,0,0,0)), u0);    __m128 t1 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(1,1,1,1)), u1);    __m128 t2 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(2,2,2,2)), u2);    __m128 t3 = _mm_mul_ps(_mm_shuffle_ps(v0, v0, _MM_SHUFFLE(3,3,3,3)), u3);    v0 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));     t0 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(0,0,0,0)), u0);    t1 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(1,1,1,1)), u1);    t2 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(2,2,2,2)), u2);    t3 = _mm_mul_ps(_mm_shuffle_ps(v1, v1, _MM_SHUFFLE(3,3,3,3)), u3);    v1 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));     t0 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(0,0,0,0)), u0);    t1 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(1,1,1,1)), u1);    t2 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(2,2,2,2)), u2);    t3 = _mm_mul_ps(_mm_shuffle_ps(v2, v2, _MM_SHUFFLE(3,3,3,3)), u3);    v2 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));     t0 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(0,0,0,0)), u0);    t1 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(1,1,1,1)), u1);    t2 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(2,2,2,2)), u2);    t3 = _mm_mul_ps(_mm_shuffle_ps(v3, v3, _MM_SHUFFLE(3,3,3,3)), u3);    v3 = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));     _mm_storeu_ps((float*)&r[0][0], v0);    _mm_storeu_ps((float*)&r[1][0], v1);    _mm_storeu_ps((float*)&r[2][0], v2);    _mm_storeu_ps((float*)&r[3][0], v3);#else    r = m1 * m2;#endif} //// Fast replacement of glm::packSnorm3x10_1x2()// The SSE2 version quantizes using round to nearest even.// The glm version quantizes using round away from zero.//inline uint32_t glm_packSnorm3x10_1x2(vec4 const& v) {     union i10i10i10i2 {        struct {            int x : 10;            int y : 10;            int z : 10;            int w : 2;        } data;        uint32_t pack;    } Result; #if GLM_ARCH & GLM_ARCH_SSE2_BIT    __m128 vclamp = _mm_min_ps(_mm_max_ps(_mm_loadu_ps((float*)&v[0]), _mm_set1_ps(-1.0f)), _mm_set1_ps(1.0f));    __m128i vpack = _mm_cvtps_epi32(_mm_mul_ps(vclamp, _mm_setr_ps(511.f, 511.f, 511.f, 1.f)));     Result.data.x = _mm_cvtsi128_si32(vpack);    Result.data.y = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(1,1,1,1)));    Result.data.z = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(2,2,2,2)));    Result.data.w = _mm_cvtsi128_si32(_mm_shuffle_epi32(vpack, _MM_SHUFFLE(3,3,3,3)));#else    ivec4 const Pack(round(clamp(v, -1.0f, 1.0f) * vec4(511.f, 511.f, 511.f, 1.f)));     Result.data.x = Pack.x;    Result.data.y = Pack.y;    Result.data.z = Pack.z;    Result.data.w = Pack.w;#endif    return Result.pack;} // convert float to int, using round-to-nearest-even (undefined on overflow)inline int fastLrintf(float x) {#if GLM_ARCH & GLM_ARCH_SSE2_BIT    return _mm_cvt_ss2si(_mm_set_ss(x));#else    // return lrintf(x);    static_assert(std::numeric_limits<double>::is_iec559, "Requires IEEE-754 double precision format");    union { double d; int64_t i; } bits = { (double)x };    bits.d += (3ULL << 51);    return (int)bits.i;#endif} // returns the FOV from the projection matrixinline glm::vec4 extractFov( const glm::mat4& m) {    static const std::array<glm::vec4, 4> CLIPS{ {                                                { 1, 0, 0, 1 },                                                { -1, 0, 0, 1 },                                                { 0, 1, 0, 1 },                                                { 0, -1, 0, 1 }                                            } };     glm::mat4 mt = glm::transpose(m);    glm::vec4 v, result;    // Left    v = mt * CLIPS[0];    result.x = -atanf(v.z / v.x);    // Right    v = mt * CLIPS[1];    result.y = atanf(v.z / v.x);    // Down    v = mt * CLIPS[2];    result.z = -atanf(v.z / v.y);    // Up    v = mt * CLIPS[3];    result.w = atanf(v.z / v.y);    return result;} #endif // hifi_GLMHelpers_h