From b5c6c200d60aae805614495512ab24eafcce1fb9 Mon Sep 17 00:00:00 2001
From: lizhanwei <Dana_Lee1016@126.com>
Date: 星期四, 02 四月 2020 17:53:45 +0800
Subject: [PATCH] 摆正考试地图

---
 app/src/main/java/safeluck/drive/evaluation/app.java                          |   14 
 app/src/main/java/safeluck/drive/evaluation/DB/rtktb/RTKConfigUpdateWorker.kt |   64 ++++++++
 app/src/main/java/safeluck/drive/evaluation/util/FileUtil.java                |   53 ++++++
 app/src/main/java/safeluck/drive/evaluation/util/Utils.java                   |   47 +++++
 app/src/main/java/safeluck/drive/evaluation/httpmodule/HttpCarRespBean.java   |  181 ++++++++++++++++++++++
 app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java         |   75 +++++++--
 6 files changed, 405 insertions(+), 29 deletions(-)

diff --git a/app/src/main/java/safeluck/drive/evaluation/DB/rtktb/RTKConfigUpdateWorker.kt b/app/src/main/java/safeluck/drive/evaluation/DB/rtktb/RTKConfigUpdateWorker.kt
index 8c0ce4a..cf6a15f 100644
--- a/app/src/main/java/safeluck/drive/evaluation/DB/rtktb/RTKConfigUpdateWorker.kt
+++ b/app/src/main/java/safeluck/drive/evaluation/DB/rtktb/RTKConfigUpdateWorker.kt
@@ -1,20 +1,31 @@
 package safeluck.drive.evaluation.DB.rtktb
 
 import android.content.Context
+import android.os.Looper
 import android.text.TextUtils
 import android.util.Log
+import android.widget.Toast
 import androidx.work.Worker
 import androidx.work.WorkerParameters
 import com.anyun.exam.lib.AYSdk
 import com.anyun.exam.lib.MyLog
 import com.google.gson.Gson
+import okhttp3.ResponseBody
 import org.json.JSONException
 import org.json.JSONObject
+import retrofit2.Call
+import retrofit2.Callback
+import retrofit2.Response
 import safeluck.drive.evaluation.DB.Constant
 import safeluck.drive.evaluation.DB.WorkRoomDataBase
 import safeluck.drive.evaluation.bean.ExamPlatformData
+import safeluck.drive.evaluation.httpmodule.HttpCarRespBean
+import safeluck.drive.evaluation.httpmodule.HttpRequetBean
+import safeluck.drive.evaluation.httpmodule.RetrofitCreator.Companion.getRestService
 import safeluck.drive.evaluation.im.IMSClientBootstrap
 import safeluck.drive.evaluation.platformMessage.PlatFormConstant
