List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getZ
public double getZ()
From source file:org.micromanager.plugins.magellan.propsandcovariants.LaserPredNet.java
private static boolean isWithinSurace(SurfaceInterpolator surface, Vector3D point) throws InterruptedException { boolean defined = surface.waitForCurentInterpolation().isInterpDefined(point.getX(), point.getY()); if (!defined) { return false; }//from w w w. j a v a 2s. c o m float interpVal = surface.waitForCurentInterpolation().getInterpolatedValue(point.getX(), point.getY()); return point.getZ() > interpVal; }
From source file:org.micromanager.plugins.magellan.surfacesandregions.SurfaceInterpolatorSimple.java
protected void interpolateSurface(LinkedList<Point3d> points) throws InterruptedException { double pixSize = Magellan.getCore().getPixelSizeUm(); //provide interpolator with current list of data points Point_dt triangulationPoints[] = new Point_dt[points.size()]; for (int i = 0; i < points.size(); i++) { triangulationPoints[i] = new Point_dt(points.get(i).x, points.get(i).y, points.get(i).z); }/* www . j a v a 2 s.c o m*/ Delaunay_Triangulation dTri = new Delaunay_Triangulation(triangulationPoints); int maxPixelDimension = (int) (Math.max(boundXMax_ - boundXMin_, boundYMax_ - boundYMin_) / pixSize); //Start with at least 20 interp points and go smaller and smaller until every pixel interped? int pixelsPerInterpPoint = 1; while (maxPixelDimension / (pixelsPerInterpPoint + 1) > 20) { pixelsPerInterpPoint *= 2; } if (Thread.interrupted()) { throw new InterruptedException(); } while (pixelsPerInterpPoint >= MIN_PIXELS_PER_INTERP_POINT) { int numInterpPointsX = (int) (((boundXMax_ - boundXMin_) / pixSize) / pixelsPerInterpPoint); int numInterpPointsY = (int) (((boundYMax_ - boundYMin_) / pixSize) / pixelsPerInterpPoint); double dx = (boundXMax_ - boundXMin_) / (numInterpPointsX - 1); double dy = (boundYMax_ - boundYMin_) / (numInterpPointsY - 1); float[][] interpVals = new float[numInterpPointsY][numInterpPointsX]; float[][] interpNormals = new float[numInterpPointsY][numInterpPointsX]; boolean[][] interpDefined = new boolean[numInterpPointsY][numInterpPointsX]; for (int yInd = 0; yInd < interpVals.length; yInd++) { for (int xInd = 0; xInd < interpVals[0].length; xInd++) { if (Thread.interrupted()) { throw new InterruptedException(); } double xVal = boundXMin_ + dx * xInd; double yVal = boundYMin_ + dy * yInd; boolean inHull = convexHullRegion_ .checkPoint(new Vector2D(xVal, yVal)) == Region.Location.INSIDE; if (inHull) { Triangle_dt tri = dTri.find(new Point_dt(xVal, yVal)); //convert to apache commons coordinates to make a plane Vector3D v1 = new Vector3D(tri.p1().x(), tri.p1().y(), tri.p1().z()); Vector3D v2 = new Vector3D(tri.p2().x(), tri.p2().y(), tri.p2().z()); Vector3D v3 = new Vector3D(tri.p3().x(), tri.p3().y(), tri.p3().z()); Plane plane = new Plane(v1, v2, v3, TOLERANCE); //intersetion of vertical line at these x+y values with plane gives point in plane Vector3D pointInPlane = plane.intersection( new Line(new Vector3D(xVal, yVal, 0), new Vector3D(xVal, yVal, 1), TOLERANCE)); float zVal = (float) pointInPlane.getZ(); interpVals[yInd][xInd] = zVal; float angle = (float) (Vector3D.angle(plane.getNormal(), new Vector3D(0, 0, 1)) / Math.PI * 180.0); interpNormals[yInd][xInd] = angle; interpDefined[yInd][xInd] = true; } else { interpDefined[yInd][xInd] = false; } } } if (Thread.interrupted()) { throw new InterruptedException(); } synchronized (interpolationLock_) { currentInterpolation_ = new SingleResolutionInterpolation(pixelsPerInterpPoint, interpDefined, interpVals, interpNormals, boundXMin_, boundXMax_, boundYMin_, boundYMax_, convexHullRegion_, convexHullVertices_, getPoints()); interpolationLock_.notifyAll(); } // System.gc(); pixelsPerInterpPoint /= 2; } }
From source file:org.micromanager.plugins.magellan.surfacesandregions.SurfaceInterpolatorSimple.java
@Override public float getExtrapolatedValue(double x, double y) { //duplicate points for thread safety final LinkedList<Point3d> points = new LinkedList<Point3d>(points_); //find 3 closest points and calculate value //find closest convex hull vertex final LinkedList<Integer> closestIndices = new LinkedList<Integer>(); final LinkedList<Double> closestDistances = new LinkedList<Double>(); for (int i = 0; i < points.size(); i++) { //get current distance Vector2D vertex = new Vector2D(points.get(i).x, points.get(i).y); double distance = vertex.distance(new Vector2D(x, y)); if (closestDistances.size() < 3) { closestIndices.add(i);// w w w . ja v a 2 s .co m closestDistances.add(distance); } else if (distance < closestDistances.get(2)) { closestIndices.removeLast(); closestDistances.removeLast(); closestIndices.add(i); closestDistances.add(distance); } //sort Collections.sort(closestIndices, new Comparator<Integer>() { public int compare(Integer left, Integer right) { return (new Double(closestDistances.get(closestIndices.indexOf(left)))) .compareTo(closestDistances.get(closestIndices.indexOf(right))); } }); Collections.sort(closestDistances); } Point3d point1 = points.get(closestIndices.get(0)); Point3d point2 = points.get(closestIndices.get(1)); Point3d point3 = points.get(closestIndices.get(2)); Vector3D v1 = new Vector3D(point1.x, point1.y, point1.z); Vector3D v2 = new Vector3D(point2.x, point2.y, point2.z); Vector3D v3 = new Vector3D(point3.x, point3.y, point3.z); Plane plane = new Plane(v1, v2, v3, TOLERANCE); //intersetion of vertical line at these x+y values with plane gives point in plane Vector3D pointInPlane = plane .intersection(new Line(new Vector3D(x, y, 0), new Vector3D(x, y, 1), TOLERANCE)); float zVal = (float) pointInPlane.getZ(); return zVal; }
From source file:org.orekit.attitudes.YawSteeringTest.java
@Test public void testCompensAxis() throws OrekitException { // Attitude laws // ************** // Target pointing attitude provider over satellite nadir at date, without yaw compensation NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape); // Target pointing attitude provider with yaw compensation YawSteering yawCompensLaw = new YawSteering(circOrbit.getFrame(), nadirLaw, CelestialBodyFactory.getSun(), Vector3D.MINUS_I);/*from ww w. j a v a2s .co m*/ // Get attitude rotations from non yaw compensated / yaw compensated laws Rotation rotNoYaw = nadirLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation(); Rotation rotYaw = yawCompensLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation(); // Compose rotations composition Rotation compoRot = rotYaw.applyTo(rotNoYaw.revert()); Vector3D yawAxis = compoRot.getAxis(); // Check axis Assert.assertEquals(0., yawAxis.getX(), Utils.epsilonTest); Assert.assertEquals(0., yawAxis.getY(), Utils.epsilonTest); Assert.assertEquals(1., yawAxis.getZ(), Utils.epsilonTest); }
From source file:org.orekit.bodies.Ellipsoid.java
/** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane. * @param planePoint point belonging to the plane, in the ellipsoid frame * @param planeNormal normal of the plane, in the ellipsoid frame * @return plane section or null if there are no intersections *//* w w w . j av a2 s. c o m*/ public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal) { // we define the points Q in the plane using two free variables and as: // Q = P + u + v // where u and v are two unit vectors belonging to the plane // Q belongs to the 3D ellipsoid so: // (xQ / a) + (yQ / b) + (zQ / c) = 1 // combining both equations, we get: // (xP + 2 xP ( xU + xV) + ( xU + xV)) / a // + (yP + 2 yP ( yU + yV) + ( yU + yV)) / b // + (zP + 2 zP ( zU + zV) + ( zU + zV)) / c // = 1 // which can be rewritten: // + + 2 + 2 + 2 + = 0 // with // = xU / a + yU / b + zU / c > 0 // = xV / a + yV / b + zV / c > 0 // = xU xV / a + yU yV / b + zU zV / c // = xP xU / a + yP yU / b + zP zU / c // = xP xV / a + yP yV / b + zP zV / c // = xP / a + yP / b + zP / c - 1 // this is the equation of a conic (here an ellipse) // Of course, we note that if the point P belongs to the ellipsoid // then = 0 and the equation holds at point P since = 0 and = 0 final Vector3D u = planeNormal.orthogonal(); final Vector3D v = Vector3D.crossProduct(planeNormal, u).normalize(); final double xUOa = u.getX() / a; final double yUOb = u.getY() / b; final double zUOc = u.getZ() / c; final double xVOa = v.getX() / a; final double yVOb = v.getY() / b; final double zVOc = v.getZ() / c; final double xPOa = planePoint.getX() / a; final double yPOb = planePoint.getY() / b; final double zPOc = planePoint.getZ() / c; final double alpha = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc; final double beta = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc; final double gamma = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc); final double delta = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc); final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc); final double zeta = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1); // reduce the general equation + + 2 + 2 + 2 + = 0 // to canonical form (/l) + (/m) = 1 // using a coordinates change // = C + cos - sin // = C + sin + cos // or equivalently // = ( - C) cos + ( - C) sin // = - ( - C) sin + ( - C) cos // C and C are the coordinates of the 2D ellipse center with respect to P // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest) // is the angle of the 2D ellipse axis corresponding to axis with length 2l // choose in order to cancel the coupling term in // expanding the general equation, we get: A + B + C + D + E + F = 0 // with C = 2[( - ) cos sin + (cos - sin)] // hence the term is cancelled when = arctan(t), with t + ( - ) t - = 0 // As the solutions of the quadratic equation obey t?t = -1, they correspond to // angles in quadrature to each other. Selecting one solution or the other simply // exchanges the principal axes. As we don't care about which axis we want as the // first one, we select an arbitrary solution final double tanTheta; if (FastMath.abs(gamma) < Precision.SAFE_MIN) { tanTheta = 0.0; } else { final double bMA = beta - alpha; tanTheta = (bMA >= 0) ? (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) : (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))); } final double tan2 = tanTheta * tanTheta; final double cos2 = 1 / (1 + tan2); final double sin2 = tan2 * cos2; final double cosSin = tanTheta * cos2; final double cos = FastMath.sqrt(cos2); final double sin = tanTheta * cos; // choose C and C in order to cancel the linear terms in and // expanding the general equation, we get: A + B + C + D + E + F = 0 // with D = 2[ ( C + C + ) cos + ( C + C + ) sin] // E = 2[-( C + C + ) sin + ( C + C + ) cos] // can be eliminated by combining the equations // D cos - E sin = 2[ C + C + ] // E cos + D sin = 2[ C + C + ] // hence the terms D and E are both cancelled (regardless of ) when // C = ( - ) / ( - ) // C = ( - ) / ( - ) final double denom = MathArrays.linearCombination(gamma, gamma, -alpha, beta); final double tauC = MathArrays.linearCombination(beta, delta, -gamma, epsilon) / denom; final double nuC = MathArrays.linearCombination(alpha, epsilon, -gamma, delta) / denom; // compute l and m // expanding the general equation, we get: A + B + C + D + E + F = 0 // with A = cos + sin + 2 cos sin // B = sin + cos - 2 cos sin // F = C + C + 2 C C + 2 C + 2 C + // hence we compute directly l = (-F/A) and m = (-F/B) final double twogcs = 2 * gamma * cosSin; final double bigA = alpha * cos2 + beta * sin2 + twogcs; final double bigB = alpha * sin2 + beta * cos2 - twogcs; final double bigF = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC + (beta * nuC + 2 * epsilon) * nuC + zeta; final double l = FastMath.sqrt(-bigF / bigA); final double m = FastMath.sqrt(-bigF / bigB); if (Double.isNaN(l + m)) { // the plane does not intersect the ellipsoid return null; } if (l > m) { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(cos, u, sin, v), new Vector3D(-sin, u, cos, v), l, m, frame); } else { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(sin, u, -cos, v), new Vector3D(cos, u, sin, v), m, l, frame); } }
From source file:org.orekit.bodies.EllipsoidTest.java
private double errorOnEllipsoid(Ellipse ps, Ellipsoid ellipsoid) { double max = 0; for (double theta = 0; theta < 2 * FastMath.PI; theta += 0.1) { Vector3D p = ps.pointAt(theta); double xOa = p.getX() / ellipsoid.getA(); double yOb = p.getY() / ellipsoid.getB(); double zOc = p.getZ() / ellipsoid.getC(); max = FastMath.max(max,// ww w . j ava 2 s. c om FastMath.abs(MathArrays.linearCombination(xOa, xOa, yOb, yOb, zOc, zOc, 1, -1))); } return max; }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public GeodeticPoint getIntersectionPoint(final Line line, final Vector3D close, final Frame frame, final AbsoluteDate date) throws OrekitException { // transform line and close to body frame final Transform frameToBodyFrame = frame.getTransformTo(bodyFrame, date); final Line lineInBodyFrame = frameToBodyFrame.transformLine(line); final Vector3D closeInBodyFrame = frameToBodyFrame.transformPosition(close); final double closeAbscissa = lineInBodyFrame.toSubSpace(closeInBodyFrame).getX(); // compute some miscellaneous variables outside of the loop final Vector3D point = lineInBodyFrame.getOrigin(); final double x = point.getX(); final double y = point.getY(); final double z = point.getZ(); final double z2 = z * z; final double r2 = x * x + y * y; final Vector3D direction = lineInBodyFrame.getDirection(); final double dx = direction.getX(); final double dy = direction.getY(); final double dz = direction.getZ(); final double cz2 = dx * dx + dy * dy; // abscissa of the intersection as a root of a 2nd degree polynomial : // a k^2 - 2 b k + c = 0 final double a = 1.0 - e2 * cz2; final double b = -(g2 * (x * dx + y * dy) + z * dz); final double c = g2 * (r2 - ae2) + z2; final double b2 = b * b; final double ac = a * c; if (b2 < ac) { return null; }//from ww w . j a v a 2s . c o m final double s = FastMath.sqrt(b2 - ac); final double k1 = (b < 0) ? (b - s) / a : c / (b + s); final double k2 = c / (a * k1); // select the right point final double k = (FastMath.abs(k1 - closeAbscissa) < FastMath.abs(k2 - closeAbscissa)) ? k1 : k2; final Vector3D intersection = lineInBodyFrame.toSpace(new Vector1D(k)); final double ix = intersection.getX(); final double iy = intersection.getY(); final double iz = intersection.getZ(); final double lambda = FastMath.atan2(iy, ix); final double phi = FastMath.atan2(iz, g2 * FastMath.sqrt(ix * ix + iy * iy)); return new GeodeticPoint(phi, lambda, 0.0); }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public Vector3D projectToGround(final Vector3D point, final AbsoluteDate date, final Frame frame) throws OrekitException { // transform point to body frame final Transform toBody = frame.getTransformTo(bodyFrame, date); final Vector3D p = toBody.transformPosition(point); final double z = p.getZ(); final double r = FastMath.hypot(p.getX(), p.getY()); // set up the 2D meridian ellipse final Ellipse meridian = new Ellipse(Vector3D.ZERO, new Vector3D(p.getX() / r, p.getY() / r, 0), Vector3D.PLUS_K, getA(), getC(), bodyFrame); // find the closest point in the meridian plane final Vector3D groundPoint = meridian.toSpace(meridian.projectToEllipse(new Vector2D(r, z))); // transform point back to initial frame return toBody.getInverse().transformPosition(groundPoint); }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public TimeStampedPVCoordinates projectToGround(final TimeStampedPVCoordinates pv, final Frame frame) throws OrekitException { // transform point to body frame final Transform toBody = frame.getTransformTo(bodyFrame, pv.getDate()); final TimeStampedPVCoordinates pvInBodyFrame = toBody.transformPVCoordinates(pv); final Vector3D p = pvInBodyFrame.getPosition(); final double r = FastMath.hypot(p.getX(), p.getY()); // set up the 2D ellipse corresponding to first principal curvature along meridian final Vector3D meridian = new Vector3D(p.getX() / r, p.getY() / r, 0); final Ellipse firstPrincipalCurvature = new Ellipse(Vector3D.ZERO, meridian, Vector3D.PLUS_K, getA(), getC(), bodyFrame);/*from www. ja v a 2 s .c o m*/ // project coordinates in the meridian plane final TimeStampedPVCoordinates gpFirst = firstPrincipalCurvature.projectToEllipse(pvInBodyFrame); final Vector3D gpP = gpFirst.getPosition(); final double gr = MathArrays.linearCombination(gpP.getX(), meridian.getX(), gpP.getY(), meridian.getY()); final double gz = gpP.getZ(); // topocentric frame final Vector3D east = new Vector3D(-meridian.getY(), meridian.getX(), 0); final Vector3D zenith = new Vector3D(gr * getC() / getA(), meridian, gz * getA() / getC(), Vector3D.PLUS_K) .normalize(); final Vector3D north = Vector3D.crossProduct(zenith, east); // set up the ellipse corresponding to second principal curvature in the zenith/east plane final Ellipse secondPrincipalCurvature = getPlaneSection(gpP, north); final TimeStampedPVCoordinates gpSecond = secondPrincipalCurvature.projectToEllipse(pvInBodyFrame); final Vector3D gpV = gpFirst.getVelocity().add(gpSecond.getVelocity()); final Vector3D gpA = gpFirst.getAcceleration().add(gpSecond.getAcceleration()); // moving projected point final TimeStampedPVCoordinates groundPV = new TimeStampedPVCoordinates(pv.getDate(), gpP, gpV, gpA); // transform moving projected point back to initial frame return toBody.getInverse().transformPVCoordinates(groundPV); }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date) throws OrekitException { // transform point to body frame final Vector3D pointInBodyFrame = frame.getTransformTo(bodyFrame, date).transformPosition(point); final double r2 = pointInBodyFrame.getX() * pointInBodyFrame.getX() + pointInBodyFrame.getY() * pointInBodyFrame.getY(); final double r = FastMath.sqrt(r2); final double z = pointInBodyFrame.getZ(); // set up the 2D meridian ellipse final Ellipse meridian = new Ellipse(Vector3D.ZERO, new Vector3D(pointInBodyFrame.getX() / r, pointInBodyFrame.getY() / r, 0), Vector3D.PLUS_K, getA(), getC(), bodyFrame);/* w w w.j av a 2 s. c om*/ // project point on the 2D meridian ellipse final Vector2D ellipsePoint = meridian.projectToEllipse(new Vector2D(r, z)); // relative position of test point with respect to its ellipse sub-point final double dr = r - ellipsePoint.getX(); final double dz = z - ellipsePoint.getY(); final double insideIfNegative = g2 * (r2 - ae2) + z * z; return new GeodeticPoint(FastMath.atan2(ellipsePoint.getY(), g2 * ellipsePoint.getX()), FastMath.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX()), FastMath.copySign(FastMath.hypot(dr, dz), insideIfNegative)); }