// read data in packet starting at byte offset and return number of bytes parsed
int AvatarData::parseDataFromBuffer(const QByteArray& buffer) {
// lazily allocate memory for HeadData in case we're not an Avatar instance
if (!_headData) {
_headData = new HeadData(this);
}
// lazily allocate memory for HandData in case we're not an Avatar instance
if (!_handData) {
_handData = new HandData(this);
}
const unsigned char* startPosition = reinterpret_cast<const unsigned char*>(buffer.data());
const unsigned char* sourceBuffer = startPosition;
quint64 now = usecTimestampNow();
// The absolute minimum size of the update data is as follows:
// 50 bytes of "plain old data" {
// position = 12 bytes
// bodyYaw = 2 (compressed float)
// bodyPitch = 2 (compressed float)
// bodyRoll = 2 (compressed float)
// targetScale = 2 (compressed float)
// headPitch = 2 (compressed float)
// headYaw = 2 (compressed float)
// headRoll = 2 (compressed float)
// leanForward = 2 (compressed float)
// leanSideways = 2 (compressed float)
// torsoTwist = 2 (compressed float)
// lookAt = 12
// audioLoudness = 4
// }
// + 1 byte for pupilSize
// + 1 byte for numJoints (0)
// = 51 bytes
int minPossibleSize = 51;
int maxAvailableSize = buffer.size();
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Malformed AvatarData packet at the start; "
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
// this packet is malformed so we report all bytes as consumed
return maxAvailableSize;
}
{ // Body world position, rotation, and scale
// position
glm::vec3 position;
memcpy(&position, sourceBuffer, sizeof(position));
sourceBuffer += sizeof(position);
if (glm::isnan(position.x) || glm::isnan(position.y) || glm::isnan(position.z)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::position; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
setPosition(position);
// rotation (NOTE: This needs to become a quaternion to save two bytes)
float yaw, pitch, roll;
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &yaw);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &pitch);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &roll);
if (glm::isnan(yaw) || glm::isnan(pitch) || glm::isnan(roll)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::yaw,pitch,roll; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
if (_bodyYaw != yaw || _bodyPitch != pitch || _bodyRoll != roll) {
_hasNewJointRotations = true;
_bodyYaw = yaw;
_bodyPitch = pitch;
_bodyRoll = roll;
}
// scale
float scale;
sourceBuffer += unpackFloatRatioFromTwoByte(sourceBuffer, scale);
if (glm::isnan(scale)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::scale; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_targetScale = scale;
} // 20 bytes
{ // Head rotation
//(NOTE: This needs to become a quaternion to save two bytes)
float headYaw, headPitch, headRoll;
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headPitch);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headYaw);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headRoll);
if (glm::isnan(headYaw) || glm::isnan(headPitch) || glm::isnan(headRoll)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::headYaw,headPitch,headRoll; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->setBasePitch(headPitch);
_headData->setBaseYaw(headYaw);
_headData->setBaseRoll(headRoll);
} // 6 bytes
{ // Head lean (relative to pelvis)
float leanForward, leanSideways, torsoTwist;
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &leanForward);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &leanSideways);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &torsoTwist);
if (glm::isnan(leanForward) || glm::isnan(leanSideways)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::leanForward,leanSideways,torsoTwise; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_leanForward = leanForward;
_headData->_leanSideways = leanSideways;
_headData->_torsoTwist = torsoTwist;
} // 6 bytes
{ // Lookat Position
glm::vec3 lookAt;
memcpy(&lookAt, sourceBuffer, sizeof(lookAt));
sourceBuffer += sizeof(lookAt);
if (glm::isnan(lookAt.x) || glm::isnan(lookAt.y) || glm::isnan(lookAt.z)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::lookAt; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_lookAtPosition = lookAt;
} // 12 bytes
{ // AudioLoudness
// Instantaneous audio loudness (used to drive facial animation)
float audioLoudness;
memcpy(&audioLoudness, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
if (glm::isnan(audioLoudness)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::audioLoudness; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_audioLoudness = audioLoudness;
} // 4 bytes
{ // bitFlags and face data
unsigned char bitItems = *sourceBuffer++;
// key state, stored as a semi-nibble in the bitItems
_keyState = (KeyState)getSemiNibbleAt(bitItems,KEY_STATE_START_BIT);
// hand state, stored as a semi-nibble plus a bit in the bitItems
// we store the hand state as well as other items in a shared bitset. The hand state is an octal, but is split
// into two sections to maintain backward compatibility. The bits are ordered as such (0-7 left to right).
// +---+-----+-----+--+
// |x,x|H0,H1|x,x,x|H2|
// +---+-----+-----+--+
// Hand state - H0,H1,H2 is found in the 3rd, 4th, and 8th bits
_handState = getSemiNibbleAt(bitItems, HAND_STATE_START_BIT)
+ (oneAtBit(bitItems, HAND_STATE_FINGER_POINTING_BIT) ? IS_FINGER_POINTING_FLAG : 0);
_headData->_isFaceTrackerConnected = oneAtBit(bitItems, IS_FACESHIFT_CONNECTED);
bool hasReferential = oneAtBit(bitItems, HAS_REFERENTIAL);
// Referential
if (hasReferential) {
Referential* ref = new Referential(sourceBuffer, this);
if (_referential == NULL ||
ref->version() != _referential->version()) {
changeReferential(ref);
} else {
delete ref;
}
_referential->update();
} else if (_referential != NULL) {
changeReferential(NULL);
}
if (_headData->_isFaceTrackerConnected) {
float leftEyeBlink, rightEyeBlink, averageLoudness, browAudioLift;
minPossibleSize += sizeof(leftEyeBlink) + sizeof(rightEyeBlink) + sizeof(averageLoudness) + sizeof(browAudioLift);
minPossibleSize++; // one byte for blendDataSize
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Malformed AvatarData packet after BitItems;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
// unpack face data
memcpy(&leftEyeBlink, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&rightEyeBlink, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&averageLoudness, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&browAudioLift, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
if (glm::isnan(leftEyeBlink) || glm::isnan(rightEyeBlink)
|| glm::isnan(averageLoudness) || glm::isnan(browAudioLift)) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Discard nan AvatarData::faceData; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_leftEyeBlink = leftEyeBlink;
_headData->_rightEyeBlink = rightEyeBlink;
_headData->_averageLoudness = averageLoudness;
_headData->_browAudioLift = browAudioLift;
int numCoefficients = (int)(*sourceBuffer++);
int blendDataSize = numCoefficients * sizeof(float);
minPossibleSize += blendDataSize;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Malformed AvatarData packet after Blendshapes;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
_headData->_blendshapeCoefficients.resize(numCoefficients);
memcpy(_headData->_blendshapeCoefficients.data(), sourceBuffer, blendDataSize);
sourceBuffer += numCoefficients * sizeof(float);
//bitItemsDataSize = 4 * sizeof(float) + 1 + blendDataSize;
}
} // 1 + bitItemsDataSize bytes
{ // pupil dilation
sourceBuffer += unpackFloatFromByte(sourceBuffer, _headData->_pupilDilation, 1.0f);
} // 1 byte
// joint data
int numJoints = *sourceBuffer++;
int bytesOfValidity = (int)ceil((float)numJoints / (float)BITS_IN_BYTE);
minPossibleSize += bytesOfValidity;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Malformed AvatarData packet after JointValidityBits;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
int numValidJoints = 0;
_jointData.resize(numJoints);
{ // validity bits
unsigned char validity = 0;
int validityBit = 0;
for (int i = 0; i < numJoints; i++) {
if (validityBit == 0) {
validity = *sourceBuffer++;
}
bool valid = (bool)(validity & (1 << validityBit));
if (valid) {
++numValidJoints;
}
_jointData[i].valid = valid;
validityBit = (validityBit + 1) % BITS_IN_BYTE;
}
}
// 1 + bytesOfValidity bytes
// each joint rotation component is stored in two bytes (sizeof(uint16_t))
int COMPONENTS_PER_QUATERNION = 4;
minPossibleSize += numValidJoints * COMPONENTS_PER_QUATERNION * sizeof(uint16_t);
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qCDebug(avatars) << "Malformed AvatarData packet after JointData;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
{ // joint data
for (int i = 0; i < numJoints; i++) {
JointData& data = _jointData[i];
if (data.valid) {
_hasNewJointRotations = true;
sourceBuffer += unpackOrientationQuatFromBytes(sourceBuffer, data.rotation);
}
}
} // numJoints * 8 bytes
int numBytesRead = sourceBuffer - startPosition;
_averageBytesReceived.updateAverage(numBytesRead);
return numBytesRead;
}
// read data in packet starting at byte offset and return number of bytes parsed
int AvatarData::parseDataAtOffset(const QByteArray& packet, int offset) {
// reset the last heard timer since we have new data for this AvatarData
_lastUpdateTimer.restart();
// lazily allocate memory for HeadData in case we're not an Avatar instance
if (!_headData) {
_headData = new HeadData(this);
}
// lazily allocate memory for HandData in case we're not an Avatar instance
if (!_handData) {
_handData = new HandData(this);
}
const unsigned char* startPosition = reinterpret_cast<const unsigned char*>(packet.data()) + offset;
const unsigned char* sourceBuffer = startPosition;
quint64 now = usecTimestampNow();
// The absolute minimum size of the update data is as follows:
// 50 bytes of "plain old data" {
// position = 12 bytes
// bodyYaw = 2 (compressed float)
// bodyPitch = 2 (compressed float)
// bodyRoll = 2 (compressed float)
// targetScale = 2 (compressed float)
// headYaw = 2 (compressed float)
// headPitch = 2 (compressed float)
// headRoll = 2 (compressed float)
// leanSideways = 4
// leanForward = 4
// lookAt = 12
// audioLoudness = 4
// }
// + 1 byte for messageSize (0)
// + 1 byte for pupilSize
// + 1 byte for numJoints (0)
// = 53 bytes
int minPossibleSize = 53;
int maxAvailableSize = packet.size() - offset;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet at the start; "
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
// this packet is malformed so we report all bytes as consumed
return maxAvailableSize;
}
{ // Body world position, rotation, and scale
// position
glm::vec3 position;
memcpy(&position, sourceBuffer, sizeof(position));
sourceBuffer += sizeof(position);
if (glm::isnan(position.x) || glm::isnan(position.y) || glm::isnan(position.z)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::position; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
setPosition(position);
// rotation (NOTE: This needs to become a quaternion to save two bytes)
float yaw, pitch, roll;
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &yaw);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &pitch);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &roll);
if (glm::isnan(yaw) || glm::isnan(pitch) || glm::isnan(roll)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::yaw,pitch,roll; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_bodyYaw = yaw;
_bodyPitch = pitch;
_bodyRoll = roll;
// scale
float scale;
sourceBuffer += unpackFloatRatioFromTwoByte(sourceBuffer, scale);
if (glm::isnan(scale)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::scale; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_targetScale = scale;
} // 20 bytes
{ // Head rotation
//(NOTE: This needs to become a quaternion to save two bytes)
float headYaw, headPitch, headRoll;
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headYaw);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headPitch);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &headRoll);
if (glm::isnan(headYaw) || glm::isnan(headPitch) || glm::isnan(headRoll)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::headYaw,headPitch,headRoll; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->setBaseYaw(headYaw);
_headData->setBasePitch(headPitch);
_headData->setBaseRoll(headRoll);
} // 6 bytes
// Head lean (relative to pelvis)
{
float leanSideways, leanForward;
memcpy(&leanSideways, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&leanForward, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
if (glm::isnan(leanSideways) || glm::isnan(leanForward)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::leanSideways,leanForward; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_leanSideways = leanSideways;
_headData->_leanForward = leanForward;
} // 8 bytes
{ // Lookat Position
glm::vec3 lookAt;
memcpy(&lookAt, sourceBuffer, sizeof(lookAt));
sourceBuffer += sizeof(lookAt);
if (glm::isnan(lookAt.x) || glm::isnan(lookAt.y) || glm::isnan(lookAt.z)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::lookAt; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_lookAtPosition = lookAt;
} // 12 bytes
{ // AudioLoudness
// Instantaneous audio loudness (used to drive facial animation)
float audioLoudness;
memcpy(&audioLoudness, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
if (glm::isnan(audioLoudness)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::audioLoudness; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_audioLoudness = audioLoudness;
} // 4 bytes
// chat
int chatMessageSize = *sourceBuffer++;
minPossibleSize += chatMessageSize;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet before ChatMessage;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
{ // chat payload
_chatMessage = string((char*)sourceBuffer, chatMessageSize);
sourceBuffer += chatMessageSize * sizeof(char);
} // 1 + chatMessageSize bytes
{ // bitFlags and face data
unsigned char bitItems = *sourceBuffer++;
// key state, stored as a semi-nibble in the bitItems
_keyState = (KeyState)getSemiNibbleAt(bitItems,KEY_STATE_START_BIT);
// hand state, stored as a semi-nibble in the bitItems
_handState = getSemiNibbleAt(bitItems,HAND_STATE_START_BIT);
_headData->_isFaceshiftConnected = oneAtBit(bitItems, IS_FACESHIFT_CONNECTED);
_isChatCirclingEnabled = oneAtBit(bitItems, IS_CHAT_CIRCLING_ENABLED);
bool hasReferential = oneAtBit(bitItems, HAS_REFERENTIAL);
// Referential
if (hasReferential) {
Referential* ref = new Referential(sourceBuffer, this);
if (_referential == NULL ||
ref->version() != _referential->version()) {
changeReferential(ref);
} else {
delete ref;
}
_referential->update();
} else if (_referential != NULL) {
changeReferential(NULL);
}
if (_headData->_isFaceshiftConnected) {
float leftEyeBlink, rightEyeBlink, averageLoudness, browAudioLift;
minPossibleSize += sizeof(leftEyeBlink) + sizeof(rightEyeBlink) + sizeof(averageLoudness) + sizeof(browAudioLift);
minPossibleSize++; // one byte for blendDataSize
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet after BitItems;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
// unpack face data
memcpy(&leftEyeBlink, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&rightEyeBlink, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&averageLoudness, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&browAudioLift, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
if (glm::isnan(leftEyeBlink) || glm::isnan(rightEyeBlink)
|| glm::isnan(averageLoudness) || glm::isnan(browAudioLift)) {
if (shouldLogError(now)) {
qDebug() << "Discard nan AvatarData::faceData; displayName = '" << _displayName << "'";
}
return maxAvailableSize;
}
_headData->_leftEyeBlink = leftEyeBlink;
_headData->_rightEyeBlink = rightEyeBlink;
_headData->_averageLoudness = averageLoudness;
_headData->_browAudioLift = browAudioLift;
int numCoefficients = (int)(*sourceBuffer++);
int blendDataSize = numCoefficients * sizeof(float);
minPossibleSize += blendDataSize;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet after Blendshapes;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
_headData->_blendshapeCoefficients.resize(numCoefficients);
memcpy(_headData->_blendshapeCoefficients.data(), sourceBuffer, blendDataSize);
sourceBuffer += numCoefficients * sizeof(float);
//bitItemsDataSize = 4 * sizeof(float) + 1 + blendDataSize;
}
} // 1 + bitItemsDataSize bytes
{ // pupil dilation
sourceBuffer += unpackFloatFromByte(sourceBuffer, _headData->_pupilDilation, 1.0f);
} // 1 byte
// joint data
int numJoints = *sourceBuffer++;
int bytesOfValidity = (int)ceil((float)numJoints / (float)BITS_IN_BYTE);
minPossibleSize += bytesOfValidity;
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet after JointValidityBits;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
int numValidJoints = 0;
_jointData.resize(numJoints);
{ // validity bits
unsigned char validity = 0;
int validityBit = 0;
for (int i = 0; i < numJoints; i++) {
if (validityBit == 0) {
validity = *sourceBuffer++;
}
bool valid = (bool)(validity & (1 << validityBit));
if (valid) {
++numValidJoints;
}
_jointData[i].valid = valid;
validityBit = (validityBit + 1) % BITS_IN_BYTE;
}
}
// 1 + bytesOfValidity bytes
// each joint rotation component is stored in two bytes (sizeof(uint16_t))
int COMPONENTS_PER_QUATERNION = 4;
minPossibleSize += numValidJoints * COMPONENTS_PER_QUATERNION * sizeof(uint16_t);
if (minPossibleSize > maxAvailableSize) {
if (shouldLogError(now)) {
qDebug() << "Malformed AvatarData packet after JointData;"
<< " displayName = '" << _displayName << "'"
<< " minPossibleSize = " << minPossibleSize
<< " maxAvailableSize = " << maxAvailableSize;
}
return maxAvailableSize;
}
{ // joint data
for (int i = 0; i < numJoints; i++) {
JointData& data = _jointData[i];
if (data.valid) {
_hasNewJointRotations = true;
sourceBuffer += unpackOrientationQuatFromBytes(sourceBuffer, data.rotation);
}
}
} // numJoints * 8 bytes
return sourceBuffer - startPosition;
}
