List of usage examples for java.lang Math sin
@HotSpotIntrinsicCandidate public static double sin(double a)
From source file:com.nextbreakpoint.nextfractal.mandelbrot.core.Expression.java
public static Number funcPow(MutableNumber out, Number x, double e) { double d = Math.pow(FastMath.hypot(x.r(), x.i()), e); double f = Math.atan2(x.i(), x.r()) * e; return out.set(d * Math.cos(f), d * Math.sin(f)); }
From source file:de.biomedical_imaging.traj.math.TrajectorySplineFit.java
/** * Calculates a spline to a trajectory. Attention: The spline is fitted through a rotated version of the trajectory. * To fit the spline the trajectory is rotated into its main direction. You can access this rotated trajectory by * {@link #getRotatedTrajectory() getRotatedTrajectory}. * @return The fitted spline/*from ww w . ja v a 2 s. c om*/ */ public PolynomialSplineFunction calculateSpline() { successfull = false; /* * 1.Calculate the minimum bounding rectangle */ ArrayList<Point2D.Double> points = new ArrayList<Point2D.Double>(); for (int i = 0; i < t.size(); i++) { Point2D.Double p = new Point2D.Double(); p.setLocation(t.get(i).x, t.get(i).y); points.add(p); } /* * 1.1 Rotate that the major axis is parallel with the xaxis */ Array2DRowRealMatrix gyr = RadiusGyrationTensor2D.getRadiusOfGyrationTensor(t); EigenDecomposition eigdec = new EigenDecomposition(gyr); double inRad = -1 * Math.atan2(eigdec.getEigenvector(0).getEntry(1), eigdec.getEigenvector(0).getEntry(0)); boolean doTransform = (Math.abs(Math.abs(inRad) - Math.PI) > 0.001); if (doTransform) { angleRotated = inRad; for (int i = 0; i < t.size(); i++) { double x = t.get(i).x; double y = t.get(i).y; double newX = x * Math.cos(inRad) - y * Math.sin(inRad); double newY = x * Math.sin(inRad) + y * Math.cos(inRad); rotatedTrajectory.add(newX, newY, 0); points.get(i).setLocation(newX, newY); } //for(int i = 0; i < rect.length; i++){ // rect[i].setLocation(rect[i].x*Math.cos(inRad)-rect[i].y*Math.sin(inRad), rect[i].x*Math.sin(inRad)+rect[i].y*Math.cos(inRad)); //} } else { angleRotated = 0; rotatedTrajectory = t; } /* * 2. Divide the rectangle in n equal segments * 2.1 Calculate line in main direction * 2.2 Project the points in onto this line * 2.3 Calculate the distance between the start of the line and the projected point * 2.4 Assign point to segment according to distance of (2.3) */ List<List<Point2D.Double>> pointsInSegments = null; boolean allSegmentsContainingAtLeastTwoPoints = true; int indexSmallestX = 0; double segmentWidth = 0; do { double smallestX = Double.MAX_VALUE; double largestX = Double.MIN_VALUE; for (int i = 0; i < points.size(); i++) { if (points.get(i).x < smallestX) { smallestX = points.get(i).x; indexSmallestX = i; } if (points.get(i).x > largestX) { largestX = points.get(i).x; } } allSegmentsContainingAtLeastTwoPoints = true; segmentWidth = (largestX - smallestX) / nSegments; pointsInSegments = new ArrayList<List<Point2D.Double>>(nSegments); for (int i = 0; i < nSegments; i++) { pointsInSegments.add(new ArrayList<Point2D.Double>()); } for (int i = 0; i < points.size(); i++) { int index = (int) Math.abs((points.get(i).x / segmentWidth)); if (index > (nSegments - 1)) { index = (nSegments - 1); } pointsInSegments.get(index).add(points.get(i)); } for (int i = 0; i < pointsInSegments.size(); i++) { if (pointsInSegments.get(i).size() < 2) { if (nSegments > 2) { nSegments--; i = pointsInSegments.size(); allSegmentsContainingAtLeastTwoPoints = false; } } } } while (allSegmentsContainingAtLeastTwoPoints == false); /* * 3. Calculate the mean standard deviation over each segment: <s> */ //Point2D.Double eMajorP1 = new Point2D.Double(p1.x - (p3.x-p1.x)/2.0,p1.y - (p3.y-p1.y)/2.0); // Point2D.Double eMajorP2 = new Point2D.Double(p2.x - (p3.x-p1.x)/2.0,p2.y - (p3.y-p1.y)/2.0); double sumSDOrthogonal = 0; int Nsum = 0; CenterOfGravityFeature cogf = new CenterOfGravityFeature(rotatedTrajectory); Point2D.Double cog = new Point2D.Double(cogf.evaluate()[0], cogf.evaluate()[1]); Point2D.Double eMajorP1 = cog; Point2D.Double eMajorP2 = new Point2D.Double(cog.getX() + 1, cog.getY()); for (int i = 0; i < nSegments; i++) { StandardDeviation sd = new StandardDeviation(); double[] distances = new double[pointsInSegments.get(i).size()]; for (int j = 0; j < pointsInSegments.get(i).size(); j++) { int factor = 1; if (isLeft(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j))) { factor = -1; } distances[j] = factor * distancePointLine(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j)); } if (distances.length > 0) { sd.setData(distances); sumSDOrthogonal += sd.evaluate(); Nsum++; } } double s = sumSDOrthogonal / Nsum; if (segmentWidth < Math.pow(10, -15)) { return null; } if (s < Math.pow(10, -15)) { //If standard deviation is zero, replace it with the half of the segment width s = segmentWidth / 2; } //rotatedTrajectory.showTrajectory("rot"); /* * 4. Build a kd-tree */ KDTree<Point2D.Double> kdtree = new KDTree<Point2D.Double>(2); for (int i = 0; i < points.size(); i++) { try { //To ensure that all points have a different key, add small random number kdtree.insert(new double[] { points.get(i).x, points.get(i).y }, points.get(i)); } catch (KeySizeException e) { e.printStackTrace(); } catch (KeyDuplicateException e) { //Do nothing! It is not important } } /* * 5. Using the first point f in trajectory and calculate the center of mass * of all points around f (radius: 3*<s>)) */ List<Point2D.Double> near = null; Point2D.Double first = points.get(indexSmallestX);//minDistancePointToLine(p1, p3, points); double r1 = 3 * s; try { near = kdtree.nearestEuclidean(new double[] { first.x, first.y }, r1); } catch (KeySizeException e) { e.printStackTrace(); } double cx = 0; double cy = 0; for (int i = 0; i < near.size(); i++) { cx += near.get(i).x; cy += near.get(i).y; } cx /= near.size(); cy /= near.size(); splineSupportPoints = new ArrayList<Point2D.Double>(); splineSupportPoints.add(new Point2D.Double(cx, cy)); /* * 6. The second point is determined by finding the center-of-mass of particles in the p/2 radian * section of an annulus, r1 < r < 2r1, that is directed toward the angle with the highest number * of particles within p/2 radians. * 7. This second point is then used as the center of the annulus for choosing the third point, and the process is repeated (6. & 7.). */ /* * 6.1 Find all points in the annolous */ /* * 6.2 Write each point in a coordinate system centered at the center of the sphere, calculate direction and * check if it in the allowed bounds */ int nCircleSegments = 100; double deltaRad = 2 * Math.PI / nCircleSegments; boolean stop = false; int minN = 7; double tempr1 = r1; double allowedDeltaDirection = 0.5 * Math.PI; while (stop == false) { List<Point2D.Double> nearestr1 = null; List<Point2D.Double> nearest2xr1 = null; try { nearestr1 = kdtree .nearestEuclidean(new double[] { splineSupportPoints.get(splineSupportPoints.size() - 1).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y }, tempr1); nearest2xr1 = kdtree .nearestEuclidean(new double[] { splineSupportPoints.get(splineSupportPoints.size() - 1).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y }, 2 * tempr1); } catch (KeySizeException e) { // TODO Auto-generated catch block e.printStackTrace(); } nearest2xr1.removeAll(nearestr1); double lThreshRad = 0; double hThreshRad = Math.PI / 2; double stopThresh = 2 * Math.PI; if (splineSupportPoints.size() > 1) { double directionInRad = Math.atan2( splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y, splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x) + Math.PI; lThreshRad = directionInRad - allowedDeltaDirection / 2 - Math.PI / 4; if (lThreshRad < 0) { lThreshRad = 2 * Math.PI + lThreshRad; } if (lThreshRad > 2 * Math.PI) { lThreshRad = lThreshRad - 2 * Math.PI; } hThreshRad = directionInRad + allowedDeltaDirection / 2 + Math.PI / 4; if (hThreshRad < 0) { hThreshRad = 2 * Math.PI + hThreshRad; } if (hThreshRad > 2 * Math.PI) { hThreshRad = hThreshRad - 2 * Math.PI; } stopThresh = directionInRad + allowedDeltaDirection / 2 - Math.PI / 4; if (stopThresh > 2 * Math.PI) { stopThresh = stopThresh - 2 * Math.PI; } } double newCx = 0; double newCy = 0; int newCN = 0; int candN = 0; //Find center with highest density of points double lastDist = 0; double newDist = 0; do { lastDist = Math.min(Math.abs(lThreshRad - stopThresh), 2 * Math.PI - Math.abs(lThreshRad - stopThresh)); candN = 0; double candCx = 0; double candCy = 0; for (int i = 0; i < nearest2xr1.size(); i++) { Point2D.Double centerOfCircle = splineSupportPoints.get(splineSupportPoints.size() - 1); Vector2d relativeToCircle = new Vector2d(nearest2xr1.get(i).x - centerOfCircle.x, nearest2xr1.get(i).y - centerOfCircle.y); relativeToCircle.normalize(); double angleInRadians = Math.atan2(relativeToCircle.y, relativeToCircle.x) + Math.PI; if (lThreshRad < hThreshRad) { if (angleInRadians > lThreshRad && angleInRadians < hThreshRad) { candCx += nearest2xr1.get(i).x; candCy += nearest2xr1.get(i).y; candN++; } } else { if (angleInRadians > lThreshRad || angleInRadians < hThreshRad) { candCx += nearest2xr1.get(i).x; candCy += nearest2xr1.get(i).y; candN++; } } } if (candN > 0 && candN > newCN) { candCx /= candN; candCy /= candN; newCx = candCx; newCy = candCy; newCN = candN; } lThreshRad += deltaRad; hThreshRad += deltaRad; if (lThreshRad > 2 * Math.PI) { lThreshRad = lThreshRad - 2 * Math.PI; } if (hThreshRad > 2 * Math.PI) { hThreshRad = hThreshRad - 2 * Math.PI; } newDist = Math.min(Math.abs(lThreshRad - stopThresh), 2 * Math.PI - Math.abs(lThreshRad - stopThresh)); } while ((newDist - lastDist) > 0); //Check if the new center is valid if (splineSupportPoints.size() > 1) { double currentDirectionInRad = Math.atan2( splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y, splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x) + Math.PI; double candDirectionInRad = Math.atan2( newCy - splineSupportPoints.get(splineSupportPoints.size() - 1).y, newCx - splineSupportPoints.get(splineSupportPoints.size() - 1).x) + Math.PI; double dDir = Math.max(currentDirectionInRad, candDirectionInRad) - Math.min(currentDirectionInRad, candDirectionInRad); if (dDir > 2 * Math.PI) { dDir = 2 * Math.PI - dDir; } if (dDir > allowedDeltaDirection) { stop = true; } } boolean enoughPoints = (newCN < minN); boolean isNormalRadius = Math.abs(tempr1 - r1) < Math.pow(10, -18); boolean isExtendedRadius = Math.abs(tempr1 - 3 * r1) < Math.pow(10, -18); if (enoughPoints && isNormalRadius) { //Not enough points, extend search radius tempr1 = 3 * r1; } else if (enoughPoints && isExtendedRadius) { //Despite radius extension: Not enough points! stop = true; } else if (stop == false) { splineSupportPoints.add(new Point2D.Double(newCx, newCy)); tempr1 = r1; } } //Sort Collections.sort(splineSupportPoints, new Comparator<Point2D.Double>() { public int compare(Point2D.Double o1, Point2D.Double o2) { if (o1.x < o2.x) { return -1; } if (o1.x > o2.x) { return 1; } return 0; } }); //Add endpoints if (splineSupportPoints.size() > 1) { Vector2d start = new Vector2d(splineSupportPoints.get(0).x - splineSupportPoints.get(1).x, splineSupportPoints.get(0).y - splineSupportPoints.get(1).y); start.normalize(); start.scale(r1 * 8); splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x + start.x, splineSupportPoints.get(0).y + start.y)); Vector2d end = new Vector2d( splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y); end.normalize(); end.scale(r1 * 6); splineSupportPoints .add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size() - 1).x + end.x, splineSupportPoints.get(splineSupportPoints.size() - 1).y + end.y)); } else { Vector2d majordir = new Vector2d(-1, 0); majordir.normalize(); majordir.scale(r1 * 8); splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x + majordir.x, splineSupportPoints.get(0).y + majordir.y)); majordir.scale(-1); splineSupportPoints .add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size() - 1).x + majordir.x, splineSupportPoints.get(splineSupportPoints.size() - 1).y + majordir.y)); } //Interpolate spline double[] supX = new double[splineSupportPoints.size()]; double[] supY = new double[splineSupportPoints.size()]; for (int i = 0; i < splineSupportPoints.size(); i++) { supX[i] = splineSupportPoints.get(i).x; supY[i] = splineSupportPoints.get(i).y; } SplineInterpolator sIinter = new SplineInterpolator(); spline = sIinter.interpolate(supX, supY); successfull = true; return spline; }
From source file:es.emergya.geo.util.GeoToUtmConverter.java
public static GeodesicPosition transform(GeodesicPosition from, double from_a, double from_f, double from_esq, double da, double df, double dx, double dy, double dz) { double slat = Math.sin(from.lat); double clat = Math.cos(from.lat); double slon = Math.sin(from.lon); double clon = Math.cos(from.lon); double ssqlat = slat * slat; double adb = 1.0 / (1.0 - from_f); // "a divided by b" double dlat, dlon, dh; double rn = from_a / Math.sqrt(1.0 - from_esq * ssqlat); double rm = from_a * (1. - from_esq) / Math.pow((1.0 - from_esq * ssqlat), 1.5); dlat = (((((-dx * slat * clon - dy * slat * slon) + dz * clat) + (da * ((rn * from_esq * slat * clat) / from_a))) + (df * (rm * adb + rn / adb) * slat * clat))) / (rm + from.h);//from www .j av a2s. c o m dlon = (-dx * slon + dy * clon) / ((rn + from.h) * clat); dh = (dx * clat * clon) + (dy * clat * slon) + (dz * slat) - (da * (from_a / rn)) + ((df * rn * ssqlat) / adb); return new GeodesicPosition(from.lon + dlon, from.lat + dlat, from.h + dh); }
From source file:com.alvermont.terraj.planet.project.MollweideProjection.java
/** * Carry out the projection/* w ww. j a v a2 s. c o m*/ */ public void project() { setcolours(); final int width = getParameters().getProjectionParameters().getWidth(); final int height = getParameters().getProjectionParameters().getHeight(); final double lat = getParameters().getProjectionParameters().getLatitudeRadians(); final double lon = getParameters().getProjectionParameters().getLongitudeRadians(); final double scale = getParameters().getProjectionParameters().getScale(); final double hgrid = getParameters().getProjectionParameters().getHgrid(); final double vgrid = getParameters().getProjectionParameters().getVgrid(); final boolean doShade = getParameters().getProjectionParameters().isDoShade(); cacheParameters(); colours = new short[width][height]; shades = new short[width][height]; depth = (3 * ((int) (log2(scale * height)))) + 6; log.debug("MollweideProjection starting with depth set to " + depth); progress.progressStart(height, "Generating Terrain"); double x; double y; double y1; double zz; double scale1; double cos2; double theta1; double theta2; int i; int j; int i1 = 1; int k; for (j = 0; j < height; ++j) { progress.progressStep(j); y1 = (2 * ((2.0 * j) - height)) / width / scale; if (Math.abs(y1) >= 1.0) { for (i = 0; i < width; ++i) { colours[i][j] = backgroundColour; if (doShade) { shades[i][j] = 255; } } } else { zz = Math.sqrt(1.0 - (y1 * y1)); y = 2.0 / Math.PI * ((y1 * zz) + Math.asin(y1)); cos2 = Math.sqrt(1.0 - (y * y)); if (cos2 > 0.0) { scale1 = (scale * width) / height / cos2 / Math.PI; depth = (3 * ((int) (log2(scale1 * height)))) + 3; for (i = 0; i < width; ++i) { theta1 = (Math.PI / zz * ((2.0 * i) - width)) / width / scale; if (Math.abs(theta1) > Math.PI) { colours[i][j] = backgroundColour; if (doShade) { shades[i][j] = 255; } } else { theta1 += (lon - (0.5 * Math.PI)); colours[i][j] = (short) planet0(Math.cos(theta1) * cos2, y, -Math.sin(theta1) * cos2); if (doShade) { shades[i][j] = shade; } } } } } } progress.progressComplete("Terrain Generated"); log.debug("MollweideProjection complete"); if (hgrid != 0.0) { /* draw horizontal gridlines */ for (theta1 = 0.0; theta1 > -ProjectionConstants.RIGHT_ANGLE_DEGREES; theta1 -= hgrid) ; for (theta1 = theta1; theta1 < ProjectionConstants.RIGHT_ANGLE_DEGREES; theta1 += hgrid) { theta2 = Math.abs(theta1); x = Math.floor(theta2 / 5.0); y = (theta2 / 5.0) - x; y = ((1.0 - y) * mollTable[(int) x]) + (y * mollTable[(int) x + 1]); if (theta1 < 0.0) { y = -y; } j = (height / 2) + (int) (0.25 * y * width * scale); if ((j >= 0) && (j < height)) { for (i = Math.max(0, (width / 2) - (int) (0.5 * width * scale * Math.sqrt(1.0 - (y * y)))); i < Math.min( width, (width / 2) + (int) (0.5 * width * scale * Math.sqrt(1.0 - (y * y)))); ++i) colours[i][j] = BLACK; } } } if (vgrid != 0.0) { /* draw vertical gridlines */ for (theta1 = 0.0; theta1 > -ProjectionConstants.CIRCLE_DEGREES; theta1 -= vgrid) ; for (theta1 = theta1; theta1 < ProjectionConstants.CIRCLE_DEGREES; theta1 += vgrid) { if (((Math.toRadians(theta1) - lon + (0.5 * Math.PI)) > -Math.PI) && ((Math.toRadians(theta1) - lon + (0.5 * Math.PI)) <= Math.PI)) { x = (0.5 * (Math.toRadians(theta1) - lon + (0.5 * Math.PI)) * width * scale) / Math.PI; j = Math.max(0, (height / 2) - (int) (0.25 * width * scale)); y = (2 * ((2.0 * j) - height)) / width / scale; i = (int) ((width / 2) + (x * Math.sqrt(1.0 - (y * y)))); for (; j <= Math.min(height, (height / 2) + (int) (0.25 * width * scale)); ++j) { y1 = (2 * ((2.0 * j) - height)) / width / scale; if (Math.abs(y1) <= 1.0) { i1 = (int) ((width / 2) + (x * Math.sqrt(1.0 - (y1 * y1)))); if ((i1 >= 0) && (i1 < width)) { colours[i1][j] = BLACK; } } if (Math.abs(y) <= 1.0) { if (i < i1) { for (k = i + 1; k < i1; ++k) { if ((k > 00) && (k < width)) { colours[k][j] = BLACK; } } } else if (i > i1) { for (k = i - 1; k > i1; --k) { if ((k >= 0) && (k < width)) { colours[k][j] = BLACK; } } } } y = y1; i = i1; } } } } if (doShade) { smoothshades(); } doOutlining(); }
From source file:com.rapidminer.tools.expression.internal.function.AntlrParserTrigonometricTest.java
@Test public void sinPiHalf() { try {/*from ww w .java2 s . c om*/ Expression expression = getExpressionWithFunctionContext("sin(pi/2)"); assertEquals(ExpressionType.DOUBLE, expression.getExpressionType()); assertEquals(Math.sin(Math.PI / 2), expression.evaluateNumerical(), 1e-15); } catch (ExpressionException e) { assertNotNull(e.getMessage()); } }
From source file:com.nextbreakpoint.nextfractal.mandelbrot.core.Expression.java
public static Number funcSqrt(MutableNumber out, Number x) { double d = Math.sqrt(FastMath.hypot(x.r(), x.i())); double f = Math.atan2(x.i(), x.r()) * 0.5; return out.set(d * Math.cos(f), d * Math.sin(f)); }
From source file:gdsc.smlm.ij.results.PSFImagePeakResults.java
public double[] setPSFParameters(double t, double sx, double sy, double[] params) { double a, b, c; double height, width; if (t == 0) { // sin(0) == 0 // cos(0) == 1 a = (-1.0 / (2.0 * sx * sx));/*from w w w .j av a 2s . c om*/ b = 0.0; c = (-1.0 / (2.0 * sy * sy)); // Calculate the range for the PSF as 3 sigma. width = 3.0 * sx; height = 3.0 * sy; } else { a = -(Math.cos(t) * Math.cos(t) / (2.0 * sx * sx) + Math.sin(t) * Math.sin(t) / (2.0 * sy * sy)); b = -(-Math.sin(2.0 * t) / (2.0 * sx * sx) + Math.sin(2.0 * t) / (2.0 * sy * sy)); c = -(Math.sin(t) * Math.sin(t) / (2.0 * sx * sx) + Math.cos(t) * Math.cos(t) / (2.0 * sy * sy)); // Note that the Gaussian2DFitter returns the angle of the major axis (sx) relative to the x-axis. // The angle is in the range -pi/2 to pi/2 // The width and height for the range to be plotted can be derived from the general parametric // form of the ellipse. // See: http://en.wikipedia.org/wiki/Ellipse#General_parametric_form // Ensure the angle is the correct range (0 to pi) if (t < 0) t += Math.PI; final double phi = t; // Angle between x-axis and major axis of ellipse final double t1 = -t; // Angle around the ellipse from 0 to 2pi for the x-axis final double t2 = t1 + Math.PI / 2; // Angle around the ellipse from 0 to 2pi for the y-axis // Calculate the size of the ellipse at 3 sigma width = Math.abs(3.0 * (sx * Math.cos(t1) * Math.cos(phi) - sy * Math.sin(t1) * Math.sin(phi))); height = Math.abs(3.0 * (sx * Math.cos(t2) * Math.sin(phi) + sy * Math.sin(t2) * Math.cos(phi))); } params[0] = a; params[1] = b; params[2] = c; params[3] = width; params[4] = height; return params; }
From source file:com.luthfihm.virtualtour.ar.GyroscopeOrientation.java
private void getRotationVectorFromGyro() { // Calculate the angular speed of the sample float magnitude = (float) Math .sqrt(Math.pow(vGyroscope[0], 2) + Math.pow(vGyroscope[1], 2) + Math.pow(vGyroscope[2], 2)); // Normalize the rotation vector if it's big enough to get the axis if (magnitude > EPSILON) { vGyroscope[0] /= magnitude;/*ww w . j a va 2 s. c o m*/ vGyroscope[1] /= magnitude; vGyroscope[2] /= magnitude; } // Integrate around this axis with the angular speed by the timestep // in order to get a delta rotation from this sample over the timestep // We will convert this axis-angle representation of the delta rotation // into a quaternion before turning it into the rotation matrix. float thetaOverTwo = magnitude * dT / 2.0f; float sinThetaOverTwo = (float) Math.sin(thetaOverTwo); float cosThetaOverTwo = (float) Math.cos(thetaOverTwo); deltaVGyroscope[0] = sinThetaOverTwo * vGyroscope[0]; deltaVGyroscope[1] = sinThetaOverTwo * vGyroscope[1]; deltaVGyroscope[2] = sinThetaOverTwo * vGyroscope[2]; deltaVGyroscope[3] = cosThetaOverTwo; // Create a new quaternion object base on the latest rotation // measurements... qGyroscopeDelta = new Quaternion(deltaVGyroscope[3], Arrays.copyOfRange(deltaVGyroscope, 0, 3)); // Since it is a unit quaternion, we can just multiply the old rotation // by the new rotation delta to integrate the rotation. qGyroscope = qGyroscope.multiply(qGyroscopeDelta); }
From source file:de.biomedical_imaging.traj.math.TrajectorySplineFitLegacy.java
/** * Calculates a spline to a trajectory. Attention: The spline is fitted through a rotated version of the trajectory. * To fit the spline the trajectory is rotated into its main direction. You can access this rotated trajectory by * {@link #getRotatedTrajectory() getRotatedTrajectory}. * @return The fitted spline//from w w w .j a v a2 s .c o m */ public PolynomialSplineFunction calculateSpline() { /* * 1.Calculate the minimum bounding rectangle */ ArrayList<Point2D.Double> points = new ArrayList<Point2D.Double>(); for (int i = 0; i < t.size(); i++) { Point2D.Double p = new Point2D.Double(); p.setLocation(t.get(i).x, t.get(i).y); points.add(p); } Point2D.Double[] rect = null; try { rect = RotatingCalipers.getMinimumBoundingRectangle(points); } catch (IllegalArgumentException e) { } catch (EmptyStackException e) { } /* * 1.1 Rotate that the major axis is parallel with the xaxis */ Point2D.Double majorDirection = null; Point2D.Double p1 = rect[2]; //top left Point2D.Double p2 = p1.distance(rect[1]) > p1.distance(rect[3]) ? rect[1] : rect[3]; //Point to long side Point2D.Double p3 = p1.distance(rect[1]) > p1.distance(rect[3]) ? rect[3] : rect[1]; //Point to short side majorDirection = new Point2D.Double(p2.x - p1.x, p2.y - p1.y); double width = p1.distance(p2); double inRad = -1 * Math.atan2(majorDirection.y, majorDirection.x); boolean doTransform = (Math.abs(Math.abs(inRad) - Math.PI) > 0.001); if (doTransform) { angleRotated = inRad; for (int i = 0; i < t.size(); i++) { double x = t.get(i).x; double y = t.get(i).y; double newX = x * Math.cos(inRad) - y * Math.sin(inRad); double newY = x * Math.sin(inRad) + y * Math.cos(inRad); rotatedTrajectory.add(newX, newY, 0); points.get(i).setLocation(newX, newY); } for (int i = 0; i < rect.length; i++) { rect[i].setLocation(rect[i].x * Math.cos(inRad) - rect[i].y * Math.sin(inRad), rect[i].x * Math.sin(inRad) + rect[i].y * Math.cos(inRad)); } p1 = rect[2]; //top left p2 = p1.distance(rect[1]) > p1.distance(rect[3]) ? rect[1] : rect[3]; //Point to long side p3 = p1.distance(rect[1]) > p1.distance(rect[3]) ? rect[3] : rect[1]; //Point to short side } else { angleRotated = 0; rotatedTrajectory = t; } /* * 2. Divide the rectangle in n equal segments * 2.1 Calculate line in main direction * 2.2 Project the points in onto this line * 2.3 Calculate the distance between the start of the line and the projected point * 2.4 Assign point to segment according to distance of (2.3) */ List<List<Point2D.Double>> pointsInSegments = null; boolean allSegmentsContainingAtLeastTwoPoints = true; do { allSegmentsContainingAtLeastTwoPoints = true; double segmentWidth = p1.distance(p2) / nSegments; pointsInSegments = new ArrayList<List<Point2D.Double>>(nSegments); for (int i = 0; i < nSegments; i++) { pointsInSegments.add(new ArrayList<Point2D.Double>()); } for (int i = 0; i < points.size(); i++) { Point2D.Double projPoint = projectPointToLine(p1, p2, points.get(i)); int index = (int) (p1.distance(projPoint) / segmentWidth); if (index > (nSegments - 1)) { index = (nSegments - 1); } pointsInSegments.get(index).add(points.get(i)); } for (int i = 0; i < pointsInSegments.size(); i++) { if (pointsInSegments.get(i).size() < 2) { if (nSegments > 2) { nSegments--; i = pointsInSegments.size(); allSegmentsContainingAtLeastTwoPoints = false; } } } } while (allSegmentsContainingAtLeastTwoPoints == false); /* * 3. Calculate the mean standard deviation over each segment: <s> */ Point2D.Double eMajorP1 = new Point2D.Double(p1.x - (p3.x - p1.x) / 2.0, p1.y - (p3.y - p1.y) / 2.0); Point2D.Double eMajorP2 = new Point2D.Double(p2.x - (p3.x - p1.x) / 2.0, p2.y - (p3.y - p1.y) / 2.0); double sumMean = 0; int Nsum = 0; for (int i = 0; i < nSegments; i++) { StandardDeviation sd = new StandardDeviation(); double[] distances = new double[pointsInSegments.get(i).size()]; for (int j = 0; j < pointsInSegments.get(i).size(); j++) { int factor = 1; if (isLeft(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j))) { factor = -1; } distances[j] = factor * distancePointLine(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j)); } if (distances.length > 0) { sd.setData(distances); sumMean += sd.evaluate(); Nsum++; } } double s = sumMean / Nsum; if (s < 0.000000000001) { s = width / nSegments; } /* * 4. Build a kd-tree */ KDTree<Point2D.Double> kdtree = new KDTree<Point2D.Double>(2); for (int i = 0; i < points.size(); i++) { try { //To ensure that all points have a different key, add small random number kdtree.insert(new double[] { points.get(i).x, points.get(i).y }, points.get(i)); } catch (KeySizeException e) { e.printStackTrace(); } catch (KeyDuplicateException e) { //Do nothing! It is not important } } /* * 5. Using the first point f in trajectory and calculate the center of mass * of all points around f (radius: 3*<s>)) */ List<Point2D.Double> near = null; Point2D.Double first = minDistancePointToLine(p1, p3, points); double r1 = 3 * s; try { near = kdtree.nearestEuclidean(new double[] { first.x, first.y }, r1); } catch (KeySizeException e) { e.printStackTrace(); } double cx = 0; double cy = 0; for (int i = 0; i < near.size(); i++) { cx += near.get(i).x; cy += near.get(i).y; } cx /= near.size(); cy /= near.size(); splineSupportPoints = new ArrayList<Point2D.Double>(); splineSupportPoints.add(new Point2D.Double(cx, cy)); /* * 6. The second point is determined by finding the center-of-mass of particles in the p/2 radian * section of an annulus, r1 < r < 2r1, that is directed toward the angle with the highest number * of particles within p/2 radians. * 7. This second point is then used as the center of the annulus for choosing the third point, and the process is repeated (6. & 7.). */ /* * 6.1 Find all points in the annolous */ /* * 6.2 Write each point in a coordinate system centered at the center of the sphere, calculate direction and * check if it in the allowed bounds */ int nCircleSegments = 100; double deltaRad = 2 * Math.PI / nCircleSegments; boolean stop = false; int minN = 7; double tempr1 = r1; double allowedDeltaDirection = 0.5 * Math.PI; while (stop == false) { List<Point2D.Double> nearestr1 = null; List<Point2D.Double> nearest2xr1 = null; try { nearestr1 = kdtree .nearestEuclidean(new double[] { splineSupportPoints.get(splineSupportPoints.size() - 1).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y }, tempr1); nearest2xr1 = kdtree .nearestEuclidean(new double[] { splineSupportPoints.get(splineSupportPoints.size() - 1).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y }, 2 * tempr1); } catch (KeySizeException e) { // TODO Auto-generated catch block e.printStackTrace(); } nearest2xr1.removeAll(nearestr1); double lThreshRad = 0; double hThreshRad = Math.PI / 2; double stopThresh = 2 * Math.PI; if (splineSupportPoints.size() > 1) { double directionInRad = Math.atan2( splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y, splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x) + Math.PI; lThreshRad = directionInRad - allowedDeltaDirection / 2 - Math.PI / 4; if (lThreshRad < 0) { lThreshRad = 2 * Math.PI + lThreshRad; } if (lThreshRad > 2 * Math.PI) { lThreshRad = lThreshRad - 2 * Math.PI; } hThreshRad = directionInRad + allowedDeltaDirection / 2 + Math.PI / 4; if (hThreshRad < 0) { hThreshRad = 2 * Math.PI + hThreshRad; } if (hThreshRad > 2 * Math.PI) { hThreshRad = hThreshRad - 2 * Math.PI; } stopThresh = directionInRad + allowedDeltaDirection / 2 - Math.PI / 4; if (stopThresh > 2 * Math.PI) { stopThresh = stopThresh - 2 * Math.PI; } } double newCx = 0; double newCy = 0; int newCN = 0; int candN = 0; //Find center with highest density of points double lastDist = 0; double newDist = 0; do { lastDist = Math.min(Math.abs(lThreshRad - stopThresh), 2 * Math.PI - Math.abs(lThreshRad - stopThresh)); candN = 0; double candCx = 0; double candCy = 0; for (int i = 0; i < nearest2xr1.size(); i++) { Point2D.Double centerOfCircle = splineSupportPoints.get(splineSupportPoints.size() - 1); Vector2d relativeToCircle = new Vector2d(nearest2xr1.get(i).x - centerOfCircle.x, nearest2xr1.get(i).y - centerOfCircle.y); relativeToCircle.normalize(); double angleInRadians = Math.atan2(relativeToCircle.y, relativeToCircle.x) + Math.PI; if (lThreshRad < hThreshRad) { if (angleInRadians > lThreshRad && angleInRadians < hThreshRad) { candCx += nearest2xr1.get(i).x; candCy += nearest2xr1.get(i).y; candN++; } } else { if (angleInRadians > lThreshRad || angleInRadians < hThreshRad) { candCx += nearest2xr1.get(i).x; candCy += nearest2xr1.get(i).y; candN++; } } } if (candN > 0 && candN > newCN) { candCx /= candN; candCy /= candN; newCx = candCx; newCy = candCy; newCN = candN; } lThreshRad += deltaRad; hThreshRad += deltaRad; if (lThreshRad > 2 * Math.PI) { lThreshRad = lThreshRad - 2 * Math.PI; } if (hThreshRad > 2 * Math.PI) { hThreshRad = hThreshRad - 2 * Math.PI; } newDist = Math.min(Math.abs(lThreshRad - stopThresh), 2 * Math.PI - Math.abs(lThreshRad - stopThresh)); } while ((newDist - lastDist) > 0); //Check if the new center is valid if (splineSupportPoints.size() > 1) { double currentDirectionInRad = Math.atan2( splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y, splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x) + Math.PI; double candDirectionInRad = Math.atan2( newCy - splineSupportPoints.get(splineSupportPoints.size() - 1).y, newCx - splineSupportPoints.get(splineSupportPoints.size() - 1).x) + Math.PI; double dDir = Math.max(currentDirectionInRad, candDirectionInRad) - Math.min(currentDirectionInRad, candDirectionInRad); if (dDir > 2 * Math.PI) { dDir = 2 * Math.PI - dDir; } if (dDir > allowedDeltaDirection) { stop = true; } } boolean enoughPoints = (newCN < minN); boolean isNormalRadius = Math.abs(tempr1 - r1) < Math.pow(10, -18); boolean isExtendedRadius = Math.abs(tempr1 - 3 * r1) < Math.pow(10, -18); if (enoughPoints && isNormalRadius) { //Not enough points, extend search radius tempr1 = 3 * r1; } else if (enoughPoints && isExtendedRadius) { //Despite radius extension: Not enough points! stop = true; } else if (stop == false) { splineSupportPoints.add(new Point2D.Double(newCx, newCy)); tempr1 = r1; } } //Sort Collections.sort(splineSupportPoints, new Comparator<Point2D.Double>() { public int compare(Point2D.Double o1, Point2D.Double o2) { if (o1.x < o2.x) { return -1; } if (o1.x > o2.x) { return 1; } return 0; } }); //Add endpoints if (splineSupportPoints.size() > 1) { Vector2d start = new Vector2d(splineSupportPoints.get(0).x - splineSupportPoints.get(1).x, splineSupportPoints.get(0).y - splineSupportPoints.get(1).y); start.normalize(); start.scale(r1 * 8); splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x + start.x, splineSupportPoints.get(0).y + start.y)); Vector2d end = new Vector2d( splineSupportPoints.get(splineSupportPoints.size() - 1).x - splineSupportPoints.get(splineSupportPoints.size() - 2).x, splineSupportPoints.get(splineSupportPoints.size() - 1).y - splineSupportPoints.get(splineSupportPoints.size() - 2).y); end.normalize(); end.scale(r1 * 6); splineSupportPoints .add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size() - 1).x + end.x, splineSupportPoints.get(splineSupportPoints.size() - 1).y + end.y)); } else { Vector2d majordir = new Vector2d(-1, 0); majordir.normalize(); majordir.scale(r1 * 8); splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x + majordir.x, splineSupportPoints.get(0).y + majordir.y)); majordir.scale(-1); splineSupportPoints .add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size() - 1).x + majordir.x, splineSupportPoints.get(splineSupportPoints.size() - 1).y + majordir.y)); } //Interpolate spline double[] supX = new double[splineSupportPoints.size()]; double[] supY = new double[splineSupportPoints.size()]; for (int i = 0; i < splineSupportPoints.size(); i++) { supX[i] = splineSupportPoints.get(i).x; supY[i] = splineSupportPoints.get(i).y; } SplineInterpolator sIinter = new SplineInterpolator(); spline = sIinter.interpolate(supX, supY); return spline; }