RTVector3 RTMath::poseFromAccelMag(const RTVector3& accel, const RTVector3& mag)
{
RTVector3 result;
RTQuaternion m;
RTQuaternion q;
accel.accelToEuler(result);
// q.fromEuler(result);
// since result.z() is always 0, this can be optimized a little
RTFLOAT cosX2 = cos(result.x() / 2.0f);
RTFLOAT sinX2 = sin(result.x() / 2.0f);
RTFLOAT cosY2 = cos(result.y() / 2.0f);
RTFLOAT sinY2 = sin(result.y() / 2.0f);
q.setScalar(cosX2 * cosY2);
q.setX(sinX2 * cosY2);
q.setY(cosX2 * sinY2);
q.setZ(-sinX2 * sinY2);
// q.normalize();
m.setScalar(0);
m.setX(mag.x());
m.setY(mag.y());
m.setZ(mag.z());
m = q * m * q.conjugate();
result.setZ(-atan2(m.y(), m.x()));
return result;
}
RTQuaternion RTQuaternion::conjugate() const
{
RTQuaternion q;
q.setScalar(m_data[0]);
q.setX(-m_data[1]);
q.setY(-m_data[2]);
q.setZ(-m_data[3]);
return q;
}
const RTQuaternion RTMatrix4x4::operator *(const RTQuaternion& q) const
{
RTQuaternion res;
res.setScalar(m_data[0][0] * q.scalar() + m_data[0][1] * q.x() + m_data[0][2] * q.y() + m_data[0][3] * q.z());
res.setX(m_data[1][0] * q.scalar() + m_data[1][1] * q.x() + m_data[1][2] * q.y() + m_data[1][3] * q.z());
res.setY(m_data[2][0] * q.scalar() + m_data[2][1] * q.x() + m_data[2][2] * q.y() + m_data[2][3] * q.z());
res.setZ(m_data[3][0] * q.scalar() + m_data[3][1] * q.x() + m_data[3][2] * q.y() + m_data[3][3] * q.z());
return res;
}
void RTFusion::calculatePose(const RTVector3& accel, const RTVector3& mag)
{
RTQuaternion m;
RTQuaternion q;
if (m_enableAccel) {
accel.accelToEuler(m_measuredPose);
} else {
m_measuredPose = m_fusionPose;
m_measuredPose.setZ(0);
}
if (m_enableCompass) {
q.fromEuler(m_measuredPose);
m.setScalar(0);
m.setX(mag.x());
m.setY(mag.y());
m.setZ(mag.z());
m = q * m * q.conjugate();
m_measuredPose.setZ(-atan2(m.y(), m.x()));
} else {
m_measuredPose.setZ(m_fusionPose.z());
}
m_measuredQPose.fromEuler(m_measuredPose);
// check for quaternion aliasing. If the quaternion has the wrong sign
// the kalman filter will be very unhappy.
int maxIndex = -1;
RTFLOAT maxVal = -1000;
for (int i = 0; i < 4; i++) {
if (fabs(m_measuredQPose.data(i)) > maxVal) {
maxVal = fabs(m_measuredQPose.data(i));
maxIndex = i;
}
}
// if the biggest component has a different sign in the measured and kalman poses,
// change the sign of the measured pose to match.
if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) ||
((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) {
m_measuredQPose.setScalar(-m_measuredQPose.scalar());
m_measuredQPose.setX(-m_measuredQPose.x());
m_measuredQPose.setY(-m_measuredQPose.y());
m_measuredQPose.setZ(-m_measuredQPose.z());
m_measuredQPose.toEuler(m_measuredPose);
}
}
void ViewClient::appClientReceiveMulticast(int servicePort, SYNTRO_EHEAD *multiCast, int totalLength)
{
SYNTRO_GLOVEDATA *data;
RTQuaternion palmQuat;
RTQuaternion thumbQuat;
RTQuaternion fingerQuat;
SYNTRO_RECORD_HEADER *head = (SYNTRO_RECORD_HEADER *)(multiCast + 1);
int recordType = SyntroUtils::convertUC2ToUInt(head->type);
if (recordType != SYNTRO_RECORD_TYPE_GLOVE) {
qDebug() << "Expecting nav record, received record type" << recordType;
} else {
data = (SYNTRO_GLOVEDATA *)(head + 1);
while (totalLength > (int)sizeof(SYNTRO_GLOVEDATA)) {
palmQuat.setScalar(data->fusionQPosePalm[0]);
palmQuat.setX(data->fusionQPosePalm[1]);
palmQuat.setY(data->fusionQPosePalm[2]);
palmQuat.setZ(data->fusionQPosePalm[3]);
thumbQuat.setScalar(data->fusionQPoseThumb[0]);
thumbQuat.setX(data->fusionQPoseThumb[1]);
thumbQuat.setY(data->fusionQPoseThumb[2]);
thumbQuat.setZ(data->fusionQPoseThumb[3]);
fingerQuat.setScalar(data->fusionQPoseFinger[0]);
fingerQuat.setX(data->fusionQPoseFinger[1]);
fingerQuat.setY(data->fusionQPoseFinger[2]);
fingerQuat.setZ(data->fusionQPoseFinger[3]);
emit newIMUData(palmQuat, thumbQuat, fingerQuat);
totalLength -= sizeof(SYNTRO_GLOVEDATA);
data++;
}
}
clientSendMulticastAck(servicePort);
free(multiCast);
}