+import safeluck.drive.evaluation.util.FileUtil
+import java.io.IOException
 
 class RTKConfigUpdateWorker(context:Context, workerParams: WorkerParameters) :Worker(context, workerParams){
 
@@ -43,6 +54,59 @@
              */
             phone = sn
             Log.i(TAG,"phone="+phone)
+
+            if (!FileUtil.isMapExist(applicationContext,"map.json")){
+                MyLog.i("鍦板浘鏂囦欢涓嶅瓨鍦紝鍘诲钩鍙板彇")
+                getRestService().getMapsAndVehicleModel(HttpRequetBean(sn, "place")).enqueue(object : Callback<ResponseBody> {
+                    override fun onResponse(call: Call<ResponseBody>, response: Response<ResponseBody>) {
+                        var str: String? = null
+                        try {
+                            str = response.body()!!.string()
+                        } catch (e: IOException) {
+                            e.printStackTrace()
+                        }
+                        Log.i(TAG, "鏈嶅姟鍣ㄨ繑鍥炵殑body=$str")
+                        if (Looper.myLooper() == Looper.getMainLooper()) {
+                            Toast.makeText(applicationContext, "涓荤嚎绋�", Toast.LENGTH_SHORT).show()
+                        }
+                    }
+
+                    override fun onFailure(call: Call<ResponseBody>, t: Throwable) {
+                        Log.i(TAG, t.localizedMessage)
+                    }
+                })
+            }
+            if (!FileUtil.isMapExist(applicationContext,"car.json")){
+                MyLog.i("杞﹁締妯″瀷鏂囦欢涓嶅瓨鍦紝鍘诲钩鍙板彇")
+                getRestService().getMapsAndVehicleModel(HttpRequetBean(sn, "car")).enqueue(object : Callback<ResponseBody> {
+                    override fun onResponse(call: Call<ResponseBody>, response: Response<ResponseBody>) {
+                        var str: String? = null
+                        try {
+                            str = response.body()!!.string()
+                            Log.i(TAG, "鏈嶅姟鍣ㄨ繑鍥炵殑body=$str")
+                            val car = gson.fromJson<HttpCarRespBean>(str,HttpCarRespBean::class.java)
+                            if (car.isResult){
+                                if (car.data !=null){
+                                    str = gson.toJson(car.data.map_json)
+                                    Log.i(TAG, "瑙f瀽涔嬪悗鐨勮溅妯″瀷=$str")
+                                    FileUtil.writeTxtFileToSD(applicationContext,"car.json",str,"")
+                                }
+                            }
+                        } catch (e: IOException) {
+                            e.printStackTrace()
+                        }
+
+                        if (Looper.myLooper() == Looper.getMainLooper()) {
+                            Toast.makeText(applicationContext, "涓荤嚎绋�", Toast.LENGTH_SHORT).show()
+                        }
+                    }
+
+                    override fun onFailure(call: Call<ResponseBody>, t: Throwable) {
+                        Log.i(TAG, t.localizedMessage)
+                    }
+                })
+            }
+
             MyLog.i(PlatFormConstant.TAG,"閲嶇疆TCP杩炴帴")
             IMSClientBootstrap.getInstance().resetConnect();
         }
diff --git a/app/src/main/java/safeluck/drive/evaluation/app.java b/app/src/main/java/safeluck/drive/evaluation/app.java
index abba3b8..548d988 100644
--- a/app/src/main/java/safeluck/drive/evaluation/app.java
+++ b/app/src/main/java/safeluck/drive/evaluation/app.java
@@ -126,7 +126,6 @@
             //2. 濡傛灉涓嶈兘鍐嶅氨鍘讳笅杞�
 
 
-
         }
 
     }
