package com.imdroid.sideslope.bd;

public class TiltUtil {
    public static double[] toPosition(Tilt tilt, Tilt tilt0, double[] position0, double r){
        if(tilt0 == null || position0 == null || r == 0 || tilt == null){
            return null;
        }
        //杆底相对与杆顶的位置
        double[] rodDownPosition = rodDownPosition(tilt0, r);
        //杆底相对于基站的位置
        double[] downPosition = new double[]{position0[0] + rodDownPosition[0], position0[1] + rodDownPosition[1], position0[2] + rodDownPosition[2]};
        //杆顶相对于杆底的位置
        double[] rodTopPosition = TiltUtil.rodTopPosition(tilt, r);
        //杆顶相对于基站的位置
        double[] topPosition = new double[]{downPosition[0] + rodTopPosition[0], downPosition[1] + rodTopPosition[1], downPosition[2] + rodTopPosition[2]};
        return topPosition;
    }


    /**
     * 计算得到杆顶点相对于杆底点的相对位置
     * @param tilt
     * @param r 杆的长度
     * @return
     */
    public static double[] rodTopPosition(Tilt tilt, double r){
        //假设yaw=0
        //杆在xoz平面投影
        double xoz = r * Math.cos(toRadians(tilt.getPitch()));
        double x = xoz * Math.cos(toRadians(90-tilt.getRoll()));
        double z = xoz * Math.cos(toRadians(tilt.getRoll()));
        //杆在yoz平面投影
        double yoz = r * Math.cos(toRadians(tilt.getRoll()));
        double y = -yoz * Math.cos(toRadians(90-tilt.getPitch()));

        //考虑到yaw，相当于逆时针旋转xoy坐标系
        double yaw = (tilt.getYaw() + 360)%360;
        double x1 = x*Math.cos(toRadians(yaw)) - y*Math.sin(toRadians(yaw));
        double y1 = y*Math.cos(toRadians(yaw)) + x*Math.sin(toRadians(yaw));

        return new double[]{x1,y1,z};
    }

    /**
     * 计算得到杆底相对于杆顶点的相对位置
     * @param tilt
     * @param r 杆的长度
     * @return
     */
    public static double[] rodDownPosition(Tilt tilt, double r){
        double[] doubles = rodTopPosition(tilt, r);
        return new double[]{-doubles[0],-doubles[1],-doubles[2]};
    }


    /**
     * 角度转弧度
     * @param angle
     * @return
     */
    public static double toRadians(double angle) {
        return angle / 180.0 * Math.PI;
    }
}

