+void Matrix4x4_FromBonePose6s(matrix4x4_t *m, float originscale, const short *pose6s)
+{
+ float origin[3];
+ float quat[4];
+ origin[0] = pose6s[0] * originscale;
+ origin[1] = pose6s[1] * originscale;
+ origin[2] = pose6s[2] * originscale;
+ quat[0] = pose6s[3] * (1.0f / 32767.0f);
+ quat[1] = pose6s[4] * (1.0f / 32767.0f);
+ quat[2] = pose6s[5] * (1.0f / 32767.0f);
+ quat[3] = 1.0f - (quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]);
+ quat[3] = quat[3] > 0.0f ? -sqrt(quat[3]) : 0.0f;
+ Matrix4x4_FromOriginQuat(m, origin[0], origin[1], origin[2], quat[0], quat[1], quat[2], quat[3]);
+}
+
+void Matrix4x4_ToBonePose6s(const matrix4x4_t *m, float origininvscale, short *pose6s)
+{
+ float origin[3];
+ float quat[4];
+ float s;
+ Matrix4x4_ToOrigin3Quat4Float(m, origin, quat);
+ // normalize quaternion so that it is unit length
+ s = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
+ if (s)
+ {
+ s = 1.0f / sqrt(s);
+ quat[0] *= s;
+ quat[1] *= s;
+ quat[2] *= s;
+ quat[3] *= s;
+ }
+ // use a negative scale on the quat because the above function produces a
+ // positive quat[3] and canonical quaternions have negative quat[3]
+ pose6s[0] = origin[0] * origininvscale;
+ pose6s[1] = origin[1] * origininvscale;
+ pose6s[2] = origin[2] * origininvscale;
+ pose6s[3] = quat[0] * -32767.0f;
+ pose6s[4] = quat[1] * -32767.0f;
+ pose6s[5] = quat[2] * -32767.0f;
+}
+