@@ -162,8 +161,6 @@
         }
         switch (cmd) {
             case Constant.NDK_START:
-//                sendVechileInfo();
-//                sendMapInfo();
                 OneTimeWorkRequest examStatausOutWorker = OneTimeWorkRequest.from(ExamStatusOutWorker.class);
                 OneTimeWorkRequest mcuUpgradeWorker = OneTimeWorkRequest.from(MCUUpgradeWorker.class);
                 WorkManager.getInstance(getApplicationContext()).beginWith(examStatausOutWorker).then(mcuUpgradeWorker).enqueue();
@@ -273,15 +270,18 @@
                 try {
                     JSONObject jsonObject =new JSONObject((String)json);
                     int errCode = jsonObject.getInt("error");
+                    MyLog.i("鏀跺埌鑰冭瘯搴旂瓟閿欒鐮�="+errCode);
                     switch (errCode){
                         case -1:
-                            sendMapInfo();
-                            break;
+//                            sendMapInfo();
+//                            break;
                         case -2:
 
-                            sendVechileInfo();
-                            break;
+//                            sendVechileInfo();
+//                            break;
                         case -3:
+                            OneTimeWorkRequest examStatausOutWorker1 = OneTimeWorkRequest.from(ExamStatusOutWorker.class);
+                            WorkManager.getInstance(getApplicationContext()).enqueue(examStatausOutWorker1);
                             break;
                     }
                 } catch (JSONException e) {
diff --git a/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java b/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
index 8e8137d..187d6e5 100644
--- a/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
+++ b/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
@@ -46,11 +46,13 @@
 import safeluck.drive.evaluation.R;
 import safeluck.drive.evaluation.bean.ExamMap;
 import safeluck.drive.evaluation.bean.ExamPlatformData;
+import safeluck.drive.evaluation.bean.Point;
 import safeluck.drive.evaluation.bean.RealTimeCarPos;
 import safeluck.drive.evaluation.cEventCenter.CEventCenter;
 import safeluck.drive.evaluation.cEventCenter.ICEventListener;
 import safeluck.drive.evaluation.util.CThreadPoolExecutor;
 import safeluck.drive.evaluation.util.FileUtil;
+import safeluck.drive.evaluation.util.Utils;
 
 
 /**
@@ -214,13 +216,10 @@
         if (canvas2 == null || bmp == null) {
             return;
         }
-//        _mActivity.runOnUiThread(new Runnable() {
-//            @Override
-//            public void run() {
                 double base_x = 300, base_y = 20;
                 double max_x = 0, min_x = 0, max_y = 0, min_y = 0;
 
-                Log.d(TAG, "DrawMap map size " + map.length + " car size " + car.length);
+                Log.d(TAG, "DrawMap map size " + map.length + " car size " + car.length+"map other length="+map_other.length);
 
                 for (int i = 0; i < map.length; i++) {
                     if (i == 0) {
@@ -264,20 +263,20 @@
 
                 Log.d(TAG, "DrawMap max_x " + max_x + " max_y " + max_y + " min_x " + min_x + " min_y " + min_y);
 
-                for (int i = 0; i < car.length; i++) {
-                    if (max_x < car[i][0]) {
-                        max_x = car[i][0];
-                    }
-                    if (min_x > car[i][0]) {
-                        min_x = car[i][0];
-                    }
-                    if (max_y < car[i][1]) {
-                        max_y = car[i][1];
-                    }
-                    if (min_y > car[i][1]) {
-                        min_y = car[i][1];
-                    }
-                }
+//                for (int i = 0; i < car.length; i++) {
+//                    if (max_x < car[i][0]) {
+//                        max_x = car[i][0];
+//                    }
+//                    if (min_x > car[i][0]) {
+//                        min_x = car[i][0];
+//                    }
+//                    if (max_y < car[i][1]) {
+//                        max_y = car[i][1];
+//                    }
+//                    if (min_y > car[i][1]) {
+//                        min_y = car[i][1];
+//                    }
+//                }
 
                 Log.d(TAG, "DrawMap max_x " + max_x + " max_y " + max_y + " min_x " + min_x + " min_y " + min_y);
 
@@ -291,6 +290,7 @@
                 }
 
                 Log.d(TAG, "DrawMap scale_x " + scale_x + " scale_y " + scale_y);
+
 
                 canvas2.drawColor(Color.WHITE);
 
@@ -752,6 +752,7 @@
                     map_id = timeCarPos.getMap_id();
 
                     List<Double> mainAnt = timeCarPos.getMain_ant();
+                    Point mainPoint = new Point(mainAnt.get(0), mainAnt.get(1));
                     List<Integer> tire1 = timeCarPos.getLeft_front_tire();
                     List<Integer> tire2 = timeCarPos.getRight_front_tire();
                     List<Integer> tire3 = timeCarPos.getLeft_rear_tire();
@@ -857,12 +858,48 @@
                                         if ((k % 2) == 0) {
                                             map[map_line][0] = xys.get(k);
                                         } else {
-                                            double value = 0 - xys.get(k);
+                                            double value = xys.get(k);
                                             Log.i(TAG, "onCEvent: map鏂板��=" + value + " 浣嶇疆锛�" + k);
                                             map[map_line][1] = value;
                                             map_line++;
                                         }
                                     }
+                                    Log.i(TAG,String.format("涓嬮潰寮�濮嬫棆杞紝map.length=%d",map.length));
+                                    Point point2 = new Point(map[0][0],map[0][1]);
+                                    double deg = 0.0;
+                                    switch (map.length){
+
+                                        case 8:
+                                            Point point1 = new Point(map[map.length-1][0],map[map.length-1][1]);
+                                            deg = Utils.YawOf(point2,point1);
+                                            deg +=270;
+                                            for (int k = 0; k < map.length; k++) {
+                                                Point point11 = new Point(map[k][0],map[k][1]);
+                                                Point point = Utils.rotatePoint(point11,point2,deg);
+                                                map[k][0] = point.getX();
+                                                map[k][1] = 0-point.getY();
+                                                Log.i(TAG,String.format("鏃嬭浆鍚庣殑鍊糾ap[%d][0]=%f,map[%d][1]=%f,deg=%f",k,map[k][0],k,map[k][1],deg));
+                                            }
+                                            break;
+                                        case 9:
+                                            break;
+                                        case 6:
+                                            break;
+                                            default:
+                                                break;
+                                    }
+                                    for (int k = 0; k < car.length; k++) {
+                                        Point oldPoint = new Point(car[k][0], car[k][1]);
+                                        Point newPoint = Utils.rotatePoint(oldPoint, point2, deg );
+                                        car[k][0] = newPoint.getX();
+                                        car[k][1] = newPoint.getY();
+
+//                                        car[k][0] = car[k][0] - point2.getX();
+//                                        car[k][1] = car[k][1] - point2.getY();
+//                                        car[k][1] = -car[k][1];
+                                        Log.i(TAG,String.format("鏃嬭浆鍚庣殑鍊糲ar[%d][0]=%f,car[%d][1]=%f,deg=%f",k,car[k][0],k,car[k][1],deg));
+                                    }
+
                                 }else{
                                     MyLog.i(TAG,"鏆傛椂鏀寔鏇茬嚎");
                                     map_other = new double[xys.size()/2][2];
diff --git a/app/src/main/java/safeluck/drive/evaluation/httpmodule/HttpCarRespBean.java b/app/src/main/java/safeluck/drive/evaluation/httpmodule/HttpCarRespBean.java
new file mode 100644
index 0000000..ff57c1e
--- /dev/null
+++ b/app/src/main/java/safeluck/drive/evaluation/httpmodule/HttpCarRespBean.java
@@ -0,0 +1,181 @@
+package safeluck.drive.evaluation.httpmodule;
+
+import java.util.List;
+
+public class HttpCarRespBean {
+    /**
+     * result : true
+     * data : {"server":"gps.safeluck.com","port":"3301","map_json":{"axial":[0,12],"left_front_tire":[8,9],"main_ant":[-9151.5063,13030.5118],"right_front_tire":[20,21],"right_rear_tire":[22,23],"name":"绉戜簩鍦鸿�冭溅妯″瀷","left_rear_tire":[10,11],"type":"car","body":[0,1,2,3,4,5,6,7,12,13,14,15,16,17,18,19],"point":[-9149.6063,13033.0768,-9149.9978,13033.3124,-9150.3258,13033.3485,-9150.8341,13032.8985,-9151.7318,13031.6639,-9152.3573,13030.83,-9152.5214,13030.5639,-9152.602,13030.3105,-9150.8053,13032.8843,-9150.6537,13032.7602,-9152.3352,13030.8124,-9152.221,13030.7008,-9152.092,13029.7892,-9151.4638,13029.4333,-9151.2111,13029.568,-9150.9808,13029.8341,-9150.3546,13030.6796,-9149.458,13031.8642,-9149.1611,13032.4276,-9149.2827,13032.7653,-9149.4842,13031.8708,-9149.6176,13031.9937,-9151.0011,13029.8717,-9151.1367,13029.9878]},"pzh":"璞獷8000娴�"}
+     */
+
+    private boolean result;
+    private DataBean data;
+
+    public boolean isResult() {
+        return result;
+    }
+
+    public void setResult(boolean result) {
+        this.result = result;
+    }
+
+    public DataBean getData() {
+        return data;
+    }
+
+    public void setData(DataBean data) {
+        this.data = data;
+    }
+
+    public static class DataBean {
+        /**
+         * server : gps.safeluck.com
+         * port : 3301
+         * map_json : {"axial":[0,12],"left_front_tire":[8,9],"main_ant":[-9151.5063,13030.5118],"right_front_tire":[20,21],"right_rear_tire":[22,23],"name":"绉戜簩鍦鸿�冭溅妯″瀷","left_rear_tire":[10,11],"type":"car","body":[0,1,2,3,4,5,6,7,12,13,14,15,16,17,18,19],"point":[-9149.6063,13033.0768,-9149.9978,13033.3124,-9150.3258,13033.3485,-9150.8341,13032.8985,-9151.7318,13031.6639,-9152.3573,13030.83,-9152.5214,13030.5639,-9152.602,13030.3105,-9150.8053,13032.8843,-9150.6537,13032.7602,-9152.3352,13030.8124,-9152.221,13030.7008,-9152.092,13029.7892,-9151.4638,13029.4333,-9151.2111,13029.568,-9150.9808,13029.8341,-9150.3546,13030.6796,-9149.458,13031.8642,-9149.1611,13032.4276,-9149.2827,13032.7653,-9149.4842,13031.8708,-9149.6176,13031.9937,-9151.0011,13029.8717,-9151.1367,13029.9878]}
+         * pzh : 璞獷8000娴�
+         */
+
+        private String server;
+        private String port;
+        private MapJsonBean map_json;
+        private String pzh;
+
+        public String getServer() {
+            return server;
+        }
+
+        public void setServer(String server) {
+            this.server = server;
+        }
+
+        public String getPort() {
+            return port;
+        }
+
+        public void setPort(String port) {
+            this.port = port;
+        }
+
+        public MapJsonBean getMap_json() {
+            return map_json;
+        }
+
+        public void setMap_json(MapJsonBean map_json) {
+            this.map_json = map_json;
+        }
+
+        public String getPzh() {
+            return pzh;
+        }
+
+        public void setPzh(String pzh) {
+            this.pzh = pzh;
+        }
+
+        public static class MapJsonBean {
+            /**
+             * axial : [0,12]
+             * left_front_tire : [8,9]
+             * main_ant : [-9151.5063,13030.5118]
+             * right_front_tire : [20,21]
+             * right_rear_tire : [22,23]
+             * name : 绉戜簩鍦鸿�冭溅妯″瀷
+             * left_rear_tire : [10,11]
+             * type : car
+             * body : [0,1,2,3,4,5,6,7,12,13,14,15,16,17,18,19]
+             * point : [-9149.6063,13033.0768,-9149.9978,13033.3124,-9150.3258,13033.3485,-9150.8341,13032.8985,-9151.7318,13031.6639,-9152.3573,13030.83,-9152.5214,13030.5639,-9152.602,13030.3105,-9150.8053,13032.8843,-9150.6537,13032.7602,-9152.3352,13030.8124,-9152.221,13030.7008,-9152.092,13029.7892,-9151.4638,13029.4333,-9151.2111,13029.568,-9150.9808,13029.8341,-9150.3546,13030.6796,-9149.458,13031.8642,-9149.1611,13032.4276,-9149.2827,13032.7653,-9149.4842,13031.8708,-9149.6176,13031.9937,-9151.0011,13029.8717,-9151.1367,13029.9878]
+             */
+
+            private String name;
+            private String type;
+            private List<Double> axial;
+            private List<Double> left_front_tire;
+            private List<Double> main_ant;
+            private List<Double> right_front_tire;
+            private List<Double> right_rear_tire;
+            private List<Double> left_rear_tire;
+            private List<Double> body;
+            private List<Double> point;
+
+            public String getName() {
+                return name;
+            }
+
+            public void setName(String name) {
+                this.name = name;
+            }
+
+            public String getType() {
+                return type;
+            }
+
+            public void setType(String type) {
+                this.type = type;
+            }
+
+            public List<Double> getAxial() {
+                return axial;
+            }
+
+            public void setAxial(List<Double> axial) {
+                this.axial = axial;
+            }
+
+            public List<Double> getLeft_front_tire() {
+                return left_front_tire;
+            }
+
+            public void setLeft_front_tire(List<Double> left_front_tire) {
+                this.left_front_tire = left_front_tire;
+            }
+
+            public List<Double> getMain_ant() {
+                return main_ant;
+            }
+
+            public void setMain_ant(List<Double> main_ant) {
+                this.main_ant = main_ant;
+            }
+
+            public List<Double> getRight_front_tire() {
+                return right_front_tire;
+            }
+
+            public void setRight_front_tire(List<Double> right_front_tire) {
+                this.right_front_tire = right_front_tire;
+            }
+
+            public List<Double> getRight_rear_tire() {
+                return right_rear_tire;
+            }
+
+            public void setRight_rear_tire(List<Double> right_rear_tire) {
+                this.right_rear_tire = right_rear_tire;
+            }
+
+            public List<Double> getLeft_rear_tire() {
+                return left_rear_tire;
+            }
+
+            public void setLeft_rear_tire(List<Double> left_rear_tire) {
+                this.left_rear_tire = left_rear_tire;
+            }
+
+            public List<Double> getBody() {
+                return body;
+            }
+
+            public void setBody(List<Double> body) {
+                this.body = body;
+            }
+
+            public List<Double> getPoint() {
+                return point;
+            }
+
+            public void setPoint(List<Double> point) {
+                this.point = point;
+            }
+        }
+    }
+}
diff --git a/app/src/main/java/safeluck/drive/evaluation/util/FileUtil.java b/app/src/main/java/safeluck/drive/evaluation/util/FileUtil.java
index 49fa4d9..262f447 100644
--- a/app/src/main/java/safeluck/drive/evaluation/util/FileUtil.java
+++ b/app/src/main/java/safeluck/drive/evaluation/util/FileUtil.java
@@ -10,6 +10,7 @@
 import com.anyun.exam.lib.MyLog;
 import com.anyun.exam.lib.util.ByteUtil;
 
+import java.io.BufferedOutputStream;
 import java.io.BufferedReader;
 import java.io.ByteArrayOutputStream;
 import java.io.File;
@@ -69,6 +70,16 @@
             Log.i(TAG, "createdirs: 鐩綍宸茬粡瀛樺湪");
         }
 
