Added six byte quaternion compression routines & tests

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)

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);

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));
+}

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);