Jag försöker skriva en funktion som kan omvandla koordinater i RT90 (användes tidigare av lantmäteriet) till WGS84/SWEREF99 (används av GPS).
Jag har börjat med att göra tvärtom, omvandla från WGS84/SWEREF99 till RT90, den formeln finns på http://sv.wikipedia.org/wiki/RT90
Jag har försökt att implementera detta i Java, men jag får det inte att funka.
Testvärden från Lantmäteriet:
RT90
x: 3239532.6315
y: 990625.1745
z: 5385197.8446
WGS84/SWEREF99
x: 3240036.3696
y: 990578.5272
z: 5385763.1648
Är det någon som kan se vad jag gör för fel?
Class för att representera en vektor med tre koordinatvärden:
public class Vector3 {
public double x;
public double y;
public double z;
public Vector3(double x, double y, double z){
this.x = x;
this.y = y;
this.z = z;
}
}
Kod för att representera en matris med tre vektorer:
public class Matrix {
public Vector3 v1;
public Vector3 v2;
public Vector3 v3;
public Matrix(Vector3 v1, Vector3 v2, Vector3 v3){
this.v1 = v1;
this.v2 = v2;
this.v3 = v3;
}
public Matrix(){
this(new Vector3(0,0,0), new Vector3(0,0,0), new Vector3(0,0,0));
}
}
Själva transformationen, main och en hjälpklass för att utföra vektor och matrisberäkningar:
public class Transform {
private static final double deltaX = -414.0978567149;
private static final double deltaY = -41.3381489658;
private static final double deltaZ = -603.0627177516;
private static final double omegaX = -0.8550434314;
private static final double omegaY = 2.1413465185;
private static final double omegaZ = -7.0227209516;
private static final double sinOmegaX = Math.sin(omegaX);
private static final double sinOmegaY = Math.sin(omegaY);
private static final double sinOmegaZ = Math.sin(omegaZ);
private static final double cosOmegaX = Math.cos(omegaX);
private static final double cosOmegaY = Math.cos(omegaY);
private static final double cosOmegaZ = Math.cos(omegaZ);
private static final Vector3 delta = new Vector3(deltaX, deltaY, deltaZ);
private static final Matrix rz =
new Matrix (new Vector3(cosOmegaZ,-sinOmegaZ,0),
new Vector3(sinOmegaZ, cosOmegaZ,0),
new Vector3(0, 0, 1));
private static final Matrix ry =
new Matrix (new Vector3(cosOmegaY, 0, sinOmegaY),
new Vector3(0, 1, 0),
new Vector3(-sinOmegaY, 0, cosOmegaY));
private static final Matrix rx =
new Matrix (new Vector3(1, 0, 0),
new Vector3(0, cosOmegaX, -sinOmegaX),
new Vector3(0, sinOmegaX, cosOmegaX));
private static final Matrix r = MatrixMath.matrixMatrixMult(rx,
MatrixMath.matrixMatrixMult(rz, ry));
public static Vector3 sweref99ToRt90(Vector3 sweref99){
return MatrixMath.vectorAdd(delta,
MatrixMath.matrixVectorMult(r, sweref99));
}
public static void main(String[] args) {
Vector3 sweref99 = new Vector3(3240036.3696, 990578.5272, 5385763.1648);
System.out.println("Sweref99 Lat: " + sweref99.x +
" Long: " + sweref99.y + " Alt: " + sweref99.z);
Vector3 rt90 = Transform.sweref99ToRt90(sweref99);
System.out.println("RT90 Lat: " + rt90.x +
" Long: " + rt90.y + " Alt: " + rt90.z);
}
private static class MatrixMath{
public static Vector3 scalarVectorMult(int scalar, Vector3 v){
Vector3 res = new Vector3(0,0,0);
res.x = v.x * scalar;
res.y = v.y * scalar;
res.z = v.z * scalar;
return res;
}
public static Vector3 matrixVectorMult(Matrix m, Vector3 v){
double newX = 0;
newX += m.v1.x * v.x;
newX += m.v1.y * v.y;
newX += m.v1.z * v.z;
double newY = 0;
newY += m.v2.x * v.x;
newY += m.v2.y * v.y;
newY += m.v2.z * v.z;
double newZ = 0;
newZ += m.v3.x * v.x;
newZ += m.v3.y * v.y;
newZ += m.v3.z * v.z;
return new Vector3(newX, newY, newZ);
}
public static Matrix matrixMatrixMult(Matrix m1, Matrix m2){
Matrix m = new Matrix();
m.v1.x = m1.v1.x * m2.v1.x + m1.v2.x * m2.v1.y + m1.v3.x * m2.v1.z;
m.v2.x = m1.v1.x * m2.v2.x + m1.v2.x * m2.v2.y + m1.v3.x * m2.v2.z;
m.v3.x = m1.v1.x * m2.v3.x + m1.v2.x * m2.v3.y + m1.v3.x * m2.v3.z;
m.v1.y = m1.v2.y * m2.v1.x + m1.v2.y * m2.v1.y + m1.v3.y * m2.v1.z;
m.v2.y = m1.v2.y * m2.v2.x + m1.v2.y * m2.v2.y + m1.v3.y * m2.v2.z;
m.v3.y = m1.v2.y * m2.v3.x + m1.v2.y * m2.v3.y + m1.v3.y * m2.v3.z;
m.v1.z = m1.v3.z * m2.v1.x + m1.v2.z * m2.v1.y + m1.v3.z * m2.v1.z;
m.v2.z = m1.v3.z * m2.v2.x + m1.v2.z * m2.v2.y + m1.v3.z * m2.v2.z;
m.v3.z = m1.v3.z * m2.v3.x + m1.v2.z * m2.v3.y + m1.v3.z * m2.v3.z;
return m;
}
public static Vector3 vectorAdd(Vector3 v1, Vector3 v2){
Vector3 res = new Vector3(0,0,0);
res.x = v1.x + v2.x;
res.y = v1.y + v2.y;
res.z = v1.z + v2.z;
return res;
}
}
}
Med den koden blir mina värden:
RT90
x: -4003529.688167752
y: -1520788.1914221882
z: -1.1397870008104721E7