コード例 #1
0
ファイル: RTMath.cpp プロジェクト: bakercp/RTIMULib2-Teensy
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;
}
コード例 #2
0
ファイル: RTMath.cpp プロジェクト: bakercp/RTIMULib2-Teensy
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;
}
コード例 #3
0
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());
}
コード例 #4
0
ファイル: vrpnServer.cpp プロジェクト: ARMK-embedded/RTIMULib
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();
}