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

Example 6 with Quaterniond

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

the class HermiteInterpolatedAttitudeDataServer 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 Attitude getAttitudeNative(final long t) throws RuntimeException {
    int left = AttitudeUtils.findLeftIndexVar(t, tNs, 0);
    if (left < 0 || left > nT - 2) {
        long time = t;
        String msg = "t < tBeg or >= tEnd, t = + " + time + ", tBeg = " + getStartTime() + ", tEnd = " + getStopTime();
        throw new RuntimeException(msg);
    }
    double qXDotL = 0.5 * (qY[left] * rateZ[left] - qZ[left] * rateY[left] + qW[left] * rateX[left]);
    double qYDotL = 0.5 * (-qX[left] * rateZ[left] + qZ[left] * rateX[left] + qW[left] * rateY[left]);
    double qZDotL = 0.5 * (qX[left] * rateY[left] - qY[left] * rateX[left] + qW[left] * rateZ[left]);
    double qWDotL = 0.5 * (-qX[left] * rateX[left] - qY[left] * rateY[left] - qZ[left] * rateZ[left]);
    double qXDotL1 = 0.5 * (qY[left + 1] * rateZ[left + 1] - qZ[left + 1] * rateY[left + 1] + qW[left + 1] * rateX[left + 1]);
    double qYDotL1 = 0.5 * (-qX[left + 1] * rateZ[left + 1] + qZ[left + 1] * rateX[left + 1] + qW[left + 1] * rateY[left + 1]);
    double qZDotL1 = 0.5 * (qX[left + 1] * rateY[left + 1] - qY[left + 1] * rateX[left + 1] + qW[left + 1] * rateZ[left + 1]);
    double qWDotL1 = 0.5 * (-qX[left + 1] * rateX[left + 1] - qY[left + 1] * rateY[left + 1] - qZ[left + 1] * rateZ[left + 1]);
    double timeUnit = 86400e9;
    double x0 = 0.0;
    double x1 = (tNs[left + 1] - tNs[left]) / timeUnit;
    double x = (t - tNs[left]) / timeUnit;
    // HERMITE
    // double intX[] = Interpolator.hermite3(x, x0, qX[left], qXDotL, x1,
    // qX[left + 1], qXDotL1);
    // double intY[] = Interpolator.hermite3(x, x0, qY[left], qYDotL, x1,
    // qY[left + 1], qYDotL1);
    // double intZ[] = Interpolator.hermite3(x, x0, qZ[left], qZDotL, x1,
    // qZ[left + 1], qZDotL1);
    // double intW[] = Interpolator.hermite3(x, x0, qW[left], qWDotL, x1,
    // qW[left + 1], qWDotL1);
    // LINEAR
    double[] intX = Interpolator.linear(x, x0, qX[left], x1, qX[left + 1]);
    double[] intY = Interpolator.linear(x, x0, qY[left], x1, qY[left + 1]);
    double[] intZ = Interpolator.linear(x, x0, qZ[left], x1, qZ[left + 1]);
    double[] intW = Interpolator.linear(x, x0, qW[left], x1, qW[left + 1]);
    Quaterniond qInt = new Quaterniond(intX[0], intY[0], intZ[0], intW[0]);
    double fact = 1.0 / Math.sqrt(qInt.len2());
    return new ConcreteAttitude(t, qInt.nor(), new Quaterniond(intX[1] * fact, intY[1] * fact, intZ[1] * fact, intW[1] * fact), false);
}

Example 7 with Quaterniond

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

the class MslAttitudeDataServer method getAttitudeNative.

/**
 * @see gaiasky.util.gaia.HermiteInterpolatedAttitudeDataServer#getAttitude(long)
 */
public Attitude getAttitudeNative(long t) throws RuntimeException {
    if (!initialized) {
        initialize();
    }
    Attitude att = super.getAttitudeNative(t);
    // modify attitude through post-multiplication by qExtraOmega
    if (extraOmega != 0.0) {
        Quaterniond q = att.getQuaternion();
        Quaterniond qDot = att.getQuaternionDot();
        q.mul(qExtraOmega);
        qDot.mul(qExtraOmega);
        att = new ConcreteAttitude(t, q, qDot, true);
    }
    return att;
}

Example 8 with Quaterniond

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

the class TransitionScanningLaw method getAttitudeNative.

/**
 * @see gaiasky.util.gaia.AnalyticalAttitudeDataServer#getAttitude(long)
 *
 * @param time
 *            - the time elapsed since the epoch of J2010 in ns (TCB)
 * @return attitude for the given time
 */
