Абсолютная ориентация датчика

Для дальнейших исследователей я столкнулся с той же проблемой. Основная проблема заключается в коэффициенте ошибок датчиков, чтобы напрямую установить положение и ориентацию вида на основе данных датчиков, вам необходимо рассчитать эти ошибки и добавить их в качестве значений смещения. Но, скорее всего, у нас есть World Wind, который сделает за нас большинство вычислений.

При использовании любого 3D-движка вы на самом деле не получаете никакой угловой информации, потому что у вас уже есть и глаза, и позиции взгляда. Вы можете рассчитать необходимые значения ориентации из этих позиций, а также управлять ими вручную и автоматически.

Здесь в моей функции устанавливается положение камеры в соответствии с заданными данными MISB KLV.

public void setCameraPosition(BTelemetryData pData){


    // Get Platform Location Information
    Angle tPlatformLatitude    = Angle.fromDegrees(Double.parseDouble(pData.getAlternatePlatformLatitude()));
    Angle tPlatformLongitude   = Angle.fromDegrees(Double.parseDouble(pData.getAlternatePlatformLongitude()));
    double tPlatformAltitude   = Double.parseDouble(pData.getPlatformGPSAltitude());
    Position tPlatfromPosition = new Position(tPlatformLatitude, tPlatformLongitude ,tPlatformAltitude);

    // Get LookAt Location Information
    Angle tLookAtLatitude = Angle.fromDegrees(Double.parseDouble(pData.getFrameCenterLatitude()));
    Angle tLookAtLongitude= Angle.fromDegrees(Double.parseDouble(pData.getFrameCenterLongitude()));
    // Note must take into account the surface elevation at given lat lon.
    double tLookAtAltitude= getWwd().getModel().getGlobe().getElevation(tLookAtLatitude, tLookAtLongitude);
    Position tLookAtPosition = new Position(tLookAtLatitude, tLookAtLongitude ,tLookAtAltitude);

    // First things first, we need to Set Field of View
    getView().setFieldOfView(Angle.fromDegrees(Double.parseDouble(pData.getSensorHorizontalFieldofView())));
    if (useAutoCameraPosition())
        setCameraPositionAutomatically(tLookAtPosition, tPlatfromPosition);
    else
        calculateAndSetCameraPosition(tLookAtPosition, tPlatfromPosition);

    getView().firePropertyChange(AVKey.VIEW, null, getView());

}

public void setCameraPositionAutomatically(Position pLookAtPosition, Position pEyePosition){
    getView().setEyePosition(pEyePosition);
    getView().setOrientation(pEyePosition, pLookAtPosition);
}
public void calculateAndSetCameraPosition(Position pLookAtPosition, Position pEyePosition){
        double tPitch   = getPitchAngleBetweenPositionInDegrees(pLookAtPosition, pEyePosition);
        double tHeading = getHeadingAngleBetweenPositionInDegrees(pLookAtPosition, pEyePosition);

        getView().setEyePosition(pEyePosition);
        getView().setHeading(Angle.fromDegrees(tHeading));
        getView().setPitch(Angle.fromDegrees(tPitch));
}


public double getPitchAngleBetweenPositionInDegrees(Position pLookAt, Position pEyePosition) {

    // Calculate the radius at given look at position
    double tRadius = getWwd().getModel().getGlobe().getRadiusAt(pLookAt);

    // Find the Surrounding Radial Length Between those positions
    double tRadialDistance = Position.greatCircleDistance(pLookAt, pEyePosition).getRadians() * tRadius;

    // Find the Ratio Between Distance, which will give the offset and Angle
    double tTheta = tRadialDistance / tRadius;

    // Get the surface elevation of lookatposition
    double tLookAtElevation = pLookAt.getElevation();

    // Get Altitude of given eye position
    double tEyeAltitude = pEyePosition.getAltitude();

    // Delta Location Changes in cartesian
    double tDeltaX  = (tRadius + tLookAtElevation) * Math.cos(tTheta);
    double tDeltaY  = (tRadius + tLookAtElevation) * Math.sin(tTheta);
    double tDeltaZ  =  tRadius + tEyeAltitude - tDeltaX;

    double alpha = Math.atan(tDeltaZ / tDeltaY)  - tTheta;

    // Convert NED to World Wind Coordinate System. The Pitch angle should be 90 - calculated.
    double degrees = 90 - Math.toDegrees(alpha); 

    System.out.println("Elevation Angle Between Positions = " + degrees);
    return degrees;
}

public  double getHeadingAngleBetweenPositionInDegrees(Position pLookAtPosition, Position  pEyePosition) {

    double tLatEye     = pEyePosition.getLatitude().radians;
    double tLatLookAt  = pLookAtPosition.getLatitude().radians;

    double tLonLookAt = pLookAtPosition.getLongitude().radians;
    double tLonEye    = pEyePosition.getLongitude().radians;
    double dLon = (tLonLookAt - tLonEye);

    double y = Math.sin(dLon) * Math.cos(tLatLookAt);
    double x = Math.cos(tLatEye) * Math.sin(tLatLookAt) - Math.sin(tLatEye)
             * Math.cos(tLatLookAt) * Math.cos(dLon);

    // Calculate the Bearing Angle. 
    double tBearing = Math.toDegrees(Math.atan2(y, x));

    // Calculate the absolute value of that Angle
    tBearing = (tBearing + 360) % 360;

    // Note that world wind takes the Heading in clockwise, if you want to make it counter clockwise, you need to subtract it from 360 degrees
   //tBearing = 360 - tBearing;

    return tBearing;
}

! //

В моем коде я не устанавливал угол крена, но, согласно документу, вы можете просто суммировать углы крена датчика и платформы, а затем установить угол крена.

Одно маленькое замечание: у World Wind есть два разных класса представления: BasicOrbitView и BasicFlyView, для имитации заданных данных вы должны использовать BasicFlyView. Причина этого в том, что в FlyView вы сохраняете положение камеры при установке углов, но, с другой стороны, в OrbitView вы сохраняете положение взгляда и изменяете как углы, так и положение камеры относительно этих углов. Вы можете использовать метод setOrientation, если вам достаточно точности.

Результат:

Хорошая кодировка :)