[etanders]

Har haft mycket annat att göra, så det tog några dagar. Hoppas du i alla fall kan ha någon nytta av detta.

Jag har inga testdata så jag har inte kontrollerat att konverteringen stämmer, men jag får i alla fall tillbaka samma värden om jag konverterar godtyckliga värden RT90 -> Sweref99 -> RT90. Jag har bara utgått från formeln på wikipedia, och de siffervärden som finns där har jag satt som konstanter. Behöver de ändras i programmet får du ju göra om det på något sätt, kanske genom att skicka in aktuella värden till konstruktorn eller nåt.

Så här använder du min lilla konverterare:

double x_RT90= 123;
double y_RT90 = 456;
double z_RT90 = 789;
double x_Sweref99;
double y_Sweref99;
double z_Sweref99;

Converter c = new Converter();
c.FromRT90ToSweref99(x_RT90, y_RT90, z_RT90, out x_Sweref99, out y_Sweref99, out z_Sweref99);

Och så själva koden:

using System;

public class Converter
{
	public const double deltaX = -414.0978567149;
	public const double deltaY = -41.3381489658;
	public const double deltaZ = -603.0627177516;

	public const double omegaX = -0.8550434314;
	public const double omegaY = 2.1413465185;
	public const double omegaZ = -7.0227209516;

	public const double smallDelta = 0d;

	public Vector Delta
	{
 get
 {
 	Vector delta = new Vector();

 	delta.SetElement(0, deltaX);
 	delta.SetElement(1, deltaY);
 	delta.SetElement(2, deltaZ);

 	return delta;
 }
	}

	public Matrix R
	{
 get
 {
 	double sx = Math.Sin(omegaX);
 	double cx = Math.Cos(omegaX);
 	double sy = Math.Sin(omegaY);
 	double cy = Math.Cos(omegaY);
 	double sz = Math.Sin(omegaZ);
 	double cz = Math.Cos(omegaZ);

 	Matrix r = new Matrix();

 	r.SetElement(0, 0, cy * cz);
 	r.SetElement(0, 1, cz * sx * sy + cx * sz);
 	r.SetElement(0, 2, -cx * cz * sy + sx * sz);

 	r.SetElement(1, 0, -cy * sz);
 	r.SetElement(1, 1, cx * cz - sx * sy * sz);
 	r.SetElement(1, 2, cz * sx + cx * sy * sz);

 	r.SetElement(2, 0, sy);
 	r.SetElement(2, 1, -cy * sx);
 	r.SetElement(2, 2, cx * cy);

 	return r;
 }
	}

	public Matrix InverseR
	{
 get
 {
 	double sx = Math.Sin(omegaX);
 	double cx = Math.Cos(omegaX);
 	double sy = Math.Sin(omegaY);
 	double cy = Math.Cos(omegaY);
 	double sz = Math.Sin(omegaZ);
 	double cz = Math.Cos(omegaZ);

 	Matrix r = new Matrix();

 	r.SetElement(0, 0, cy * cz);
 	r.SetElement(0, 1, -cy * sz);
 	r.SetElement(0, 2, sy);

 	r.SetElement(1, 0, cz * sx * sy + cx * sz);
 	r.SetElement(1, 1, cx * cz - sx * sy * sz);
 	r.SetElement(1, 2, -cy * sx);

 	r.SetElement(2, 0, -cx * cz * sy + sx * sz);
 	r.SetElement(2, 1, cz * sx + cx * sy * sz);
 	r.SetElement(2, 2, cx * cy);

 	return r;
 }
	}

	public void FromSweref99ToRT90(double x_Sweref99, double y_Sweref99, double z_Sweref99,
 out double x_RT90, out double y_RT90, out double z_RT90)
	{
 Vector Sweref99 = new Vector();
 Sweref99.SetElement(0, x_Sweref99);
 Sweref99.SetElement(1, y_Sweref99);
 Sweref99.SetElement(2, z_Sweref99);

 Vector RT90 = MatrixMath.VectorAddition(
 	Delta,
 	MatrixMath.ScalarVectorMultiplication(
  1 + smallDelta,
  MatrixMath.MatrixVectorMultiplication(
  	R,
  	Sweref99)));

 x_RT90 = RT90.GetElement(0);
 y_RT90 = RT90.GetElement(1);
 z_RT90 = RT90.GetElement(2);
	}

	public void FromRT90ToSweref99(double x_RT90, double y_RT90, double z_RT90,
 out double x_Sweref99, out double y_Sweref99, out double z_Sweref99)
	{
 Vector RT90 = new Vector();
 RT90.SetElement(0, x_RT90);
 RT90.SetElement(1, y_RT90);
 RT90.SetElement(2, z_RT90);

 Vector Sweref99 = MatrixMath.ScalarVectorMultiplication(
 	1 / (1 + smallDelta),
 	MatrixMath.MatrixVectorMultiplication(
  InverseR,
  MatrixMath.VectorAddition(
  	RT90,
  	MatrixMath.ScalarVectorMultiplication(
   -1,
   Delta))));

 x_Sweref99 = Sweref99.GetElement(0);
 y_Sweref99 = Sweref99.GetElement(1);
 z_Sweref99 = Sweref99.GetElement(2);
	}
}

public class Matrix
{
	private static readonly int Dim = 3;
	private readonly double[,] m_elements;

	public Matrix()
	{
 m_elements = new double[Dim, Dim];
	}

	public static int Dimension
	{
 get { return Dim; }
	}

	public void SetElement(int i, int j, double value)
	{
 m_elements[i, j] = value;
	}

	public double GetElement(int i, int j)
	{
 return m_elements[i, j];
	}
}

public class Vector
{
	private static readonly int Dim = 3;
	private readonly double[] m_elements;

	public Vector()
	{
 m_elements = new double[Dim];
	}

	public static int Dimension
	{
 get { return Dim; }
	}

	public void SetElement(int i, double value)
	{
 m_elements[i] = value;
	}

	public double GetElement(int i)
	{
 return m_elements[i];
	}
}

public class MatrixMath
{
	public static Matrix MatrixMultiplication(Matrix m1, Matrix m2)
	{
 Matrix m = new Matrix();

 for (int i = 0; i < Matrix.Dimension; i++)
 {
 	for (int j = 0; j < Matrix.Dimension; j++)
 	{
  double value = 0.0;
  for (int k = 0; k < Matrix.Dimension; k++)
  {
  	value += m1.GetElement(i, k) * m2.GetElement(k, j);
  }
  m.SetElement(i, j, value);
 	}
 }

 return m;
	}

	public static Vector MatrixVectorMultiplication(Matrix m, Vector v)
	{
 Vector v2 = new Vector();

 for (int i = 0; i < Vector.Dimension; i++)
 {
 	double value = 0.0;
 	for (int j = 0; j < Vector.Dimension; j++)
 	{
  value += m.GetElement(i, j) * v.GetElement(j);
 	}
 	v2.SetElement(i, value);
 }

 return v2;
	}

	public static Vector ScalarVectorMultiplication(double scalar, Vector v)
	{
 Vector v2 = new Vector();

 for (int i = 0; i < Vector.Dimension; i++)
 {
 	v2.SetElement(i, scalar * v.GetElement(i));
 }

 return v2;
	}

	public static Vector VectorAddition(Vector v1, Vector v2)
	{
 Vector v = new Vector();

 for (int i = 0; i < Vector.Dimension; i++)
 {
 	v.SetElement(i, v1.GetElement(i) + v2.GetElement(i));
 }

 return v;
	}
}