Examples with Quaterniond gaiasky.util.math.Quaterniond used on opensource projects

Example 11 with Quaterniond

use of gaiasky.util.math.Quaterniond in project gaiasky by langurmonkey.

the class ConcreteAttitude method getSpinVectorInSrs.

/**
 */
@Override
public Vector3d getSpinVectorInSrs() {
    // Using (A.18) in AGIS paper (A&A 538, A78, 2012):
    Quaterniond tmp = q.cpy();
    tmp.inverse().mul(qDot);
    return new Vector3d(2. * tmp.x, 2. * tmp.y, 2. * tmp.z);
}

Example 12 with Quaterniond

use of gaiasky.util.math.Quaterniond in project gaiasky by langurmonkey.

the class Epsl method getAttitudeNative.

/**
 * @see gaiasky.util.gaia.BaseAttitudeDataServer#getAttitude(long)
 *
 * @param t - the time elapsed since the epoch of J2010 in ns (TCB)
 * @return attitude for the given time
 */
@Override
public synchronized Attitude getAttitudeNative(long t) {
    // Set time to calculate solar longitude and rate of longitude
    nslSun.setTime(t);
    // Calculate the scan phase angle Omega (modulo 4*PI), which has to be 0 at the
    // reference time
    long tElapsed = t - getRefTime();
    long twicePeriod = 2L * getTargetScanPeriod();
    omegaRevs = 2 * (int) (tElapsed / twicePeriod);
    long tRemainder = tElapsed % twicePeriod;
    omega = getOmegaRef() + FOUR_PI * (double) tRemainder / (double) twicePeriod;
    /**
     * SOME AXES NEED TO BE SWAPPED TO ALIGN WITH OUR REF SYS:
     * 	GLOBAL	GAIASANDBOX
     * 	Z -> Y
     * 	X -> Z
     * 	Y -> X
     */
    // Calculate and set the attitude quaternion
    Quaterniond q = new Quaterniond(Z_AXIS, OBLIQUITY_DEG);
    q.mul(new Quaterniond(Y_AXIS, Math.toDegrees(nslSun.getSolarLongitude())));
    q.mul(new Quaterniond(Z_AXIS, Math.toDegrees(super.getNuRef() - PI_HALF)));
    q.mul(new Quaterniond(X_AXIS, Math.toDegrees(PI_HALF - super.getXiRef())));
    q.mul(new Quaterniond(Y_AXIS, Math.toDegrees(omega)));
    ConcreteAttitude att = new ConcreteAttitude(t, q, null, true);
    // Calculate and set the time derivative (rate) of the attitude
    // quaternion [1/day]. First compute the inertial spin vector [rad/day]
    // in ICRS...
    spinVector.set(Epsl.NECLP).scl(nslSun.getSolarLongitudeDot());
    spinVector.scaleAdd(getTargetScanRate() * ARCSEC_PER_S_TO_DEG_PER_DAY, att.getSrsAxes(xyz)[2]);
    // ...then convert to quaternion rate using (A.17) in AGIS paper
    Quaterniond qDot = new Quaterniond(0.5 * spinVector.z, 0.5 * spinVector.x, 0.5 * spinVector.y, 0.0);
    qDot.mul(q);
    att.setQuaternionDot(qDot);
    return att;
}

Example 13 with Quaterniond

use of gaiasky.util.math.Quaterniond in project gaiasky by langurmonkey.

the class MslAttitudeDataServer method initialize.

/**
 * @see gaiasky.util.gaia.HermiteInterpolatedAttitudeDataServer#initialize()
 */
