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