List of usage examples for java.lang Math atan2
@HotSpotIntrinsicCandidate public static double atan2(double y, double x)
From source file:ExSpotLight.java
public AnnotationArrow(float x, float y, float z, float x2, float y2, float z2) { super(x, y, z, x2, y2, z2); setLineWidth(lineWidth);/*from ww w .j a v a 2s.com*/ // Compute the length and direction of the line float deltaX = x2 - x; float deltaY = y2 - y; float deltaZ = z2 - z; float theta = -(float) Math.atan2(deltaZ, deltaX); float phi = (float) Math.atan2(deltaY, deltaX); if (deltaX < 0.0f) { phi = (float) Math.PI - phi; } // Compute a matrix to rotate a cone to point in the line's // direction, then place the cone at the line's endpoint. Matrix4f mat = new Matrix4f(); Matrix4f mat2 = new Matrix4f(); mat.setIdentity(); // Move to the endpoint of the line mat2.setIdentity(); mat2.setTranslation(new Vector3f(x2, y2, z2)); mat.mul(mat2); // Spin around Y mat2.setIdentity(); mat2.rotY(theta); mat.mul(mat2); // Tilt up or down around Z mat2.setIdentity(); mat2.rotZ(phi); mat.mul(mat2); // Tilt cone to point right mat2.setIdentity(); mat2.rotZ(-1.571f); mat.mul(mat2); arrowTrans = new TransformGroup(); arrowTrans.setCapability(Group.ALLOW_CHILDREN_WRITE); Transform3D trans = new Transform3D(mat); arrowTrans.setTransform(trans); // Create an appearance arrowAppearance = new Appearance(); arrowAppearance.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE); getLineColor(arrowColor); coloringAttributes = new ColoringAttributes(); coloringAttributes.setColor(arrowColor); coloringAttributes.setShadeModel(ColoringAttributes.SHADE_FLAT); arrowAppearance.setColoringAttributes(coloringAttributes); // Build a cone for the arrow head arrowHead = new Cone(arrowRadius, // base radius arrowLength, // height 0, // don't generate normals radialDivisions, // divisions radially sideDivisions, // divisions vertically arrowAppearance); // appearance arrowTrans.addChild(arrowHead); addChild(arrowTrans); }
From source file:org.esa.s1tbx.sentinel1.gpf.AzimuthShiftOp.java
private double estimateAzOffsets(final Band mBandI, final Band mBandQ, final Band sBandI, final Band sBandQ, final Rectangle backwardRectangle, final Rectangle forwardRectangle, final double spectralSeparation) { final int mDataType = mBandI.getDataType(); final int sDataType = sBandI.getDataType(); final Tile mTileIBack = getSourceTile(mBandI, backwardRectangle); final Tile mTileQBack = getSourceTile(mBandQ, backwardRectangle); final Tile sTileIBack = getSourceTile(sBandI, backwardRectangle); final Tile sTileQBack = getSourceTile(sBandQ, backwardRectangle); double[] mIBackArray = null; double[] mQBackArray = null; if (mDataType == ProductData.TYPE_INT16) { final short[] mIBackArrayShort = (short[]) mTileIBack.getDataBuffer().getElems(); final short[] mQBackArrayShort = (short[]) mTileQBack.getDataBuffer().getElems(); mIBackArray = new double[mIBackArrayShort.length]; mQBackArray = new double[mQBackArrayShort.length]; for (int i = 0; i < mIBackArrayShort.length; i++) { mIBackArray[i] = (double) mIBackArrayShort[i]; mQBackArray[i] = (double) mQBackArrayShort[i]; }//from w ww .ja v a 2s . c o m } else { mIBackArray = (double[]) mTileIBack.getDataBuffer().getElems(); mQBackArray = (double[]) mTileQBack.getDataBuffer().getElems(); } double[] sIBackArray, sQBackArray; if (sDataType == ProductData.TYPE_FLOAT32) { final float[] sIBackArrayFloat = (float[]) sTileIBack.getDataBuffer().getElems(); final float[] sQBackArrayFloat = (float[]) sTileQBack.getDataBuffer().getElems(); sIBackArray = new double[sIBackArrayFloat.length]; sQBackArray = new double[sQBackArrayFloat.length]; for (int i = 0; i < sIBackArrayFloat.length; i++) { sIBackArray[i] = (double) sIBackArrayFloat[i]; sQBackArray[i] = (double) sQBackArrayFloat[i]; } } else { sIBackArray = (double[]) sTileIBack.getDataBuffer().getElems(); sQBackArray = (double[]) sTileQBack.getDataBuffer().getElems(); } final Tile mTileIFor = getSourceTile(mBandI, forwardRectangle); final Tile mTileQFor = getSourceTile(mBandQ, forwardRectangle); final Tile sTileIFor = getSourceTile(sBandI, forwardRectangle); final Tile sTileQFor = getSourceTile(sBandQ, forwardRectangle); double[] mIForArray = null; double[] mQForArray = null; if (mDataType == ProductData.TYPE_INT16) { final short[] mIForArrayShort = (short[]) mTileIFor.getDataBuffer().getElems(); final short[] mQForArrayShort = (short[]) mTileQFor.getDataBuffer().getElems(); mIForArray = new double[mIForArrayShort.length]; mQForArray = new double[mQForArrayShort.length]; for (int i = 0; i < mIForArrayShort.length; i++) { mIForArray[i] = (double) mIForArrayShort[i]; mQForArray[i] = (double) mQForArrayShort[i]; } } else { mIForArray = (double[]) mTileIFor.getDataBuffer().getElems(); mQForArray = (double[]) mTileQFor.getDataBuffer().getElems(); } double[] sIForArray = null; double[] sQForArray = null; if (sDataType == ProductData.TYPE_FLOAT32) { final float[] sIForArrayFloat = (float[]) sTileIFor.getDataBuffer().getElems(); final float[] sQForArrayFloat = (float[]) sTileQFor.getDataBuffer().getElems(); sIForArray = new double[sIForArrayFloat.length]; sQForArray = new double[sQForArrayFloat.length]; for (int i = 0; i < sIForArrayFloat.length; i++) { sIForArray[i] = (double) sIForArrayFloat[i]; sQForArray[i] = (double) sQForArrayFloat[i]; } } else { sIForArray = (double[]) sTileIFor.getDataBuffer().getElems(); sQForArray = (double[]) sTileQFor.getDataBuffer().getElems(); } final int arrayLength = mIBackArray.length; final double[] backIntReal = new double[arrayLength]; final double[] backIntImag = new double[arrayLength]; complexArrayMultiplication(mIBackArray, mQBackArray, sIBackArray, sQBackArray, backIntReal, backIntImag); final double[] forIntReal = new double[arrayLength]; final double[] forIntImag = new double[arrayLength]; complexArrayMultiplication(mIForArray, mQForArray, sIForArray, sQForArray, forIntReal, forIntImag); final double[] diffIntReal = new double[arrayLength]; final double[] diffIntImag = new double[arrayLength]; complexArrayMultiplication(forIntReal, forIntImag, backIntReal, backIntImag, diffIntReal, diffIntImag); double sumReal = 0.0; double sumImag = 0.0; for (int i = 0; i < arrayLength; i++) { final double theta = Math.atan2(diffIntImag[i], diffIntReal[i]); sumReal += FastMath.cos(theta); sumImag += FastMath.sin(theta); } final double phase = Math.atan2(sumImag, sumReal); return phase / (2 * Math.PI * spectralSeparation * subSwath[subSwathIndex - 1].azimuthTimeInterval); }
From source file:org.traccar.web.server.model.DataServiceImpl.java
private double distance(double lat1, double lat2, double lon1, double lon2, double el1, double el2) { final int R = 6371; // Radius of the earth Double latDistance = deg2rad(lat2 - lat1); Double lonDistance = deg2rad(lon2 - lon1); Double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2) + Math.cos(deg2rad(lat1)) * Math.cos(deg2rad(lat2)) * Math.sin(lonDistance / 2) * Math.sin(lonDistance / 2); Double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a)); double distance = R * c;// In kilometer double height = el1 - el2; distance = Math.pow(distance, 2) + Math.pow(height, 2); return Math.sqrt(distance); }
From source file:org.orekit.tle.DeepSDP4.java
/** Computes periodic terms from current coordinates and epoch. * @param t offset from initial epoch (min) *///from w ww.j ava 2s. co m protected void deepPeriodicEffects(final double t) { // If the time didn't change by more than 30 minutes, // there's no good reason to recompute the perturbations; // they don't change enough over so short a time span. // However, the Dundee code _always_ recomputes, so if // we're attempting to replicate its results, we've gotta // recompute everything, too. if ((Math.abs(savtsn - t) >= 30.0) || isDundeeCompliant) { savtsn = t; // Update solar perturbations for time T double zm = zmos + ZNS * t; double zf = zm + 2 * ZES * Math.sin(zm); double sinzf = Math.sin(zf); double f2 = 0.5 * sinzf * sinzf - 0.25; double f3 = -0.5 * sinzf * Math.cos(zf); final double ses = se2 * f2 + se3 * f3; final double sis = si2 * f2 + si3 * f3; final double sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; final double sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; final double shs = sh2 * f2 + sh3 * f3; // Update lunar perturbations for time T zm = zmol + ZNL * t; zf = zm + 2 * ZEL * Math.sin(zm); sinzf = Math.sin(zf); f2 = 0.5 * sinzf * sinzf - 0.25; f3 = -0.5 * sinzf * Math.cos(zf); final double sel = ee2 * f2 + e3 * f3; final double sil = xi2 * f2 + xi3 * f3; final double sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; final double sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; final double sh1 = xh2 * f2 + xh3 * f3; // Sum the solar and lunar contributions pe = ses + sel; pinc = sis + sil; pl = sls + sll; pgh = sghs + sghl; ph = shs + sh1; } xinc += pinc; final double sinis = Math.sin(xinc); final double cosis = Math.cos(xinc); /* Add solar/lunar perturbation correction to eccentricity: */ em += pe; xll += pl; omgadf += pgh; xinc = MathUtils.normalizeAngle(xinc, 0); if (Math.abs(xinc) >= 0.2) { // Apply periodics directly final double temp_val = ph / sinis; omgadf -= cosis * temp_val; xnode += temp_val; } else { // Apply periodics with Lyddane modification final double sinok = Math.sin(xnode); final double cosok = Math.cos(xnode); final double alfdp = ph * cosok + (pinc * cosis + sinis) * sinok; final double betdp = -ph * sinok + (pinc * cosis + sinis) * cosok; final double delta_xnode = MathUtils.normalizeAngle(Math.atan2(alfdp, betdp) - xnode, 0); final double dls = -xnode * sinis * pinc; omgadf += dls - cosis * delta_xnode; xnode += delta_xnode; } }
From source file:org.geowebcache.service.kml.KMLService.java
private static double[] getGeographic(double x, double y, double z) { double theta, phi, radius; radius = distance(new double[] { x, y, z }, new double[] { 0, 0, 0 }); theta = Math.atan2(Math.sqrt(x * x + y * y), z); phi = Math.atan2(y, x);/* www . j a va 2s . co m*/ double lat = 90 - (theta * 180 / Math.PI); double lon = 90 - (phi * 180 / Math.PI); return new double[] { (lon > 180 ? lon - 360 : lon), lat, radius }; }
From source file:org.deegree.geometry.linearization.CurveLinearizer.java
private Points interpolate(Point p0, Point p1, Point p2, int numPoints, boolean isCircle) { // shift the points down (to reduce the occurrence of floating point errors), independently on the x and y axes double minOrd0 = findShiftOrd0(p0, p1, p2); double minOrd1 = findShiftOrd1(p0, p1, p2); // if the points are already shifted, this does no harm! Point p0Shifted = new DefaultPoint(null, p0.getCoordinateSystem(), p0.getPrecision(), new double[] { p0.get0() - minOrd0, p0.get1() - minOrd1 }); Point p1Shifted = new DefaultPoint(null, p1.getCoordinateSystem(), p1.getPrecision(), new double[] { p1.get0() - minOrd0, p1.get1() - minOrd1 }); Point p2Shifted = new DefaultPoint(null, p2.getCoordinateSystem(), p2.getPrecision(), new double[] { p2.get0() - minOrd0, p2.get1() - minOrd1 }); List<Point> interpolationPoints = new ArrayList<Point>(numPoints); Point center = calcCircleCenter(p0Shifted, p1Shifted, p2Shifted); double centerX = center.get0(); double centerY = center.get1(); double dx = p0Shifted.get0() - centerX; double dy = p0Shifted.get1() - centerY; double ex = p2Shifted.get0() - centerX; double ey = p2Shifted.get1() - centerY; double startAngle = Math.atan2(dy, dx); double endAngle = isCircle ? startAngle : Math.atan2(ey, ex); double radius = Math.sqrt(dx * dx + dy * dy); double angleStep = createAngleStep(startAngle, endAngle, numPoints, isClockwise(p0Shifted, p1Shifted, p2Shifted)); ICRS crs = p0Shifted.getCoordinateSystem(); // ensure numerical stability for start point (= use original circle start point) interpolationPoints.add(p0Shifted);/*from w w w . j a v a2 s . c o m*/ // calculate intermediate (=interpolated) points on arc for (int i = 1; i < numPoints - 1; i++) { double angle = startAngle + i * angleStep; double x = centerX + Math.cos(angle) * radius; double y = centerY + Math.sin(angle) * radius; interpolationPoints.add(geomFac.createPoint(null, new double[] { x, y }, crs)); } // ensure numerical stability for end point (= use original circle start point) interpolationPoints.add(isCircle ? p0Shifted : p2Shifted); // shift the points back up List<Point> realPoints = new ArrayList<Point>(interpolationPoints.size()); for (Point p : interpolationPoints) { realPoints.add(new DefaultPoint(null, p.getCoordinateSystem(), p.getPrecision(), new double[] { p.get0() + minOrd0, p.get1() + minOrd1 })); } return new PointsList(realPoints); }
From source file:es.emergya.ui.gis.CustomMapView.java
/** * Si algun {@link Marker} esta en {@link CustomMapView#follow} lo sigue, * gira y hace zoom si corresponde./*from w ww . ja v a 2s . c om*/ */ protected void follow() { if (lastFollowPos == null) { lastFollowPos = follow.eastNorth; } else { // distancias double dy = follow.eastNorth.getY() - lastFollowPos.getY(); double dx = follow.eastNorth.getX() - lastFollowPos.getX(); // si se ha movido lo bastante... if (Math.abs(dx) > FOLLOW_THRESSHOLD || Math.abs(dy) > FOLLOW_THRESSHOLD) { if (autoTurn) { double na = Math.atan2(dy, dx); na -= lastFollowAngle; // Normalize the angle if (na < -Math.PI) { na += PI2; } if (na > Math.PI) { na -= PI2; } if (smoothTurn) { final double target = na; // para usarlo enel thread if (turner != null && turner.isAlive()) // paramos el // thread si // vamos a girar // otra vez { turner.interrupt(); } turner = new Thread(new Runnable() { public void run() { double na = target; while (Math.abs(na) > 0) { // vamos girando 0.5 // de lo que // queremos na /= 2; lastFollowAngle += na; zoomToFactor(center, zoomFactor, PI_MEDIO - lastFollowAngle); try { Thread.sleep(60); } catch (InterruptedException e) { } } } }); turner.start(); } else { lastFollowAngle += na; // giro brusco } // Normalize the angle if (lastFollowAngle < 0) { lastFollowAngle += PI2; } if (lastFollowAngle > PI2) { lastFollowAngle -= PI2; } } else { lastFollowAngle = -getAngle() + PI_MEDIO; // deshacemos el // giro si no se // gira } if (autoZoom) { // Adjusts the current zoom factor to match an appropiate // zoom // level for the speed of the followed object double dist = (lastDistance + Main.proj.eastNorth2latlon(follow.eastNorth) .greatCircleDistance(Main.proj.eastNorth2latlon(lastFollowPos))) / 2; LatLon ll1 = getLatLon(0, 0); LatLon ll2 = getLatLon((int) (40 / fps), 0); double sampledist = ll1.greatCircleDistance(ll2); // get a // sample distance of 20px on the screen if (dist > sampledist * 1.5) { // if we move more than +-50% // of 20px -> zoom out zoomFactor = Math.max(zoomFactor - 1, getMinZoom()); ; } else if (dist < sampledist * 0.5) { // if we move less // than +-50% of // 20px -> zoom in zoomFactor = Math.min(zoomFactor + 1, getMaxZoom()); ; } lastDistance = dist; } zoomToFactor(follow.eastNorth, zoomFactor, PI_MEDIO - lastFollowAngle); lastFollowPos = new EastNorth(follow.eastNorth.east(), follow.eastNorth.north()); // zoomTo(follow.eastNorth, getScale(), PI_MEDIO - // lastFollowAngle); } } }
From source file:Rotation.java
/** Get the Cardan or Euler angles corresponding to the instance. /*from ww w.ja v a 2 s .c o m*/ * <p>The equations show that each rotation can be defined by two * different values of the Cardan or Euler angles set. For example * if Cardan angles are used, the rotation defined by the angles * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as * the rotation defined by the angles π + a<sub>1</sub>, π * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements * the following arbitrary choices:</p> * <ul> * <li>for Cardan angles, the chosen set is the one for which the * second angle is between -π/2 and π/2 (i.e its cosine is * positive),</li> * <li>for Euler angles, the chosen set is the one for which the * second angle is between 0 and π (i.e its sine is positive).</li> * </ul> * <p>Cardan and Euler angle have a very disappointing drawback: all * of them have singularities. This means that if the instance is * too close to the singularities corresponding to the given * rotation order, it will be impossible to retrieve the angles. For * Cardan angles, this is often called gimbal lock. There is * <em>nothing</em> to do to prevent this, it is an intrinsic problem * with Cardan and Euler representation (but not a problem with the * rotation itself, which is perfectly well defined). For Cardan * angles, singularities occur when the second angle is close to * -π/2 or +π/2, for Euler angle singularities occur when the * second angle is close to 0 or π, this implies that the identity * rotation is always singular for Euler angles!</p> * @param order rotation order to use * @return an array of three angles, in the order specified by the set * @exception CardanEulerSingularityException if the rotation is * singular with respect to the angles set specified */ public double[] getAngles(RotationOrder order) { if (order == RotationOrder.XYZ) { // r (Vector3D.plusK) coordinates are : // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) // (-r) (Vector3D.plusI) coordinates are : // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) // and we can choose to have theta in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusK); Vector3D v2 = applyInverseTo(Vector3D.plusI); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(-(v1.getY()), v1.getZ()), Math.asin(v2.getZ()), Math.atan2(-(v2.getY()), v2.getX()) }; } else if (order == RotationOrder.XZY) { // r (Vector3D.plusJ) coordinates are : // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) // (-r) (Vector3D.plusI) coordinates are : // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) // and we can choose to have psi in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusJ); Vector3D v2 = applyInverseTo(Vector3D.plusI); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getZ(), v1.getY()), -Math.asin(v2.getY()), Math.atan2(v2.getZ(), v2.getX()) }; } else if (order == RotationOrder.YXZ) { // r (Vector3D.plusK) coordinates are : // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) // (-r) (Vector3D.plusJ) coordinates are : // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) // and we can choose to have phi in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusK); Vector3D v2 = applyInverseTo(Vector3D.plusJ); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getX(), v1.getZ()), -Math.asin(v2.getZ()), Math.atan2(v2.getX(), v2.getY()) }; } else if (order == RotationOrder.YZX) { // r (Vector3D.plusI) coordinates are : // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) // (-r) (Vector3D.plusJ) coordinates are : // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) // and we can choose to have psi in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusI); Vector3D v2 = applyInverseTo(Vector3D.plusJ); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(-(v1.getZ()), v1.getX()), Math.asin(v2.getX()), Math.atan2(-(v2.getZ()), v2.getY()) }; } else if (order == RotationOrder.ZXY) { // r (Vector3D.plusJ) coordinates are : // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) // (-r) (Vector3D.plusK) coordinates are : // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) // and we can choose to have phi in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusJ); Vector3D v2 = applyInverseTo(Vector3D.plusK); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(-(v1.getX()), v1.getY()), Math.asin(v2.getY()), Math.atan2(-(v2.getX()), v2.getZ()) }; } else if (order == RotationOrder.ZYX) { // r (Vector3D.plusI) coordinates are : // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) // (-r) (Vector3D.plusK) coordinates are : // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) // and we can choose to have theta in the interval [-PI/2 ; +PI/2] Vector3D v1 = applyTo(Vector3D.plusI); Vector3D v2 = applyInverseTo(Vector3D.plusK); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getY(), v1.getX()), -Math.asin(v2.getX()), Math.atan2(v2.getY(), v2.getZ()) }; } else if (order == RotationOrder.XYX) { // r (Vector3D.plusI) coordinates are : // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) // (-r) (Vector3D.plusI) coordinates are : // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) // and we can choose to have theta in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusI); Vector3D v2 = applyInverseTo(Vector3D.plusI); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getY(), -v1.getZ()), Math.acos(v2.getX()), Math.atan2(v2.getY(), v2.getZ()) }; } else if (order == RotationOrder.XZX) { // r (Vector3D.plusI) coordinates are : // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) // (-r) (Vector3D.plusI) coordinates are : // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) // and we can choose to have psi in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusI); Vector3D v2 = applyInverseTo(Vector3D.plusI); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getZ(), v1.getY()), Math.acos(v2.getX()), Math.atan2(v2.getZ(), -v2.getY()) }; } else if (order == RotationOrder.YXY) { // r (Vector3D.plusJ) coordinates are : // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) // (-r) (Vector3D.plusJ) coordinates are : // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) // and we can choose to have phi in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusJ); Vector3D v2 = applyInverseTo(Vector3D.plusJ); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getX(), v1.getZ()), Math.acos(v2.getY()), Math.atan2(v2.getX(), -v2.getZ()) }; } else if (order == RotationOrder.YZY) { // r (Vector3D.plusJ) coordinates are : // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) // (-r) (Vector3D.plusJ) coordinates are : // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) // and we can choose to have psi in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusJ); Vector3D v2 = applyInverseTo(Vector3D.plusJ); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getZ(), -v1.getX()), Math.acos(v2.getY()), Math.atan2(v2.getZ(), v2.getX()) }; } else if (order == RotationOrder.ZXZ) { // r (Vector3D.plusK) coordinates are : // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) // (-r) (Vector3D.plusK) coordinates are : // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) // and we can choose to have phi in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusK); Vector3D v2 = applyInverseTo(Vector3D.plusK); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { System.out.println("CardanEulerSingularityException"); } return new double[] { Math.atan2(v1.getX(), -v1.getY()), Math.acos(v2.getZ()), Math.atan2(v2.getX(), v2.getY()) }; } else { // last possibility is ZYZ // r (Vector3D.plusK) coordinates are : // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) // (-r) (Vector3D.plusK) coordinates are : // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) // and we can choose to have theta in the interval [0 ; PI] Vector3D v1 = applyTo(Vector3D.plusK); Vector3D v2 = applyInverseTo(Vector3D.plusK); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { throw new RuntimeException("false"); } return new double[] { Math.atan2(v1.getY(), v1.getX()), Math.acos(v2.getZ()), Math.atan2(v2.getY(), -v2.getX()) }; } }
From source file:biogenesis.Organism.java
/** * Calculates the position of all organism points in the world, depending on * its rotation. It also calculates the bounding rectangle of the organism. * This method must be called from outside this class only when doing * manual drawing. /*from ww w . j a va2 s. c o m*/ * * @param force To avoid calculations, segments position are only calculated * if the organism's rotation has changed in the last frame. If it is necessary * to calculate them even when the rotation hasn't changed, assign true to this * parameter. */ public void calculateBounds(boolean force) { double left = java.lang.Double.MAX_VALUE, right = java.lang.Double.MIN_VALUE, top = java.lang.Double.MAX_VALUE, bottom = java.lang.Double.MIN_VALUE; double theta; for (int i = _segments - 1; i >= 0; i--) { /* Save calculation: if rotation hasn't changed and it is not forced, * don't calculate points again. */ if (_lastTheta != _theta || force) { theta = _theta + Math.atan2(_startPointY[i], _startPointX[i]); x1[i] = (int) (_m1[i] * Math.cos(theta)); y1[i] = (int) (_m1[i] * Math.sin(theta)); theta = _theta + Math.atan2(_endPointY[i], _endPointX[i]); x2[i] = (int) (_m2[i] * Math.cos(theta)); y2[i] = (int) (_m2[i] * Math.sin(theta)); } // Finds the rectangle that comprises the organism left = Utils.min(left, x1[i] + _dCenterX, x2[i] + _dCenterX); right = Utils.max(right, x1[i] + _dCenterX, x2[i] + _dCenterX); top = Utils.min(top, y1[i] + _dCenterY, y2[i] + _dCenterY); bottom = Utils.max(bottom, y1[i] + _dCenterY, y2[i] + _dCenterY); } setBounds((int) left, (int) top, (int) (right - left + 1) + 1, (int) (bottom - top + 1) + 1); _lastTheta = _theta; }