Example usage for org.apache.commons.math3.geometry.euclidean.threed Vector3D orthogonal

List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D orthogonal

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.threed Vector3D orthogonal.

Prototype

public Vector3D orthogonal() throws MathArithmeticException 

Source Link

Document

Get a vector orthogonal to the instance.

Usage

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
 *///from   w  w w. j a v  a 2 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.orbits.CartesianParametersTest.java

@Test
public void testShiftCircular() {
    Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
    Vector3D velocity = new Vector3D(FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
    PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
    CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
    testShift(orbit, new CircularOrbit(orbit), 1.0e-15);
}

From source file:org.orekit.orbits.CartesianParametersTest.java

@Test
public void testShiftHyperbolic() {
    Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
    Vector3D velocity = new Vector3D(3 * FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
    PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
    CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
    testShift(orbit, new KeplerianOrbit(orbit), 1.0e-15);
}