用于无人机风力巡检，生成的三维航点坐标

代码示例

package com.walkera.map.threedimensional;

import com.walkera.map.threedimensional.WtgPosBfBean;
import com.walkera.map.threedimensional.WtgPosEfBean;

import java.util.ArrayList;
import java.util.List;

/**
 * @author：Zach 创建日期：2019/11/25 15:18
 * 最后修改时间：
 * 类说明：风力检测生成的三维航点
 */
public class CreateThreeDimensionalPoints {

    public class Location {
        public int alt;
        public int lat;
        public int lng;
    }

    private static final float LOCATION_SCALING_FACTOR = 0.011131884502145034f;
    private static final float DEG_TO_RAD = 0.01745329f;

    private double longitudeScale(Location loc) {
        double scale = Math.cos(loc.lat * 1.0e-7f * DEG_TO_RAD);
        return ((scale) < (0.01f) ? (0.01f) : ((scale) > (1.0f) ? (1.0f) : (scale)));
    }

    private Location setLocationFromPosition(Location location, double posX, double posY, double posZ) {
        Location locDestination = new Location();
        locDestination.lat = (int) (location.lat + (posX / LOCATION_SCALING_FACTOR));
        locDestination.lng = (int) (location.lng + (posY / (LOCATION_SCALING_FACTOR * longitudeScale(location))));
        locDestination.alt = (int) ((location.alt + posZ) * 100);
        return locDestination;
    }

    /**
     * app 设置中心点坐标（center_location），风力发电机航向（WTG_yaw），风力发电机机体角度（WTG_frame_yaw）
     */
    public double WTGYaw = 0, WTGFrameYaw = 0;//单位度，范围0~360
    public Location centerLocation = new Location();
    private List<Location> missionWayPointLocation = new ArrayList<>();
    private double quaternion0, quaternion1, quaternion2, quaternion3;
    private List<WtgPosEfBean> wtgPosEfs = new ArrayList<>();
    private List<WtgPosBfBean> wtgPosBfs = new ArrayList<>();
    private double mWtgYaw, mWtgRoll;

    public void setCenterLocation(int lat, int lng, int alt, double wtgYaw, double wtgFrameYaw) {
        this.centerLocation.lat = lat;
        this.centerLocation.lng = lng;
        this.centerLocation.alt = alt;
        this.WTGYaw = wtgYaw;
        this.WTGFrameYaw = wtgFrameYaw;
    }

