diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index bd8bffefd2..e81958f407 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -135,6 +135,87 @@ int unpackOrientationQuatFromBytes(const unsigned char* buffer, glm::quat& quatO return sizeof(quatParts); } +#define HI_BYTE(x) (uint8_t)(x >> 8) +#define LO_BYTE(x) (uint8_t)(0xff & x) + +int packOrientationQuatToSixBytes(unsigned char* buffer, const glm::quat& quatInput) { + + // find largest component + uint8_t largestComponent = 0; + for (int i = 1; i < 4; i++) { + if (fabs(quatInput[i]) > fabs(quatInput[largestComponent])) { + largestComponent = i; + } + } + + // ensure that the sign of the dropped component is always negative. + glm::quat q = quatInput[largestComponent] > 0 ? -quatInput : quatInput; + + const float MAGNITUDE = 1.0f / sqrt(2.0f); + const uint32_t NUM_BITS_PER_COMPONENT = 15; + const uint32_t RANGE = (1 << NUM_BITS_PER_COMPONENT) - 1; + + // quantize the smallest three components into integers + uint16_t components[3]; + for (int i = 0, j = 0; i < 4; i++) { + if (i != largestComponent) { + // transform component into 0..1 range. + float value = (q[i] + MAGNITUDE) / (2.0f * MAGNITUDE); + + // quantize 0..1 into 0..range + components[j] = (uint16_t)(value * RANGE); + j++; + } + } + + // encode the largestComponent into the high bits of the first two components + components[0] = (0x7fff & components[0]) | ((0x01 & largestComponent) << 15); + components[1] = (0x7fff & components[1]) | ((0x02 & largestComponent) << 14); + + buffer[0] = HI_BYTE(components[0]); + buffer[1] = LO_BYTE(components[0]); + buffer[2] = HI_BYTE(components[1]); + buffer[3] = LO_BYTE(components[1]); + buffer[4] = HI_BYTE(components[2]); + buffer[5] = LO_BYTE(components[2]); + + return 6; +} + +int unpackOrientationQuatFromSixBytes(const unsigned char* buffer, glm::quat& quatOutput) { + + uint16_t components[3]; + components[0] = ((uint16_t)(0x7f & buffer[0]) << 8) | buffer[1]; + components[1] = ((uint16_t)(0x7f & buffer[2]) << 8) | buffer[3]; + components[2] = ((uint16_t)(0x7f & buffer[4]) << 8) | buffer[5]; + + // largestComponent is encoded into the highest bits of the first 2 components + uint8_t largestComponent = ((0x80 & buffer[2]) >> 6) | ((0x80 & buffer[0]) >> 7); + + const uint32_t NUM_BITS_PER_COMPONENT = 15; + const float RANGE = (float)((1 << NUM_BITS_PER_COMPONENT) - 1); + const float MAGNITUDE = 1.0f / sqrtf(2.0f); + float floatComponents[3]; + for (int i = 0; i < 3; i++) { + floatComponents[i] = ((float)components[i] / RANGE) * (2.0f * MAGNITUDE) - MAGNITUDE; + } + + // missingComponent is always negative. + float missingComponent = -sqrtf(1.0f - floatComponents[0] * floatComponents[0] - floatComponents[1] * floatComponents[1] - floatComponents[2] * floatComponents[2]); + + for (int i = 0, j = 0; i < 4; i++) { + if (i != largestComponent) { + quatOutput[i] = floatComponents[j]; + j++; + } else { + quatOutput[i] = missingComponent; + } + } + + return 6; +} + + // Safe version of glm::eulerAngles; uses the factorization method described in David Eberly's // http://www.geometrictools.com/Documentation/EulerAngles.pdf (via Clyde, // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java) diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 8b1446d4e5..ae9ec25195 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -97,6 +97,14 @@ int unpackFloatAngleFromTwoByte(const uint16_t* byteAnglePointer, float* destina int 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 16bits int packFloatRatioToTwoByte(unsigned char* buffer, float ratio); diff --git a/tests/shared/src/GLMHelpersTests.cpp b/tests/shared/src/GLMHelpersTests.cpp index afb634ecbd..a796d62ba5 100644 --- a/tests/shared/src/GLMHelpersTests.cpp +++ b/tests/shared/src/GLMHelpersTests.cpp @@ -54,4 +54,51 @@ void GLMHelpersTests::testEulerDecomposition() { } } +static void testQuatCompression(glm::quat testQuat) { + float MAX_COMPONENT_ERROR = 4.3e-5f; + + glm::quat q; + uint8_t bytes[6]; + packOrientationQuatToSixBytes(bytes, testQuat); + unpackOrientationQuatFromSixBytes(bytes, q); + if (glm::dot(q, testQuat) < 0.0f) { + q = -q; + } + QCOMPARE_WITH_ABS_ERROR(q.x, testQuat.x, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.y, testQuat.y, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.z, testQuat.z, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.w, testQuat.w, MAX_COMPONENT_ERROR); +} + +void GLMHelpersTests::testSixByteOrientationCompression() { + const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f)); + const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + + testQuatCompression(ROT_X_90); + testQuatCompression(ROT_Y_180); + testQuatCompression(ROT_Z_30); + testQuatCompression(ROT_X_90 * ROT_Y_180); + testQuatCompression(ROT_Y_180 * ROT_X_90); + testQuatCompression(ROT_Z_30 * ROT_X_90); + testQuatCompression(ROT_X_90 * ROT_Z_30); + testQuatCompression(ROT_Z_30 * ROT_Y_180); + testQuatCompression(ROT_Y_180 * ROT_Z_30); + testQuatCompression(ROT_X_90 * ROT_Y_180 * ROT_Z_30); + testQuatCompression(ROT_Y_180 * ROT_Z_30 * ROT_X_90); + testQuatCompression(ROT_Z_30 * ROT_X_90 * ROT_Y_180); + + testQuatCompression(-ROT_X_90); + testQuatCompression(-ROT_Y_180); + testQuatCompression(-ROT_Z_30); + testQuatCompression(-(ROT_X_90 * ROT_Y_180)); + testQuatCompression(-(ROT_Y_180 * ROT_X_90)); + testQuatCompression(-(ROT_Z_30 * ROT_X_90)); + testQuatCompression(-(ROT_X_90 * ROT_Z_30)); + testQuatCompression(-(ROT_Z_30 * ROT_Y_180)); + testQuatCompression(-(ROT_Y_180 * ROT_Z_30)); + testQuatCompression(-(ROT_X_90 * ROT_Y_180 * ROT_Z_30)); + testQuatCompression(-(ROT_Y_180 * ROT_Z_30 * ROT_X_90)); + testQuatCompression(-(ROT_Z_30 * ROT_X_90 * ROT_Y_180)); +} diff --git a/tests/shared/src/GLMHelpersTests.h b/tests/shared/src/GLMHelpersTests.h index 5e880899e8..40d552a07b 100644 --- a/tests/shared/src/GLMHelpersTests.h +++ b/tests/shared/src/GLMHelpersTests.h @@ -19,6 +19,7 @@ class GLMHelpersTests : public QObject { Q_OBJECT private slots: void testEulerDecomposition(); + void testSixByteOrientationCompression(); }; float getErrorDifference(const float& a, const float& b);