+    }
+
+    public static boolean isMapExist(Context context,String mapName){
+        File file = new File(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+context.getPackageName(),mapName);
+
+        if (file.exists()){
+            return true;
+        }else{
+            return false;
+        }
     }
 
     public static StringBuffer readTxtFileFromSD(Context context,String fileName,boolean isRootDir){
@@ -317,4 +328,46 @@
         }
         return data;
     }
+    public static void main(String[] args){
+    }
+
+
+    /**
+     * 鏍圭洰褰曚笅+"鍖呭悕"鐨勮矾寰勪笅鐨勫瓙璺緞
+     * @param context
+     * @param subDir
+     */
+    public static void createdirs(Context context,String subDir){
+        String dir = Environment.getExternalStorageDirectory().getAbsolutePath()+"/"+context.getPackageName()+"/";
+        if (!TextUtils.isEmpty(subDir)){
+            dir += subDir+"/";
+        }
+        File file = new File(dir);
+        if (!file.exists()){
+            file.mkdirs();
+
+        }else{
+            Log.i(TAG, "createdirs: 鐩綍宸茬粡瀛樺湪");
+        }
+
+    }
+    public static void writeTxtFileToSD(Context context, String fileName, String content,String subDir){
+        Log.i(TAG, "writeTxtFileToSD: content:"+content);
+        createdirs(context,subDir);
+        String dir = Environment.getExternalStorageDirectory().getAbsolutePath()+"/"+context.getPackageName()+"/"+subDir+"/";
+        File file = new File(dir,fileName);
+        if (file.exists()){
+            file.delete();
+        }
+        try {
+            OutputStream outputStream = new FileOutputStream(file);
+            BufferedOutputStream bufferedOutputStream = new BufferedOutputStream(outputStream);
+            bufferedOutputStream.write(content.getBytes());
+            bufferedOutputStream.flush();
+            outputStream.close();
+            bufferedOutputStream.close();
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+    }
 }
