List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getY
public double getY()
From source file:ch.epfl.leb.sass.models.illuminations.internal.SquareUniformElectricField.java
/** * Creates a new instance of this command. * /*from w ww. j a v a2 s .co m*/ * Any z-component of the E-field orientation will be ignored since this * class propagates along the +z direction by default. * * @param width The extent of the field in x from 0 to width. * @param height The extend of the field in y from 0 to height. * @param orientation The orientation of the electric field vector. * @param wavelength The wavelength of the radiation. * @param refractiveIndex The sample's refractive index distribution. */ private SquareUniformElectricField(double width, double height, Vector3D orientation, double wavelength, RefractiveIndex refractiveIndex) { this.width = width; this.height = height; this.wavelength = wavelength; this.refractiveIndex = refractiveIndex; // Ignore any z-component of the field. this.orientation = (new Vector3D(orientation.getX(), orientation.getY(), 0)).normalize(); }
From source file:com.mapr.synth.drive.DriveTest.java
@Test public void testPlanning() throws FileNotFoundException { Random rand = new Random(3); GeoPoint start = new GeoPoint((rand.nextDouble() - 0.5) * Math.PI / 2, rand.nextDouble() * Math.PI * 2); GeoPoint end = start.nearby(10, rand); Vector3D east = start.east(); Vector3D north = start.north(east); try (PrintWriter out = new PrintWriter(new File("short-drive.csv"))) { out.printf("i, highway, x0, y0, x1, y1"); double highway = 0; int n = 0; for (int i = 0; i < 50; i++) { List<Car.Segment> plan = Car.plan(start, end, rand); assertTrue(plan.size() < 20); Vector3D here = project(east, north, start.as3D()); for (Car.Segment segment : plan) { Vector3D step = project(east, north, segment.getEnd().as3D()); n++;// w w w . j a v a2 s .c om if (segment instanceof Car.Highway) { highway++; } out.printf("%d, %d, %.4f, %.4f, %.4f, %.4f\n", i, (segment instanceof Car.Local) ? 1 : 0, here.getX(), here.getY(), step.getX(), step.getY()); here = step; } // should arrive at our destination! assertEquals(0.0, here.subtract(project(east, north, end.as3D())).getNorm(), 0.01); } // most steps should be local, not highway assertEquals(0.0, highway / n, 0.1); } }
From source file:Engine.Projectile.java
public Projectile(Vector3D[] forces, Vector3D size, Vector3D intialPosition, Vector3D velocities[], boolean affectedByGravity, Screen screen, IProjectileFunction projectileFunction, double magicMultiplayer, Object[] ownserId) { this.ownerId = ownserId; this.magicMultiplayer = magicMultiplayer; this.intialPosition = new Vector3D(intialPosition.getX(), intialPosition.getY(), intialPosition.getZ()); this.actualPosition = new Vector3D(intialPosition.getX(), intialPosition.getY(), intialPosition.getZ()); //this. boundingSphere = new BoundingSphere(actualPosition.getX(),actualPosition.getY(), actualPosition.getZ(), Math.max(Math.max(size.getX(), size.getY()), size.getZ())); this.boundingBox = new BoundingBox(intialPosition.getX() - (size.getX() / 2.0), intialPosition.getY() - (size.getY() / 2.0), intialPosition.getZ() - (size.getZ() / 2.0), intialPosition.getX() + (size.getX() / 2.0), intialPosition.getY() + (size.getY() / 2.0), intialPosition.getZ() + (size.getZ() / 2.0), true); this.screen = screen; this.size = size; this.projectileFunction = projectileFunction; if (velocities != null) { this.velocities = new Vector3D[velocities.length]; for (int a = 0; a < velocities.length; a++) this.velocities[a] = new Vector3D(velocities[a].getX(), velocities[a].getY(), velocities[a].getZ()); }/*from w ww . jav a 2 s. c om*/ int forcesCount = 0; if (forces != null) { forcesCount += forces.length; if (affectedByGravity) forcesCount++; this.forces = new Vector3D[forcesCount]; for (int a = 0; a < forces.length; a++) this.forces[a] = new Vector3D(forces[a].getX(), forces[a].getY(), forces[a].getZ()); if (affectedByGravity) this.forces[forcesCount - 1] = new Vector3D(gravity.getX(), gravity.getY(), gravity.getZ()); } else { if (affectedByGravity) { forcesCount++; this.forces = new Vector3D[forcesCount]; this.forces[forcesCount - 1] = new Vector3D(gravity.getX(), gravity.getY(), gravity.getZ()); } } }
From source file:edu.mit.fss.tutorial.part4.ControlPanel.java
/** * Helper function to get the screen location for a given element position. * * @param position the position//from www. j a va 2s . c o m * @return the screen location */ private int[] getScreenLocation(Vector3D position) { // Convert X-position to pixels using display scale, and offset by half // the screen width (i.e. X=0 is at the center of screen). int x = (int) Math.round(getWidth() / 2 + position.getX() / DISPLAY_SCALE); // Convert Y-position to pixels using display scale, and offset by half // the screen height (i.e. Y=0 is at the middle of screen). int y = (int) Math.round(getHeight() / 2 - position.getY() / DISPLAY_SCALE); // Return X- and Y-locations on screen. return new int[] { x, y }; }
From source file:lambertmrev.Lambert.java
/** Constructs and solves a Lambert problem. * * \param[in] R1 first cartesian position * \param[in] R2 second cartesian position * \param[in] tof time of flight/*w ww . jav a 2 s .com*/ * \param[in] mu gravity parameter * \param[in] cw when 1 a retrograde orbit is assumed * \param[in] multi_revs maximum number of multirevolutions to compute */ public void lambert_problem(Vector3D r1, Vector3D r2, double tof, double mu, Boolean cw, int multi_revs) { // sanity checks if (tof <= 0) { System.out.println("ToF is negative! \n"); } if (mu <= 0) { System.out.println("mu is below zero"); } // 1 - getting lambda and T double m_c = FastMath.sqrt((r2.getX() - r1.getX()) * (r2.getX() - r1.getX()) + (r2.getY() - r1.getY()) * (r2.getY() - r1.getY()) + (r2.getZ() - r1.getZ()) * (r2.getZ() - r1.getZ())); double R1 = r1.getNorm(); double R2 = r2.getNorm(); double m_s = (m_c + R1 + R2) / 2.0; Vector3D ir1 = r1.normalize(); Vector3D ir2 = r2.normalize(); Vector3D ih = Vector3D.crossProduct(ir1, ir2); ih = ih.normalize(); if (ih.getZ() == 0) { System.out.println("angular momentum vector has no z component \n"); } double lambda2 = 1.0 - m_c / m_s; double m_lambda = FastMath.sqrt(lambda2); Vector3D it1 = new Vector3D(0.0, 0.0, 0.0); Vector3D it2 = new Vector3D(0.0, 0.0, 0.0); if (ih.getZ() < 0.0) { // Transfer angle is larger than 180 degrees as seen from abive the z axis m_lambda = -m_lambda; it1 = Vector3D.crossProduct(ir1, ih); it2 = Vector3D.crossProduct(ir2, ih); } else { it1 = Vector3D.crossProduct(ih, ir1); it2 = Vector3D.crossProduct(ih, ir2); } it1.normalize(); it2.normalize(); if (cw) { // Retrograde motion m_lambda = -m_lambda; it1.negate(); it2.negate(); } double lambda3 = m_lambda * lambda2; double T = FastMath.sqrt(2.0 * mu / m_s / m_s / m_s) * tof; // 2 - We now hava lambda, T and we will find all x // 2.1 - let us first detect the maximum number of revolutions for which there exists a solution int m_Nmax = FastMath.toIntExact(FastMath.round(T / FastMath.PI)); double T00 = FastMath.acos(m_lambda) + m_lambda * FastMath.sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * FastMath.PI); double T1 = 2.0 / 3.0 * (1.0 - lambda3); double DT = 0.0; double DDT = 0.0; double DDDT = 0.0; if (m_Nmax > 0) { if (T < T0) { // We use Halley iterations to find xM and TM int it = 0; double err = 1.0; double T_min = T0; double x_old = 0.0, x_new = 0.0; while (true) { ArrayRealVector deriv = dTdx(x_old, T_min, m_lambda); DT = deriv.getEntry(0); DDT = deriv.getEntry(1); DDDT = deriv.getEntry(2); if (DT != 0.0) { x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); } err = FastMath.abs(x_old - x_new); if ((err < 1e-13) || (it > 12)) { break; } tof = x2tof(x_new, m_Nmax, m_lambda); x_old = x_new; it++; } if (T_min > T) { m_Nmax -= 1; } } } // We exit this if clause with Mmax being the maximum number of revolutions // for which there exists a solution. We crop it to multi_revs m_Nmax = FastMath.min(multi_revs, m_Nmax); // 2.2 We now allocate the memory for the output variables m_v1 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_v2 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_iters = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); //RealMatrix m_x = MatrixUtils.createRealMatrix(m_Nmax*2+1, 3); ArrayRealVector m_x = new ArrayRealVector(m_Nmax * 2 + 1); // 3 - We may now find all solution in x,y // 3.1 0 rev solution // 3.1.1 initial guess if (T >= T00) { m_x.setEntry(0, -(T - T00) / (T - T00 + 4)); } else if (T <= T1) { m_x.setEntry(0, T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1); } else { m_x.setEntry(0, FastMath.pow((T / T00), 0.69314718055994529 / FastMath.log(T1 / T00)) - 1.0); } // 3.1.2 Householder iterations //m_iters.setEntry(0, 0, housOutTmp.getEntry(0)); m_x.setEntry(0, householder(T, m_x.getEntry(0), 0, 1e-5, 15, m_lambda)); // 3.2 multi rev solutions double tmp; double x0; for (int i = 1; i < m_Nmax + 1; i++) { // 3.2.1 left householder iterations tmp = FastMath.pow((i * FastMath.PI + FastMath.PI) / (8.0 * T), 2.0 / 3.0); m_x.setEntry(2 * i - 1, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i - 1), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i - 1, x0); //m_iters.setEntry(2*i-1, 0, housOutTmp.getEntry(0)); //3.2.1 right Householder iterations tmp = FastMath.pow((8.0 * T) / (i * FastMath.PI), 2.0 / 3.0); m_x.setEntry(2 * i, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i, x0); //m_iters.setEntry(2*i, 0, housOutTmp.getEntry(0)); } // 4 - For each found x value we recontruct the terminal velocities double gamma = FastMath.sqrt(mu * m_s / 2.0); double rho = (R1 - R2) / m_c; double sigma = FastMath.sqrt(1 - rho * rho); double vr1, vt1, vr2, vt2, y; ArrayRealVector ir1_vec = new ArrayRealVector(3); ArrayRealVector ir2_vec = new ArrayRealVector(3); ArrayRealVector it1_vec = new ArrayRealVector(3); ArrayRealVector it2_vec = new ArrayRealVector(3); // set Vector3D values to a mutable type ir1_vec.setEntry(0, ir1.getX()); ir1_vec.setEntry(1, ir1.getY()); ir1_vec.setEntry(2, ir1.getZ()); ir2_vec.setEntry(0, ir2.getX()); ir2_vec.setEntry(1, ir2.getY()); ir2_vec.setEntry(2, ir2.getZ()); it1_vec.setEntry(0, it1.getX()); it1_vec.setEntry(1, it1.getY()); it1_vec.setEntry(2, it1.getZ()); it2_vec.setEntry(0, it2.getX()); it2_vec.setEntry(1, it2.getY()); it2_vec.setEntry(2, it2.getZ()); for (int i = 0; i < m_x.getDimension(); i++) { y = FastMath.sqrt(1.0 - lambda2 + lambda2 * m_x.getEntry(i) * m_x.getEntry(i)); vr1 = gamma * ((m_lambda * y - m_x.getEntry(i)) - rho * (m_lambda * y + m_x.getEntry(i))) / R1; vr2 = -gamma * ((m_lambda * y - m_x.getEntry(i)) + rho * (m_lambda * y + m_x.getEntry(i))) / R2; double vt = gamma * sigma * (y + m_lambda * m_x.getEntry(i)); vt1 = vt / R1; vt2 = vt / R2; for (int j = 0; j < 3; ++j) m_v1.setEntry(i, j, vr1 * ir1_vec.getEntry(j) + vt1 * it1_vec.getEntry(j)); for (int j = 0; j < 3; ++j) m_v2.setEntry(i, j, vr2 * ir2_vec.getEntry(j) + vt2 * it2_vec.getEntry(j)); } }
From source file:msi.gama.metamodel.shape.GamaShape.java
/** * Same as above, but applies a (optional) rotation along a given vector and * (optional) translation to the geometry * // w w w .ja va2 s . co m * @param source * cannot be null * @param geom * can be null * @param rotation * can be null, expressed in degrees * @param newLocation * can be null */ public GamaShape(final IShape source, final Geometry geom, final Double rotation, final GamaPoint vector, final ILocation newLocation) { this(source, geom); if (!isPoint() && rotation != null) { if (vector == null) { final Coordinate c = geometry.getCentroid().getCoordinate(); geometry.apply(AffineTransform3D.createRotationOz(FastMath.toRadians(rotation), c.x, c.y)); } else { final Vector3D v3D = new Vector3D(vector.getX(), vector.getY(), vector.getZ()); final Rotation rot = new Rotation(v3D, FastMath.toRadians(rotation)); for (final Coordinate c : this.getInnerGeometry().getCoordinates()) { final Vector3D result = rot.applyTo(new Vector3D(c.x, c.y, c.z)); c.x = result.getX(); c.y = result.getY(); c.z = result.getZ(); } } } if (newLocation != null) { setLocation(newLocation); } }
From source file:gin.melec.Cube.java
/** * Set the planes of the cube./*w w w.j a va2s .c o m*/ */ private void setPlanes() { Vector3D orig = new Vector3D(xMin, yMin, zMin); Vector3D origX = new Vector3D(xMax, yMin, zMin); Vector3D origY = new Vector3D(xMin, yMax, zMin); Vector3D origZ = new Vector3D(xMin, yMin, zMax); Vector3D xZ = new Vector3D(xMax, yMin, zMax); Vector3D xY = new Vector3D(xMax, yMax, zMin); Vector3D yZ = new Vector3D(xMin, yMax, zMax); Vector3D xYZ = new Vector3D(xMax, yMax, zMax); Vector3D constructedVector; constructedVector = new Vector3D((origY.getX() - 100), (origY.getY() + 100), origY.getZ()); frontLeftPlane = new CustomPlane(frontSplit, leftSplit, origY, yZ, constructedVector, 0.001); constructedVector = new Vector3D((xY.getX() - 100), (xY.getY() - 100), xY.getZ()); frontRightPlane = new CustomPlane(frontSplit, rightSplit, xY, xYZ, constructedVector, 0.001); constructedVector = new Vector3D(origY.getX(), (origY.getY() - 100), (origY.getZ() + 100)); frontUpPlane = new CustomPlane(frontSplit, upSplit, origY, xY, constructedVector, 0.001); constructedVector = new Vector3D(yZ.getX(), (yZ.getY() - 100), (yZ.getZ() - 100)); frontDownPlane = new CustomPlane(frontSplit, downSplit, yZ, xYZ, constructedVector, 0.001); constructedVector = new Vector3D((orig.getX() + 100), (orig.getY() + 100), orig.getZ()); backLeftPlane = new CustomPlane(backSplit, leftSplit, orig, origZ, constructedVector, 0.001); constructedVector = new Vector3D((origX.getX() - 100), (origX.getY() + 100), origX.getZ()); backRightPlane = new CustomPlane(backSplit, rightSplit, origX, xZ, constructedVector, 0.001); constructedVector = new Vector3D(orig.getX(), (orig.getY() + 100), (orig.getZ() + 100)); backUpPlane = new CustomPlane(backSplit, upSplit, orig, origX, constructedVector, 0.001); constructedVector = new Vector3D(origZ.getX(), (origZ.getY() + 100), (origZ.getZ() - 100)); backDownPlane = new CustomPlane(backSplit, downSplit, origZ, xZ, constructedVector, 0.001); constructedVector = new Vector3D((orig.getX() + 100), orig.getY(), (orig.getZ() + 100)); leftUpPlane = new CustomPlane(leftSplit, upSplit, orig, origY, constructedVector, 0.001); constructedVector = new Vector3D((origZ.getX() + 100), origZ.getY(), (origZ.getZ() - 100)); leftDownPlane = new CustomPlane(leftSplit, downSplit, origZ, yZ, constructedVector, 0.001); constructedVector = new Vector3D((origX.getX() - 100), origX.getY(), (origX.getZ() + 100)); rightUpPlane = new CustomPlane(rightSplit, upSplit, origX, xY, constructedVector, 0.001); constructedVector = new Vector3D((xZ.getX() - 100), xZ.getY(), (xZ.getZ() - 100)); rightDownPlane = new CustomPlane(rightSplit, downSplit, xZ, xYZ, constructedVector, 0.001); }
From source file:com.mapr.synth.drive.Trails.java
@Override public void draw() { colorMode(RGB, 256);//from w w w .jav a 2 s .c o m if (old != null) { // status displays at the top fill(0xee + dither(5), 0xee + dither(5), 0xff + dither(5), 20); stroke(0, 0, 0, 100); textSize(20F); fill(0, 0, 0, 100); Rect2D faster = drawText(300, 30, "Faster"); Rect2D slower = drawText(300, 60, "Slower"); if (mousePressed) { System.out.printf("%d %d\n", mouseX, mouseY); if (faster.contains(mouseX, mouseY)) { clicks++; if (clicks > 60) { clicks = 60; } System.out.printf("%d\n", clicks); } else if (slower.contains(mouseX, mouseY)) { clicks--; if (clicks < 0) { clicks = 0; } System.out.printf("%d\n", clicks); } } // fill(0xee + dither(5), 0xee + dither(5), 0xff + dither(5), 60); // drawText(250, 50, "%8.1f %8.1f", old.here.getX(), old.here.getY()); // fill(0xee + dither(5), 0xee + dither(5), 0xff + dither(5), 60); // drawText(50, 50, "%8.0f %8.1f", old.car.getRpm(), old.car.getSpeed() / Constants.MPH); speed.display(); rpm.display(); throttle.display(); } if (clicks > 0) { //Fade everything which is drawn if (frameCount % 10 == 0) { // noStroke(); // fill(0xee + dither(5), 0xee + dither(5), 0xfe + dither(5), 3); colorMode(HSB, 100); stroke(0, 0, 0, 100); fill(0F, 0F, 120 + dither(11), 6F); rect(0, 90, width, height - 380); } translate(50, 390); scale(-30F, -30F); started = true; colorMode(HSB, 100); double meanSpeed = 0; noStroke(); fill(25, 100, 80); ellipse(0, 0, 0.3F, 0.3F); fill(0, 100, 80); ellipse(-12, 7, 0.3F, 0.3F); for (int i = 0; !input.isEmpty() && i < clicks; i++) { State state = input.remove(); Vector3D p = state.here; if (old != null) { stroke(0, 0, 0); strokeWeight(.1F); double stepSize = old.here.subtract(p).getNorm(); if (stepSize < 10) { meanSpeed += (old.car.getSpeed() - meanSpeed) * 0.4; speedDistribution.add(meanSpeed); // double hue = speedDistribution.cdf(old.car.getSpeed()); double hue = 100 * Math.pow(old.car.getSpeed() / Constants.MPH, 2) / Math.pow(100, 2); stroke((float) hue, 70, 80); line((float) old.here.getX(), (float) old.here.getY(), (float) p.getX(), (float) p.getY()); } speed.addData((float) (old.car.getSpeed() / Constants.MPH)); rpm.addData((float) (old.car.getRpm())); throttle.addData((float) old.car.getThrottle()); } old = state; } } }
From source file:com.diozero.imu.drivers.invensense.MPU9150DataFactory.java
public static ImuData newInstance(MPU9150FIFOData fifoData, short[] compassData, double gyroScaleFactor, double accelScaleFactor, double compassScaleFactor, double quatScaleFactor, float temperature) { Vector3D gyro = ImuDataFactory.createVector(fifoData.getGyro(), gyroScaleFactor); Vector3D accel = ImuDataFactory.createVector(fifoData.getAccel(), accelScaleFactor); // TODO What is the scale factor for the quaternion data?! Quaternion quaternion = ImuDataFactory.createQuaternion(fifoData.getQuat(), quatScaleFactor); Vector3D compass = ImuDataFactory.createVector(compassData, compassScaleFactor); // From https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/Examples/MPU9150_AHRS.ino // The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally // pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis. // The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors. // The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and // other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record // the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two // should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched // compared to the gyro and accelerometer! // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // From Richards Tech (not sure if this is needed if the orientation has been set correctly by the DMP driver!) // Sort out gyro axes //gyro = new Vector3D(gyro.getX(), -gyro.getY(), -gyro.getZ()); // Sort out accel axes //accel = new Vector3D(-accel.getX(), accel.getY(), accel.getZ()); // Sort out compass axes // TODO Can this be handled by a generic transformation matrix? compass = new Vector3D(compass.getY(), -compass.getX(), -compass.getZ()); long timestamp = fifoData.getTimestamp(); return new ImuData(gyro, accel, quaternion, compass, temperature, timestamp); }
From source file:com.diozero.imu.drivers.invensense.ImuDataFactory.java
public static ImuData newInstance(MPU9150FIFOData fifoData, short[] compassData, double gyroScaleFactor, double accelScaleFactor, double compassScaleFactor, double quatScaleFactor, float temperature) { Vector3D gyro = createVector(fifoData.getGyro(), gyroScaleFactor); Vector3D accel = createVector(fifoData.getAccel(), accelScaleFactor); // TODO What is the scale factor for the quaternion data?! Quaternion quaternion = createQuaternion(fifoData.getQuat(), quatScaleFactor); Vector3D compass = createVector(compassData, compassScaleFactor); // From https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/Examples/MPU9150_AHRS.ino // The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally // pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis. // The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors. // The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and // other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record // the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two // should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched // compared to the gyro and accelerometer! // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // From Richards Tech (not sure if this is needed if the orientation has been set correctly by the DMP driver!) // Sort out gyro axes //gyro = new Vector3D(gyro.getX(), -gyro.getY(), -gyro.getZ()); // Sort out accel axes //accel = new Vector3D(-accel.getX(), accel.getY(), accel.getZ()); // Sort out compass axes // TODO Can this be handled by a generic transformation matrix? compass = new Vector3D(compass.getY(), -compass.getX(), -compass.getZ()); long timestamp = fifoData.getTimestamp(); return new ImuData(gyro, accel, quaternion, compass, temperature, timestamp); }