/*********************************************
*
* class representing the DebugData Structure
*
* Author: Marcus -LiGi- Bueschleb
*
* see README for further Infos
*
* Some code taken from here:
* http://www.koders.com/java/fidFC75A641A87B51BB757E9CD3136C7886C491487F.aspx
*
* and
* http://www.movable-type.co.uk/scripts/latlong.html
*
* thanx a lot for sharing!
*
********************************************/
import java.lang.Math;
public class MKGPSPosition
{
public final byte GPS_FORMAT_DECIMAL=
0;
public final byte GPS_FORMAT_MINSEC=
1;
public final byte GPS_FORMAT_COUNT=
2;
byte act_gps_format=GPS_FORMAT_DECIMAL
;
public final static int MAX_WAYPOINTS=
100;
int[] LongWP
;
int[] LatWP
;
String[] NameWP
;
int last_wp=
0;
int Longitude
;
int Latitude
;
int TargetLongitude
;
int TargetLatitude
;
int Distance2Target
;
int Angle2Target
;
byte Used_Sat
;
/**
* Holds value Math.PI.
*/
public static final double PI =
Math.
PI;
/**
* Holds value PI / 2.0.
*/
public static final double PI_div2 = PI /
2.0;
/**
* Holds value PI / 4.0.
*/
public static final double PI_div4 = PI /
4.0;
public static final double RADIANS = PI /
180.0;
public static final double DEGREES =
180.0 / PI
;
private static final double p4 = 0.161536412982230228262e2
;
/**
* Holds value 0.26842548195503973794141e3.
*/
private static final double p3 = 0.26842548195503973794141e3
;
/**
* Holds value 0.11530293515404850115428136e4.
*/
private static final double p2 = 0.11530293515404850115428136e4
;
/**
* Holds value 0.178040631643319697105464587e4.
*/
private static final double p1 = 0.178040631643319697105464587e4
;
/**
* Holds value 0.89678597403663861959987488e3.
*/
private static final double p0 = 0.89678597403663861959987488e3
;
/**
* Holds value 0.5895697050844462222791e2.
*/
private static final double q4 = 0.5895697050844462222791e2
;
/**
* Holds value 0.536265374031215315104235e3.
*/
private static final double q3 = 0.536265374031215315104235e3
;
/**
* Holds value 0.16667838148816337184521798e4.
*/
private static final double q2 = 0.16667838148816337184521798e4
;
/**
* Holds value 0.207933497444540981287275926e4.
*/
private static final double q1 = 0.207933497444540981287275926e4
;
/**
* Holds value 0.89678597403663861962481162e3.
*/
private static final double q0 = 0.89678597403663861962481162e3
;
private static double _ATAN
(double X
) {
if (X
< 0.414213562373095048802) { /* tan(PI/8) */
return _ATANX
(X
);
}
else if (X
> 2.414213562373095048802) { /* tan(3*PI/8) */
return PI_div2 - _ATANX
(1.0 / X
);
}
else {
return PI_div4 + _ATANX
((X -
1.0) /
(X +
1.0));
}
}
private static double _ATANX
(double X
) {
double XX = X
* X
;
return X
* ((((p4
* XX + p3
) * XX + p2
) * XX + p1
) * XX + p0
)
/
(((((XX + q4
) * XX + q3
) * XX + q2
) * XX + q1
) * XX + q0
);
}
public double aTan2
(double Y,
double X
) {
//private static double ATAN2(double Y, double X) {
// return Math.atan2(Y,X); // not in CLDC 1.1
// X=0
if (X ==
0.0) {
if (Y
> 0.0) {
// mid Q1/Q2
return PI_div2
;
}
else if (Y
< 0.0) {
// mid Q3/Q4
return -PI_div2
;
}
else {
// undefined
return 0.0;
}
}
// X<0
if (X
< 0.0) {
if (Y
>=
0.0) {
// Q2
return (PI - _ATAN
(Y / -X
)); // Y>=0,X<0 |Y/X|
}
else {
// Q3
return -
(PI - _ATAN
(Y / X
)); // Y<0,X<0 |Y/X|
}
}
// X>0
if (X
> 0.0) {
// Q1/A4
//return ATAN( Y / X);
if (Y
> 0.0) {
// Q1
return _ATAN
(Y / X
);
}
else {
// Q4
return -_ATAN
(-Y / X
);
}
}
/* will never reach here */
return 0.0;
}
/*
double coeff_1 = Math.PI / 4d;
double coeff_2 = 3d * coeff_1;
double abs_y = Math.abs(y);
double angle;
if (x >= 0d) {
double r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
double r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
return y < 0d ? -angle : angle;
}
*/
public int distance2wp
(int id
)
{
double lat1=
(Latitude/
10000000.0)*RADIANS
;
double long1=
(Longitude/
10000000.0)*RADIANS
;
double lat2=
(LatWP
[id
]/
10000000.0)*RADIANS
;
double long2=
(LongWP
[id
]/
10000000.0)*RADIANS
;
double dLat=
(lat2-lat1
);
double dLon=
(long2-long1
);
double a =
Math.
sin(dLat/
2.0) * Math.
sin(dLat/
2.0) +
Math.
cos(lat1
) * Math.
cos(lat2
) *
Math.
sin(dLon/
2.0) * Math.
sin(dLon/
2.0);
return (int)(( 2.0 * aTan2
(Math.
sqrt(a
),
Math.
sqrt(1.0-a
)) )*6371008.8);
}
public int angle2wp
(int id
)
{
// TODO reuse from distance
double lat1=
(Latitude/
10000000.0)*RADIANS
;
double long1=
(Longitude/
10000000.0)*RADIANS
;
double lat2=
(LatWP
[id
]/
10000000.0)*RADIANS
;
double long2=
(LongWP
[id
]/
10000000.0)*RADIANS
;
double dLat=
(lat2-lat1
);
double dLon=
(long2-long1
);
double y =
Math.
sin(dLon
) * Math.
cos(lat2
);
double x =
Math.
cos(lat1
)*Math.
sin(lat2
) -
Math.
sin(lat1
)*Math.
cos(lat2
)*Math.
cos(dLon
);
return ((int)(aTan2
(y, x
)*DEGREES
)+
360)%360
;
}
public void push_wp
()
{
LongWP
[last_wp
]=Longitude
;
LatWP
[last_wp
]=Latitude
;
last_wp++
;
}
public void next_gps_format
()
{
act_gps_format=
(byte)((act_gps_format+
1)%GPS_FORMAT_COUNT
);
}
public String act_gps_format_str
(int val
)
{
switch(act_gps_format
)
{
case GPS_FORMAT_DECIMAL:
return "" + val/
10000000 +
"." +val
%10000000
;
case GPS_FORMAT_MINSEC:
return "" + val/
10000000 +
"^" +
((val
%10000000
)*60)/
10000000 +
"'" +
((((val
%10000000
)*60)%10000000
)*60)/
10000000 +
"." +
((((val
%10000000
)*60)%10000000
)*60)%10000000
;
default:
return "invalid format";
}
}
public String WP_Latitude_str
(int id
)
{
return "" + act_gps_format_str
(LatWP
[id
]); //+ "''N" ;
}
public String WP_Longitude_str
(int id
)
{
return "" + act_gps_format_str
(LongWP
[id
]); //+ "''E" ;
}
public String Latitude_str
()
{
return "" + act_gps_format_str
(Latitude
) ;
}
public String Longitude_str
()
{
return "" + act_gps_format_str
(Longitude
) ;
}
// Constructor
public MKGPSPosition
()
{
LongWP=
new int[MAX_WAYPOINTS
];
LatWP=
new int[MAX_WAYPOINTS
];
NameWP=
new String[MAX_WAYPOINTS
];
// predefined waypoints
LongWP
[0]=
123230170;
LatWP
[0]=
513600170 ;
NameWP
[0]=
"Sicherer PC1";
LongWP
[1]=
123269000;
LatWP
[1]=
513662670;
NameWP
[1]=
"Sicherer PC2";
LongWP
[2]=
123475570;
LatWP
[2]=
513569750 ;
NameWP
[2]=
"Treffpunkt Seba";
last_wp=
2;
}
private int parse_arr
(int offset,
int[] in_arr
)
{
return ((in_arr
[offset+
3]<<24) |
(in_arr
[offset+
2]<<16) |
(in_arr
[offset+
1]<<8) |
(in_arr
[offset+
0]));
}
public void set_by_mk_data
(int[] in_arr,MKVersion version
)
{
Longitude=parse_arr
(0,in_arr
);
Latitude=parse_arr
(4,in_arr
);
TargetLongitude=parse_arr
(8,in_arr
);
TargetLatitude=parse_arr
(12,in_arr
);
Distance2Target=parse_arr
(16,in_arr
);
Angle2Target=parse_arr
(20,in_arr
);
Used_Sat=
(byte)in_arr
[24];
}
}