diff --git a/app/src/main/java/safeluck/drive/evaluation/util/Utils.java b/app/src/main/java/safeluck/drive/evaluation/util/Utils.java
index 4098c9f..6336c9f 100644
--- a/app/src/main/java/safeluck/drive/evaluation/util/Utils.java
+++ b/app/src/main/java/safeluck/drive/evaluation/util/Utils.java
@@ -416,13 +416,13 @@
     private static boolean isEqual(double a,double b){
         return (Math.abs(a-b)<1e-3);
     }
-    private static Point rotatePoint(Point oldPoint,Point centre,double degree){
+    public static Point rotatePoint(Point oldPoint,Point centre,double degree){
 
         Point newPoint = new Point(0.0,0.0);
         newPoint.setX(getdouble((oldPoint.getX()-centre.getX())*cos(toRadians(degree)) -
-                (oldPoint.getY()-centre.getY())*sin(toRadians(degree)) + centre.getX(),6)) ;
+                (oldPoint.getY()-centre.getY())*sin(toRadians(degree)) + centre.getX(),4)) ;
         newPoint.setY(getdouble( (oldPoint.getX()-centre.getX())*sin(toRadians(degree))
-                + (oldPoint.getY()-centre.getY())*cos(toRadians(degree)) + centre.getY(),6));
+                + (oldPoint.getY()-centre.getY())*cos(toRadians(degree)) + centre.getY(),4));
         return newPoint;
     }
 