@Override
public void initialize() {
    long tBeg = super.getStartTime();
    long tEnd = super.getStopTime();
    // adjust tBegNs such that the interval since the reference epoch is a
    // multiple of the timeGranularity (the largest such multiple not
    // greater than the original tBeg is chosen):
    long refEpoch = getRefTime();
    long tBegNsFromRef = tBeg - refEpoch;
    tBegNsFromRef = timeGranularity * (tBegNsFromRef / timeGranularity);
    long tBegNs = refEpoch + tBegNsFromRef;
    // adjust tEndNs such that the interval since the reference epoch is a
    // multiple of the timeGranularity (the smallest such multiple not
    // less than the original tEnd is chosen):
    long tEndNsFromRef = tEnd - refEpoch;
    tEndNsFromRef = timeGranularity * ((tEndNsFromRef - 1L) / timeGranularity + 1L);
    long tEndNs = refEpoch + tEndNsFromRef;
    // adjust step to be the largest integer fraction of the timeGranularity
    // less than or equal to the specified maximum step:
    long maxStep = Math.round(maxStepSec * 1e9);
    long part = 1L;
    step = timeGranularity;
    while (step > maxStep || timeGranularity - part * step > 0L) {
        part++;
        step = timeGranularity / part;
    }
    nT = 1 + (int) ((tEndNsFromRef - tBegNsFromRef) / step);
    if (nT < 2) {
        nT = 2;
        step = tEndNsFromRef - tBegNsFromRef;
    }
    // Hermite interpolation
    if (useDefaultStepForIntegrator) {
        stepForIntegrator = step;
    }
    msl.setMaxInternalTimeStep(stepForIntegrator);
    tNs = new long[nT];
    qX = new double[nT];
    qY = new double[nT];
    qZ = new double[nT];
    qW = new double[nT];
    rateX = new double[nT];
    rateY = new double[nT];
    rateZ = new double[nT];
    reducedPrecession = new boolean[nT];
    transitionPrecession = new boolean[nT];
    double[] om = new double[nT];
    tNs[0] = tBegNs;
    for (int i = 1; i < nT - 1; i++) {
        tNs[i] = tNs[0] + i * step;
    }
    tNs[nT - 1] = tEndNs;
    long tNowNs = msl.getGTimeBeg();
    for (int i = 0; i < nT; i++) {
        msl.stepForward(tNs[i] - tNowNs);
        double lSun = msl.getLSun();
        double xi = msl.getXi();
        double nu = msl.getNuMod4Pi();
        om[i] = msl.getOmegaMod4Pi();
        double omega = om[i];
        double lSunDot = msl.getLSunDot();
        double nuDot = msl.getNuDot();
        double omegaDot = msl.getOmegaDot();
        Quaterniond[] qq = AttitudeConverter.heliotropicToQuaternions(lSun, xi, nu, omega, lSunDot, nuDot, omegaDot);
        Quaterniond q = qq[0];
        Quaterniond qInvQDot = qq[1].mulLeftInverse(q);
        qX[i] = q.x;
        qY[i] = q.y;
        qZ[i] = q.z;
        qW[i] = q.w;
        rateX[i] = 2 * qInvQDot.x;
        rateY[i] = 2 * qInvQDot.y;
        rateZ[i] = 2 * qInvQDot.z;
        reducedPrecession[i] = (msl.getStatus() != ModifiedScanningLaw.ScanState.NOMINAL);
        transitionPrecession[i] = (msl.getStatus() == ModifiedScanningLaw.ScanState.TRANSITION);
        tNowNs = tNs[i];
    }
    initialized = true;
}

Example 14 with Quaterniond

use of gaiasky.util.math.Quaterniond in project gaiasky by langurmonkey.

the class Interpolator method qEval.

/**
 * Evaluates the quaternion derivative, value or integral at point tx, using
 * Hermite interpolation in t[], q[], qDot[]. left is such that t[left]
 * <= tx < t[left+1]. Kind = DER returns the derivative at tx, VAL
 * returns the value at tx, and INT returns the integral from t[left] to tx.
 *
 * @param tx
 *            time at which the derivative, value or integral is evaluated
 * @param t
 *            array of times (length >= 2)
 * @param q
 *            array of quaternions
 * @param qDot
 *            array of quaternion derivatives
 * @param left
 *            index in t, q and qDot susch that t[left] <= tx <
 *            t[left+1]
 * @param kind
 *            which kind of result is returned (derivative, value or
 *            integral)
 * @return The quaternion
 */
public static Quaterniond qEval(double tx, double[] t, Quaterniond[] q, Quaterniond[] qDot, int left, Kind kind) {
    double dt = t[left + 1] - t[left];
    double x = (tx - t[left]) / dt;
    double[] p;
    switch(kind) {
        case DER:
            p = Interpolator.interPolDer(x);
            p[0] /= dt;
            p[1] /= dt;
            break;
        case VAL:
            p = Interpolator.interPolVal(x);
            p[2] *= dt;
            p[3] *= dt;
            break;
        case INT:
            p = Interpolator.interPolInt(x);
            p[0] *= dt;
            p[1] *= dt;
            p[2] *= dt * dt;
            p[3] *= dt * dt;
            break;
        default:
            p = null;
            break;
    }
    Quaterniond qx = q[left].cpy().mul(p[0]).mulAdd(q[left + 1], p[1]).mulAdd(qDot[left], p[2]).mulAdd(qDot[left + 1], p[3]);
    return qx;
}

Example 15 with Quaterniond

use of gaiasky.util.math.Quaterniond in project gaiasky by langurmonkey.

the class Gaia method setToLocalTransform.

public void setToLocalTransform(float sizeFactor, Matrix4 localTransform, boolean forceUpdate) {
    if (sizeFactor != 1 || forceUpdate) {
        translation.getMatrix(localTransform).scl(size * sizeFactor);
        if (attitude != null) {
            Quaterniond attQuat = attitude.getQuaternion();
            quaterniond.set(attQuat.x, attQuat.y, attQuat.z, attQuat.w);
            quaternion.set((float) quaterniond.x, (float) quaterniond.y, (float) quaterniond.z, (float) quaterniond.w);
            // Update orientation
            orientation.idt().rotate(quaterniond).rotate(0, 0, 1, 180);
            matauxd.set(localTransform).mul(orientation);
            matauxd.putIn(localTransform);
        }
    } else {
        localTransform.set(this.localTransform);
    }
}