@Override
public Attitude getAttitudeNative(long time) {
    if (!isInitialized()) {
        initialize();
    }
    // t = time in [days] from tRef
    double t = (time - getRefTime()) * 1e-9 / Nature.D_TO_S;
    double tNorm = t / ramp.asDays();
    // tNorm must by in [-eps, |ramp|+eps] for a valid t
    if (tNorm < -eps || tNorm > 1.0 + eps) {
        throw new RuntimeException("TSL requested for time outside of ramp: t = " + t + " days, ramp = " + ramp.asDays() + " days");
    }
    // nu(t) is calculated for constant acceleration
    double nu = getNuRef() + 0.5 * acc * t * t;
    double nuDot = acc * t;
    // omega(t) is calculated to fourth order
    double omega = om0 + t * (om1 + t * (om2 + t * (om3 + t * om4)));
    double omegaDot = om1 + t * (2 * om2 + t * (3 * om3 + t * 4 * om4));
    NslSun sunDir = getNominalSunVector();
    // the sunDir uses the same reference epoch as this class, so we can pass the ns directly
    sunDir.setTime(time);
    // convert heliotropic angles and rates to quaternion and rate
    Quaterniond[] qr = AttitudeConverter.heliotropicToQuaternions(sunDir.getSolarLongitude(), getXiRef(), nu, omega, sunDir.getSolarLongitudeDot(), nuDot, omegaDot);
    return new ConcreteAttitude(time, qr[0], qr[1], false);
}

Example 9 with Quaterniond

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

the class Interpolator method qHermiteAverage.

/**
 * Static method for computing the average attitude quaternion over a finite
 * time interval ta <= t <= tb, using cubic Hermite interpolation, as
 * well as the average time derivative
 *
 * It is assumed that ta <= tb. If tb-ta is less than dtMin then no
 * average is computed but the instantaneous (interpolated) values at the
 * instant (ta+tb)/2 are returned instead.
 *
 * The times ta, tb, t[] are in [days] from some arbitrary but common
 * origin. Time derivatives are in [1/day].
 *
 * The lengths of the array arguments must be: t.length >= 2, q.length
 * >= t.length, qDot-length >= t.length. No check is made of these
 * conditions.
 *
 * The argument indx is such that t[indx] is not far from ta and tb. It must
 * be in the range 0 <= indx <= t.length-2
 *
 * @param ta
 *            start time of the averaging interval
 * @param tb
 *            end time of the averaging interval
 * @param t
 *            array array of increasing times encompassing the averaging
 *            interval (i.e., t[0] <= ta and tb <= t[t.length-1])
 * @param indx
 *            an index in the array t[] that is a suitable starting point
 *            for locating ta and tb in the array
 * @param q
 *            array of attitude quaternions at times t[]
 * @param qDot
 *            array of attitude quaternion rates [1/timeUnit] at times t[]
 * @return array containing the average attitude quaternion as the first
 *         element and the average attitude quaternion rate [1/timeUnit] as
 *         the second element
 */
public static Quaterniond[] qHermiteAverage(final double ta, final double tb, final double[] t, final int indx, final Quaterniond[] q, final Quaterniond[] qDot) {
    Quaterniond qAve;
    Quaterniond qDotAve;
    if (tb - ta < dtMin) {
        double tm = (ta + tb) / 2;
        int left = getLeftVar(tm, t, indx);
        qAve = Interpolator.qEval(tm, t, q, qDot, left, Kind.VAL);
        qDotAve = Interpolator.qEval(tm, t, q, qDot, left, Kind.DER);
    } else {
        int lefta = getLeftVar(ta, t, indx);
        Quaterniond qa = Interpolator.qEval(ta, t, q, qDot, lefta, Kind.INT);
        int leftb = getLeftVar(tb, t, indx);
        Quaterniond qb = Interpolator.qEval(tb, t, q, qDot, leftb, Kind.INT);
        qAve = qb.mulAdd(qa, -1);
        for (int left = lefta; left < leftb; left++) {
            double dth = (t[left + 1] - t[left]) / 2;
            qAve.mulAdd(q[left], dth).mulAdd(q[left + 1], dth).mulAdd(qDot[left], dth * dth / 3).mulAdd(qDot[left + 1], -dth * dth / 3);
        }
        qAve.mul(1 / (tb - ta));
        qa = Interpolator.qEval(ta, t, q, qDot, lefta, Kind.VAL);
        qb = Interpolator.qEval(tb, t, q, qDot, leftb, Kind.VAL);
        qDotAve = qb.mulAdd(qa, -1).mul(1 / (tb - ta));
    }
    return new Quaterniond[] { qAve, qDotAve };
}

Example 10 with Quaterniond

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

the class GaiaAttitudeWriter method attitudeZAxisToFile.