    public void getWtgMissionLocation() {
        mWtgYaw = WTGYaw / 57.29577952;
        mWtgRoll = WTGFrameYaw / 57.29577952;

        quaternion0 = Math.cos(0.5f * mWtgRoll) * Math.cos(0.5f * 0) * Math.cos(0.5f * mWtgYaw) + Math.sin(0.5f * mWtgRoll) * Math.sin(0.5f * 0) * Math.sin(0.5f * mWtgYaw);  //w
        quaternion1 = Math.sin(0.5f * mWtgRoll) * Math.cos(0.5f * 0) * Math.cos(0.5f * mWtgYaw) - Math.cos(0.5f * mWtgRoll) * Math.sin(0.5f * 0) * Math.sin(0.5f * mWtgYaw);  //x   绕x轴旋转是roll
        quaternion2 = Math.cos(0.5f * mWtgRoll) * Math.sin(0.5f * 0) * Math.cos(0.5f * mWtgYaw) + Math.sin(0.5f * mWtgRoll) * Math.cos(0.5f * 0) * Math.sin(0.5f * mWtgYaw);  //y   绕y轴旋转是pitch
        quaternion3 = Math.cos(0.5f * mWtgRoll) * Math.cos(0.5f * 0) * Math.sin(0.5f * mWtgYaw) - Math.sin(0.5f * mWtgRoll) * Math.sin(0.5f * 0) * Math.cos(0.5f * mWtgYaw);  //z   绕z轴旋转是Yaw

        double q0q0 = quaternion0 * quaternion0;
        double q0q1 = quaternion0 * quaternion1;
        double q0q2 = quaternion0 * quaternion2;
        double q0q3 = quaternion0 * quaternion3;
        double q1q1 = quaternion1 * quaternion1;
        double q1q2 = quaternion1 * quaternion2;
        double q1q3 = quaternion1 * quaternion3;
        double q2q2 = quaternion2 * quaternion2;
        double q2q3 = quaternion2 * quaternion3;
        double q3q3 = quaternion3 * quaternion3;

//        wtgPosBfs.add(new WtgPosBfBean(30, 0, 0));
//        wtgPosBfs.add(new WtgPosBfBean(30, 0, 75));
//        wtgPosBfs.add(new WtgPosBfBean(0, 0, 90));
//        wtgPosBfs.add(new WtgPosBfBean(-30, 0, 75));
//        wtgPosBfs.add(new WtgPosBfBean(-30, 0, 0));
//        wtgPosBfs.add(new WtgPosBfBean(-30, 65, -37.5));
//        wtgPosBfs.add(new WtgPosBfBean(0, 78, -45));
//        wtgPosBfs.add(new WtgPosBfBean(30, 65, -37.5));
//        wtgPosBfs.add(new WtgPosBfBean(30, 0, 0));
//        wtgPosBfs.add(new WtgPosBfBean(30, -65, -37.5));
//        wtgPosBfs.add(new WtgPosBfBean(0, -78, -45));
//        wtgPosBfs.add(new WtgPosBfBean(-30, -65, -37.5));
//        wtgPosBfs.add(new WtgPosBfBean(-30, 0, 0));

        wtgPosBfs.add(new WtgPosBfBean(20, 0, 0));
        wtgPosBfs.add(new WtgPosBfBean(20, 0, 65));

        wtgPosBfs.add(new WtgPosBfBean(0, 0, 80));
        wtgPosBfs.add(new WtgPosBfBean(-20, 0, 65));

        wtgPosBfs.add(new WtgPosBfBean(-20, 0, 0));
        wtgPosBfs.add(new WtgPosBfBean(-20, 56.3, -32.5));

        wtgPosBfs.add(new WtgPosBfBean(0, 69.3, -40));
        wtgPosBfs.add(new WtgPosBfBean(20, 56.3, -32.5));

        wtgPosBfs.add(new WtgPosBfBean(20, 0, 0));
        wtgPosBfs.add(new WtgPosBfBean(20, -56.3, -32.5));

        wtgPosBfs.add(new WtgPosBfBean(0, -69.3, -40));
        wtgPosBfs.add(new WtgPosBfBean(-20, -56.3, -32.5));

        wtgPosBfs.add(new WtgPosBfBean(-20, 0, 0));


        for (int i = 0; i < wtgPosBfs.size(); i++) {
            double x = wtgPosBfs.get(i).x * (q0q0 + q1q1 - q2q2 - q3q3) + wtgPosBfs.get(i).y * (q1q2 - q0q3) * 2 + wtgPosBfs.get(i).z * (q1q3 + q0q2) * 2;
            double y = wtgPosBfs.get(i).x * (q1q2 + q0q3) * 2 + wtgPosBfs.get(i).y * (q0q0 - q1q1 + q2q2 - q3q3) + wtgPosBfs.get(i).z * (q2q3 - q0q1) * 2;
            double z = wtgPosBfs.get(i).x * (q1q3 - q0q2) * 2 + wtgPosBfs.get(i).y * (q0q1 + q2q3) * 2 + wtgPosBfs.get(i).z * (q0q0 - q1q1 - q2q2 + q3q3);
            //生成航点发送给飞控
            wtgPosEfs.add(new WtgPosEfBean(x, y, z));
        }

        for (int i = 0; i < wtgPosEfs.size(); i++) {
            Location location = setLocationFromPosition(centerLocation, wtgPosEfs.get(i).x, wtgPosEfs.get(i).y, wtgPosEfs.get(i).z);
            missionWayPointLocation.add(location);
        }
    }

    public List<Location> missionWayPointLocation() {
        return missionWayPointLocation;
    }

    public void clearAllList() {
        if (wtgPosBfs != null) {
            wtgPosBfs.clear();
        }
        if (wtgPosEfs != null) {
            wtgPosEfs.clear();
        }
        if (missionWayPointLocation != null) {
            missionWayPointLocation.clear();
        }
    }
}

package com.walkera.map.threedimensional;

public class WtgPosBfBean {
    public double x;
    public double y;
    public double z;

    public WtgPosBfBean(double x, double y, double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }
}

package com.walkera.map.threedimensional;

public class WtgPosEfBean {
    public double x;
    public double y;
    public double z;

    public WtgPosEfBean(double x, double y, double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }
}

效果图：