@@ -436,6 +436,47 @@
         return b.setScale(reserve, BigDecimal.ROUND_HALF_UP).doubleValue();
     }
 
+
+   public static double YawOf(Point p1, Point p2)
+    {
+        double deg = 0.0;
+
+        if (Math.abs(p1.getY() - p2.getY()) <= 1e-3) {
+            if (p1.getX() > p2.getX()) {
+                deg = 90;
+            } else {
+                deg = 270;
+            }
+        } else if (Math.abs(p1.getX() - p2.getX()) <= 1e-3) {
+            if (p1.getY() > p2.getY()) {
+                deg = 0;
+            } else {
+                deg = 180;
+            }
+        } else {
+            deg = Math.atan(Math.abs(p1.getX() - p2.getX()) /
+                    Math.abs(p1.getY() - p2.getY()));
+
+            deg = Math.toDegrees(deg);
+
+            if (p1.getX() > p2.getX() &&
+                    p1.getY() > p2.getY()) {
+
+            } else if (p1.getX() < p2.getX() &&
+                    p1.getY() > p2.getY()) {
+                deg = 360 - deg;
+            } else if (p1.getX() < p2.getX() &&
+                    p1.getY() < p2.getY()) {
+                deg = 180 + deg;
+            } else if (p1.getX() > p2.getX() &&
+                    p1.getY() < p2.getY()) {
+                deg = 180 - deg;
+            }
+        }
+
+        return deg;
+    }
+
     public static String formatTimeYYMMDDHHmmSS(long begin_time) {
         SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss");
 

--
Gitblit v1.8.0