void RTMath::display(const char *label, RTQuaternion& quat) { Serial.print(label); Serial.print(" scalar:"); Serial.print(quat.scalar()); Serial.print(" x:"); Serial.print(quat.x()); Serial.print(" y:"); Serial.print(quat.y()); Serial.print(" z:"); Serial.print(quat.z()); }
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; }
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 trackerVrpnServer::update_tracking(const RTVector3& position, const RTQuaternion& quaternion) { //Quaternions update d_quat[0] = quaternion.x(); d_quat[1] = quaternion.y(); d_quat[2] = quaternion.z(); d_quat[3] = quaternion.scalar(); //Location update pos[0] = position.x(); pos[1] = position.y(); pos[2] = position.z(); }
void RTVector3::accelToQuaternion(RTQuaternion& qPose) const { RTVector3 normAccel = *this; RTVector3 vec; RTVector3 z(0, 0, 1.0); normAccel.normalize(); RTFLOAT angle = acos(RTVector3::dotProduct(z, normAccel)); RTVector3::crossProduct(normAccel, z, vec); vec.normalize(); qPose.fromAngleVector(angle, vec); }
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); }
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; }
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; }
RTQuaternion& RTQuaternion::operator *=(const RTQuaternion& qb) { RTQuaternion qa; qa = *this; m_data[0] = qa.scalar() * qb.scalar() - qa.x() * qb.x() - qa.y() * qb.y() - qa.z() * qb.z(); m_data[1] = qa.scalar() * qb.x() + qa.x() * qb.scalar() + qa.y() * qb.z() - qa.z() * qb.y(); m_data[2] = qa.scalar() * qb.y() - qa.x() * qb.z() + qa.y() * qb.scalar() + qa.z() * qb.x(); m_data[3] = qa.scalar() * qb.z() + qa.x() * qb.y() - qa.y() * qb.x() + qa.z() * qb.scalar(); return *this; }