コード例 #1
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());
}
コード例 #2
0
ファイル: RTMath.cpp プロジェクト: bakercp/RTIMULib2-Teensy
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;
}
コード例 #3
0
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);
    }
}
コード例 #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();
}
コード例 #5
0
ファイル: RTMath.cpp プロジェクト: bakercp/RTIMULib2-Teensy
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);
}
コード例 #6
0
ファイル: ViewClient.cpp プロジェクト: robotage/SyntroApps
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);
}
コード例 #7
0
ファイル: RTMath.cpp プロジェクト: bakercp/RTIMULib2-Teensy
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;
}
コード例 #8
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;
}
コード例 #9
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;
}