public static void attitudeZAxisToFile(OutputType type) {
    Calendar iniCal = GregorianCalendar.getInstance();
    // 2014-01-15T15:43:04
    iniCal.set(2014, Calendar.FEBRUARY, 15, 15, 43, 4);
    Date ini = iniCal.getTime();
    // 2019-06-20T06:20:05
    iniCal.set(2019, Calendar.JULY, 20, 5, 40, 0);
    Date end = iniCal.getTime();
    long tini = ini.getTime();
    long tend = end.getTime();
    Vector3d v1 = new Vector3d();
    Vector3d v2 = new Vector3d();
    Vector3d sph1 = new Vector3d();
    Vector3d sph2 = new Vector3d();
    String filenameEcl = "Gaia-ecliptic-Z-" + System.currentTimeMillis() + ".csv";
    String filenameEq = "Gaia-equatorial-Z-" + System.currentTimeMillis() + ".csv";
    File fecl = new File(System.getProperty("java.io.tmpdir") + File.separator + filenameEcl);
    File feq = new File(System.getProperty("java.io.tmpdir") + File.separator + filenameEq);
    try {
        Writer writerEcl = new BufferedWriter(new OutputStreamWriter(new FileOutputStream(fecl), StandardCharsets.UTF_8));
        Writer writerEq = new BufferedWriter(new OutputStreamWriter(new FileOutputStream(feq), StandardCharsets.UTF_8));
        if (type.equals(OutputType.UP_VECTOR)) {
            writerEcl.append("#EclLong[deg], EclLat[deg], t[ms-January_1_1970_00:00:00_GMT], current[date], attitudeFile\n");
            writerEq.append("#alpha[deg], delta[deg], t[ms-January_1_1970_00:00:00_GMT], current[date], attitudeFile\n");
        } else if (type.equals(OutputType.FOV_VECTORS)) {
            writerEcl.append("#EclLong-fov1[deg], EclLat-fov1[deg], EclLong-fov2[deg], EclLat-fov2[deg],t[ms-January_1_1970_00:00:00_GMT], current[date], attitudeFile\n");
            writerEq.append("#alpha-fov1[deg], delta-fov1[deg], alpha-fov2[deg], delta-fov2[deg],t[ms-January_1_1970_00:00:00_GMT], current[date], attitudeFile\n");
        }
        // 10 minutes
        long step = 10 * 60000;
        for (long t = tini; t <= tend; t += step) {
            Date current = new Date(t);
            try {
                Attitude att = GaiaAttitudeServer.instance.getAttitude(current);
                Quaterniond quat = att.getQuaternion();
                if (type.equals(OutputType.UP_VECTOR)) {
                    // Up vector in Gaia coordinates
                    v1.set(0, 1, 0);
                    // Equatorial
                    v1.rotateVectorByQuaternion(quat);
                    Coordinates.cartesianToSpherical(v1, sph1);
                    writerEq.append(String.valueOf(Math.toDegrees(sph1.x))).append(", ").append(String.valueOf(Math.toDegrees(sph1.y))).append(", ").append(String.valueOf(t)).append(", ").append(String.valueOf(current)).append(", ").append(GaiaAttitudeServer.instance.getCurrentAttitudeName()).append("\n");
                    // Ecliptic
                    v1.mul(Coordinates.eqToEcl());
                    Coordinates.cartesianToSpherical(v1, sph1);
                    writerEcl.append(String.valueOf(Math.toDegrees(sph1.x))).append(", ").append(String.valueOf(Math.toDegrees(sph1.y))).append(", ").append(String.valueOf(t)).append(", ").append(String.valueOf(current)).append(", ").append(GaiaAttitudeServer.instance.getCurrentAttitudeName()).append("\n");
                } else if (type.equals(OutputType.FOV_VECTORS)) {
                    // Fov 1 and Fov 2
                    v1.set(0, 0, 1).rotate(BAM_2, 0, 1, 0);
                    v2.set(0, 0, 1).rotate(-BAM_2, 0, 1, 0);
                    // Equatorial
                    v1.rotateVectorByQuaternion(quat);
                    v2.rotateVectorByQuaternion(quat);
                    Coordinates.cartesianToSpherical(v1, sph1);
                    Coordinates.cartesianToSpherical(v2, sph2);
                    writerEq.append(String.valueOf(Math.toDegrees(sph1.x))).append(", ").append(String.valueOf(Math.toDegrees(sph1.y))).append(", ").append(String.valueOf(Math.toDegrees(sph2.x))).append(", ").append(String.valueOf(Math.toDegrees(sph2.y))).append(", ").append(String.valueOf(t)).append(", ").append(String.valueOf(current)).append(", ").append(GaiaAttitudeServer.instance.getCurrentAttitudeName()).append("\n");
                    // Ecliptic
                    v1.mul(Coordinates.eqToEcl());
                    v2.mul(Coordinates.eqToEcl());
                    Coordinates.cartesianToSpherical(v1, sph1);
                    Coordinates.cartesianToSpherical(v2, sph2);
                    writerEcl.append(String.valueOf(Math.toDegrees(sph1.x))).append(", ").append(String.valueOf(Math.toDegrees(sph1.y))).append(", ").append(String.valueOf(Math.toDegrees(sph2.x))).append(", ").append(String.valueOf(Math.toDegrees(sph2.y))).append(", ").append(String.valueOf(t)).append(", ").append(String.valueOf(current)).append(", ").append(GaiaAttitudeServer.instance.getCurrentAttitudeName()).append("\n");
                }
            } catch (Exception e) {
                System.out.println("Error: t=" + current);
            }
        }
        writerEcl.close();
        writerEq.close();
    } catch (IOException e) {
        e.printStackTrace();
    }
}