Develop Reference

The function outputs the wrong value

When ever I input these coordinates I get a wrong output.
static double ReadCoordinateFromConsole(double lat1, double lon1, double
lat2, double lon2)
{
var R = 6371; // Radius of the earth in km
var dLat = deg2rad(lat2 - lat1);
var dLon = deg2rad(lon2 - lon1);
var a =
Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
var d = R * c; // Distance in km
return d;
}
static double deg2rad(double deg)
{
return deg * (Math.PI / 180);
}
Then in my function where I input some coordinates. 41.507483 -99.436554 38.504048 -98.315949. These coordinates should equal about 347 but instead, I get the output 7022,88 which is wrong and I have no clue as to why.
static double ReadDoubleFromConsole(string msg)
while (true)
{
Console.Write(msg);
string test = Console.ReadLine();
string[] words = test.Split(' ');
bool inputContainsNumber = Regex.IsMatch(words[0], #"^-*[0-9,\.]+$");
bool inputContainsNumber2 = Regex.IsMatch(words[1], #"^-*[0-9,\.]+$");
bool inputContainsNumber3 = Regex.IsMatch(words[2], #"^-*[0-9,\.]+$");
bool inputContainsNumber4 = Regex.IsMatch(words[3], #"^-*[0-9,\.]+$");
if(inputContainsNumber && inputContainsNumber2 && inputContainsNumber3
&& inputContainsNumber4)
{
double test1 = double.Parse(words[0]);
double test2 = double.Parse(words[1]);
double test3 = double.Parse(words[2]);
double test4 = double.Parse(words[3]);
double test5 = ReadCoordinateFromConsole(test1, test2, test3,
test4);
return test5;
}
Console.WriteLine("hmm, doesn't look correct - try again");
}
}

As you've been shown by the other answer your algorithm is correct - your parsing of the user input is wrong!
Here's the demonstration: http://rextester.com/IEEA93176
And the reason is, that in the culture that rextester runs in the , is used as a decimal separator - and I assume its the same in your environment. When you use double.parse("41.1234") in a culture expecting a , you get the value 411234 not 41.1234.
One fix is to force the culture
System.Threading.Thread.CurrentThread.CurrentCulture = new CultureInfo("en-GB");
Live demo(working): http://rextester.com/KALRN89806

There's nothing wrong with your implementation of the Haversine formula - it generates the correct output.
The following prints 347.328348039426:
using System;
namespace ConsoleApp1
{
class Program
{
static void Main()
{
Console.WriteLine(ReadCoordinateFromConsole(41.507483, -99.436554, 38.504048, -98.315949));
}
static double ReadCoordinateFromConsole(double lat1, double lon1, double
lat2, double lon2)
{
var R = 6371; // Radius of the earth in km
var dLat = deg2rad(lat2 - lat1);
var dLon = deg2rad(lon2 - lon1);
var a =
Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
var d = R * c; // Distance in km
return d;
}
static double deg2rad(double deg)
{
return deg * (Math.PI / 180);
}
}
}
Therefore you must look elsewhere for your error. I suspect you're not feeding the correct values to the method - I recommend that you single-step your code in the debugger to determine what's happening.
Incidentally, is there any reason that you're not using the .Net GeoCoordinate class to calculate this? For example:
using System;
using System.Device.Location;
namespace ConsoleApp1
{
class Program
{
static void Main()
{
var a = new GeoCoordinate(41.507483, -99.436554);
var b = new GeoCoordinate(38.504048, -98.315949);
Console.WriteLine(a.GetDistanceTo(b)/1000.0);
}
}
}
This prints 347.628192006498.

How to pass a hard coded differential equation through Runge-Kutta 4

I'm trying to implement Runge-Kutta for example problems
dy/dt = y - t^2 + 1 and dy/dt = t * y + t^3 in C# and I cannot seem to get the output I'm expecting. I have split my program into several classes to try and look at the work individually. I think that my main error is coming from trying to pass a method through the Runge-Kutta process as a variable using a delegate.
Equation Class:
namespace RK4
{
public class Eqn
{
double t;
double y;
double dt;
double b;
public Eqn(double t, double y, double dt, double b)
{
this.t = t;
this.y = y;
this.dt = dt;
this.b = b;
}
public void Run1()
{
double temp;
int step = 1;
RK4 n = new RK4();
while (t < b)
{
temp = n.Runge(t, y, dt, FN1);
y = temp;
Console.WriteLine("At step number {0}, t: {1}, y: {2}", step, t, y);
t = t + dt;
step++;
}
}
public void Run2()
{
int step = 1;
RK4 m = new RK4();
while (t < b)
{
y = m.Runge(t, y, dt, FN2);
Console.WriteLine("At step number {0}, t: {1}, y: {2}", step, t, y);
t = t + dt;
step++;
}
}
public static double FN1(double t, double y)
{
double x = y - Math.Pow(t, 2) + 1;
return x;
}
public static double FN2(double t, double y)
{
double x = t * y + Math.Pow(t, 3);
return x;
}
}
}
Then Runge-Kutta 4 Class:
namespace RK4
{
class RK4
{
public delegate double Calc(double t, double y);
public double Runge(double t, double y, double dt, Calc yp)
{
double k1 = dt * yp(t, y);
double k2 = dt * yp(t + 0.5 * dt, y + k1 * 0.5 * dt);
double k3 = dt * yp(t + 0.5 * dt, y + k2 * 0.5 * dt);
double k4 = dt * yp(t + dt, y + k3 * dt);
return (y + (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4));
}
}
}
And my Program Class:
namespace RK4
{
class Program
{
static void Main(string[] args)
{
RunProgram();
}
public static void RunProgram()
{
Console.WriteLine("*******************************************************************************");
Console.WriteLine("************************** Fourth Order Runge-Kutta ***************************");
Console.WriteLine("*******************************************************************************");
Console.WriteLine("\nWould you like to implement the fourth-order Runge-Kutta on:");
string Fn1 = "y' = y - t^2 + 1";
string Fn2 = "y' = t * y + t^3";
Console.WriteLine("1) {0}", Fn1);
Console.WriteLine("2) {0}", Fn2);
Console.WriteLine("Please enter 1 or 2");
switch (Int32.Parse(Console.ReadLine()))
{
case 1:
Console.WriteLine("\nPlease enter beginning of the interval (a):");
double a = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter end of the interval (b):");
double b = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter the step size (h) to be used:");
double h = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter the inital conditions to satisfy y({0}) = d",a);
Console.WriteLine("d = ");
double d = Double.Parse(Console.ReadLine());
Console.Clear();
Console.WriteLine("Using the interval [{0},{1}] and step size of {2} and the inital condition of y({3}) = {4}:", a, b, h, a, d);
Console.WriteLine("With equation: {0}", Fn1);
Eqn One = new Eqn(a, d, h, b);
One.Run1();
Console.WriteLine("Press enter to exit.");
Console.ReadLine();
Environment.Exit(1);
break;
case 2:
Console.WriteLine("\nPlease enter beginning of the interval (a):");
a = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter end of the interval (b):");
b = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter the step size (h) to be used:");
h = Double.Parse(Console.ReadLine());
Console.WriteLine("Please enter the inital conditions to satisfy y({0}) = d",a);
Console.WriteLine("d = ");
d = Double.Parse(Console.ReadLine());
Console.Clear();
Console.WriteLine("Using the interval [{0},{1}] and step size of {2} and the inital condition of y({3}) = {4}:", a, b, h, a, d);
Console.WriteLine("With equation: {0}", Fn1);
Eqn Two = new Eqn(a, d, h, b);
Two.Run2();
Console.WriteLine("Press enter to exit.");
Console.ReadLine();
Environment.Exit(1);
break;
default:
Console.WriteLine("Improper input, please press enter to exit.");
Console.ReadLine();
Environment.Exit(1);
break;
}
}
}
}
This is not elegant programming by any means but I don't have the working knowledge to know what I'm doing wrong at this point. From what I was reading I thought that the delegate within the RK4 class would be able to pass through my hard coded diff eq.

You are doing a classical error in the RK4 implementation: Having two variants to position the multiplication with dt to choose from, you are using both.
It is either
k2 = dt*f(t+0.5*dt, y+0.5*k1)
or
k2 = f(t+0.5*dt, y+0.5*dt*k1)
and analogously in the other lines of the algorithm.

Quadratic Equation Solver in C#

I'm trying to write a simple quadratic equation solver in C#, but for some reason it's not quite giving me the correct answers. In fact, it's giving me extremely large numbers as answers, usually well into the millions.
Could anyone please shed some light? I get the exact same answer for the positive root and the negative root as well. (Tried two different methods of math)
static void Main(string[] args)
{
int a;
int b;
int c;
Console.WriteLine("Hi, this is a Quadratic Equation Solver!");
Console.WriteLine("a-value: ");
try
{
a = int.Parse(Console.ReadLine());
Console.WriteLine("b-value: ");
b = int.Parse(Console.ReadLine());
Console.WriteLine("c-value: ");
c = int.Parse(Console.ReadLine());
Console.WriteLine("Okay, so your positive root is: " + quadForm(a, b, c, true));
Console.WriteLine("And your negative root is: " + quadForm(a, b, c, false));
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
Console.ReadLine();
}
static int quadForm(int a, int b, int c, Boolean pos)
{
int x = 0;
if (pos)
x = ((-b + (int) (Math.Sqrt((b * b) - (4 * a * c)))) / (2 * a));
else
x = ((-Math.Abs(b) - (int) (Math.Sqrt(Math.Pow(b,2) - (4 * a * c)))) / (2 * a));
return x;
}

Try this version of quadForm:
static double quadForm(int a, int b, int c, bool pos)
{
var preRoot = b * b - 4 * a * c;
if (preRoot < 0)
{
return double.NaN;
}
else
{
var sgn = pos ? 1.0 : -1.0;
return (sgn * Math.Sqrt(preRoot) - b) / (2.0 * a);
}
}

B-Spline recursive definition in C#

I'm trying to implement the recursive definition for B-Splines in c# but I can't get it right. Here's what I've done:
public static Double RecursiveBSpline(int i, Double[] t, int order, Double x)
{
Double result = 0;
if (order == 0)
{
if (t[i] <= x && x < t[i + 1])
{
result = 1;
}
else
{
result = 0;
}
}
else
{
Double denom1, denom2, num1, num2;
denom1 = t[i + order + 1] - t[i + 1];
denom2 = t[i + order] - t[i];
if (denom1 == 0)
{
num1 = 0;
}
else
{
num1 = t[i + order + 1] - x / denom1;
}
if (denom2 == 0)
{
num2 = 0;
}
else
{
num2 = x - t[i] / denom2;
}
result = num1 * RecursiveBSpline(i + 1, t, order - 1, x)
+ num2 * RecursiveBSpline(i, t, order - 1, x);
}
return result;
}
And here is how I call the function:
Double[] vect = new Double[] { 0, 1, 2, 3 };
MessageBox.Show(BSpline.RecursiveBSpline(0,vect,2,0.5).ToString());
I should see 0,125 on the screen, instead I get 0,25. The two denominator variables are used to check if they equal 0 and if they do, the number should be set to 0 by definition. Can someone point out where I'm getting this wrong?

Bear in mind, that the mathematical and logical operators in C# have a precedence order. Your second solution works fine if you put the right terms in braces (explanation follows). This line:
num2 = x - t[i] / denom2;
should be changed to:
num2 = (x - t[i]) / denom2;
and so on. Then the result is as desired: 0.125
The division operator has a higher order precedence as the addition operator. To affect the execution order use braces (everything in braces will be evaluated at first):
var r1 = 2 + 2 / 2; // Step1: 2 / 2 = 1 Step2: 2 + 1 Output: 3
var r2 = (2 + 2) / 2; // Step1: (2 + 2) = 4 Step2: 4 / 2 = 2 Output: 2

Conversion from NZMG to latitude and longitude

I want to convert NEW ZEALAND MAP GRID coordinates i.e. Northing and Easting into WGS84 coordinates i.e. Latitude and Longitude.
I have searched on internet but there is no proper explanation of how to do this, or an online calculator to do this.
My final goal is to write a program in C# or JAVA , which will convert NZMG coordinates into WGS84 coordinates.

Please note: This implementation does not convert with pinpoint accuracy.
Here is a c# implementation I put together for converting from NZMG to NZGD1949 and then to datum shift from NZGD1949 to NZGD2000. NZGD2000 coordinates are apparently compatible with WGS84 source: http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/geodetic-datum-conversions/wgs84-nzgd2000. I have added a 'useStaticCorrection' parameter to the datum shift method because I found my results were consistently off by approximately 40 meters in latitude - this varies depending on the input coordinates. This may be from a mistake in the logic but, as far as i can tell, it appears all correct (as by the sources referenced).
Sources:
Constants and coefficients
http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/projection-conversions/new-zealand-map-grid
Datum info.
http://www.linz.govt.nz/regulatory/25000
NZMG -> NZGD1949 formula
http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/projection-conversions/new-zealand-map-grid
NZ1949 -> NZGD2000 differences
http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/geodetic-datum-conversions/nzgd1949-nzgd2000/three-seven
Equivilent of a Three parameter shift.
http://sas2.elte.hu/tg/eesti_datums_egs9.htm
//Lat / Long fudge amounts http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/geodetic-datum-conversions/datum-transformation-examples
public class Pair<T, U>
{
public Pair()
{
}
public Pair(T first, U second)
{
this.First = first;
this.Second = second;
}
public T First { get; set; }
public U Second { get; set; }
};
using System.Numerics;
public class NZMGConverter
{
//Constants and coefficients
//Source: http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/projection-conversions/new-zealand-map-grid
private static Double N0 = 6023150;
private static Double E0 = 2510000;
private static Double NZGD1949a = 6378388;
private static Double NZMGLatOrigin = -41;
private static Double NZMGLongOrigin = 173;
//some of these are unused but are included for completeness. (unused arrays are for converting from wgs84 / nzgd2000 -> nzmg).
private static Double[] A = new Double[] { 0.6399175073, -0.1358797613, 0.0632944090, -0.0252685300, 0.0117879000, -0.0055161000, 0.0026906000, -0.0013330000, 0.0006700000, -0.0003400000 };
private static Complex[] B = new Complex[] { new Complex(0.7557853228, 0.0), new Complex(0.249204646, 0.003371507), new Complex(-0.001541739, 0.04105856), new Complex(-0.10162907, 0.01727609), new Complex(-0.26623489, -0.36249218), new Complex(-0.6870983, -1.1651967) };
private static Complex[] C = new Complex[] { new Complex(1.3231270439, 0.0), new Complex(-0.577245789, -0.007809598), new Complex(0.508307513, -0.112208952), new Complex(-0.15094762, 0.18200602), new Complex(1.01418179, 1.64497696), new Complex(1.9660549, 2.5127645) };
private static Double[] D = new Double[] { 1.5627014243, 0.5185406398, -0.0333309800, -0.1052906000, -0.0368594000, 0.0073170000, 0.0122000000, 0.0039400000, -0.0013000000 };
//Datum info.
//Source: http://www.linz.govt.nz/regulatory/25000
private static Double NZGD1949f = 0.003367003;
//Double NZGD1949InverseFlattening = 297;
private static Double NZGD1949e2 = (2 * 0.003367003) - Math.Pow(0.003367003, 2);
private static Double NZGD2000a = 6378137;
private static Double NZGD2000f = 0.003352811;
//Double NZGD2000InverseFlattening = 298.2572221;
private static Double NZGD2000e2 = (2 * 0.003352811) - Math.Pow(0.003352811, 2);
//NZ1949 -> NZGD2000 differences
//Source: http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/geodetic-datum-conversions/nzgd1949-nzgd2000/three-seven
private static Double differenceX = 54.4;
private static Double differenceY = -20.1;
private static Double differenceZ = 183.1;
//Source: http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/projection-conversions/new-zealand-map-grid
public static Pair<Double, Double> convertToNZGD1949(Double nzmgX, Double nzmgY)
{
Pair<Double, Double> result = null;
Complex z = new Complex((nzmgY - N0) / NZGD1949a, (nzmgX - E0) / NZGD1949a);
Complex theta0 = new Complex();
for (int i = 0; i < C.Length; ++i) { theta0 += Complex.Multiply(C[i], Complex.Pow(z, i + 1)); }
Double deltaLatWtheta0 = 0;
for (int i = 0; i < D.Length; ++i) { deltaLatWtheta0 += D[i] * Math.Pow(theta0.Real, (i + 1)); }
result = new Pair<double, double>();
result.First = NZMGLatOrigin + (Math.Pow(10, 5) / 3600) * deltaLatWtheta0; //NZGD1949 lat
result.Second = NZMGLongOrigin + 180 / Math.PI * theta0.Imaginary; //NZGD1949 long
return result;
}
//Equivilent of a Three parameter shift.
//Source: http://sas2.elte.hu/tg/eesti_datums_egs9.htm
public static Pair<Double, Double> datumShiftNZGD1949toNZGD2000(Double nzgd1949Lat, Double nzgd1949Long, Boolean useStaticCorrection)
{
Pair<Double, Double> result = null;
Double latInRaidans = nzgd1949Lat * (Math.PI / 180);
Double lngInRaidans = nzgd1949Long * (Math.PI / 180);
Double n = NZGD1949a / Math.Sqrt((1 - NZGD1949e2 * Math.Pow(Math.Sin(latInRaidans), 2)) * (3 / 2));
Double m = NZGD1949a * ((1 - NZGD1949e2) / (1 - NZGD1949e2 * Math.Pow(Math.Sin(latInRaidans), 2) * (3 / 2)));
Double difF = NZGD2000f - NZGD1949f;
Double difA = NZGD2000a - NZGD1949a;
Double changeInLat = (-1 * differenceX * Math.Sin(latInRaidans) * Math.Cos(lngInRaidans)
- differenceY * Math.Sin(latInRaidans) * Math.Sin(lngInRaidans)
+ differenceZ * Math.Cos(latInRaidans) + (NZGD1949a * difF + NZGD1949f * difA) * Math.Sin(2 * latInRaidans))
/ (m * Math.Sin(1));
Double changeInLong = (-1 * differenceX * Math.Sin(lngInRaidans) + differenceY * Math.Cos(lngInRaidans)) / (n * Math.Cos(latInRaidans) * Math.Sin(1));
result = new Pair<Double, Double>();
result.First = (latInRaidans + changeInLat) * (180 / Math.PI);
result.Second = (lngInRaidans + changeInLong) * (180 / Math.PI);
if (useStaticCorrection)
{
//Difference found by 3-parameter test values # http://www.linz.govt.nz/data/geodetic-system/coordinate-conversion/geodetic-datum-conversions/datum-transformation-examples
Double staticLatCorrection = 0.00037432;
Double staticLongCorrection = -0.00003213;
result.First -= staticLatCorrection;
result.Second += staticLongCorrection;
}
return result;
}
}

NZMG to Latitude/Longitude using Swift. Thanks to DavidG's post above. Fixed 40m latitude error.
import Foundation
import Darwin
class NZMGConverter {
private let easting: Double!
private let northing: Double!
private var z: Complex! = nil
private let N0: Double = 6023150; //false Northing
private let E0: Double = 2510000; //false Easting
private let NZMGLatOrigin: Double = -41;
private let NZMGLongOrigin: Double = 173;
private let c: [Complex] = [Complex(real: 1.3231270439, imag: 0.0), Complex(real: -0.577245789, imag: -0.007809598), Complex(real: 0.508307513, imag: -0.112208952), Complex(real: -0.15094762, imag: 0.18200602), Complex(real: 1.01418179, imag: 1.64497696), Complex(real: 1.9660549, imag: 2.5127645)]
private let b: [Complex] = [Complex(real: 0.7557853228, imag: 0.0), Complex(real: 0.249204646, imag: 0.003371507), Complex(real: -0.001541739, imag: 0.04105856), Complex(real: -0.10162907, imag: 0.01727609), Complex(real: -0.26623489, imag: -0.36249218), Complex(real: -0.6870983, imag: -1.1651967)]
private let d: [Double] = [1.5627014243, 0.5185406398, -0.0333309800, -0.1052906000, -0.0368594000, 0.0073170000, 0.0122000000, 0.0039400000, -0.0013000000]
//Datum info.
private let NZGD1949f: Double = 0.003367003;
//Double NZGD1949InverseFlattening = 297;
private let NZGD1949e2: Double = (2 * 0.003367003) - pow(0.003367003, 2);
private let NZGD1949a: Double = 6378388; //Semi-major axis
private let NZGD2000a: Double = 6378137;
private let NZGD2000f: Double = 0.003352811;
//Double NZGD2000InverseFlattening = 298.2572221;
private let NZGD2000e2: Double = (2 * 0.003352811) - pow(0.003352811, 2);
//NZ1949 -> NZGD2000 differences
private let differenceX: Double = 54.4;
private let differenceY: Double = -20.1;
private let differenceZ: Double = 183.1;
init(easting: Double, northing: Double) {
self.easting = easting
self.northing = northing
}
func nzmgToNZGD1949() -> (latitude: Double, longitude: Double) {
z = Complex(real: (northing - N0) / NZGD1949a, imag: (easting - E0) / NZGD1949a)
let theta0 = thetaZero(z)
let theta: Complex = thetaSuccessive(theta0, i: 0)
let latlong = calcLatLong(theta)
print(latlong)
return datumShift(latlong.0, longitude: latlong.1)
}
func thetaZero(z: Complex) -> Complex {
var theta0 = Complex()
for var i = 0; i < c.count; i++ {
theta0 = theta0 + (c[i] * (z ^ (i + 1)))
//print(theta0)
}
return theta0
}
func thetaSuccessive(theta0: Complex, i: Int) -> Complex {
var numerator: Complex = Complex()
var denominator: Complex = Complex()
for var n = 1; n < b.count; n++ {
numerator = numerator + (Complex(real: n) * b[n] * (theta0 ^ (n + 1)))
}
numerator = z + numerator
for var n = 0; n < b.count; n++ {
denominator = denominator + (Complex(real: n + 1) * b[n] * (theta0 ^ n))
}
let theta = numerator / denominator
if i == 2 {
return theta
} else {
return thetaSuccessive(theta, i: i + 1)
}
}
func calcLatLong(theta: Complex) -> (latitude: Double, longitude: Double) {
let deltaPsi = theta.real
let deltaLambda = theta.imag
var deltaPhi: Double = 0
for var n = 0; n < d.count; n++ {
deltaPhi += d[n] * pow(deltaPsi, Double(n + 1))
}
let lat: Double = NZMGLatOrigin + ((pow(10, 5) / 3600) * deltaPhi)
let long: Double = NZMGLongOrigin + ((180 / M_PI) * deltaLambda)
return (latitude: lat, longitude: long)
}
func datumShift(latitude: Double, longitude: Double) -> (latitude: Double, longitude: Double) {
let latInRadians = latitude * (M_PI / 180)
let lngInRadians = longitude * (M_PI / 180)
let v = NZGD1949a / (sqrt(1 - (NZGD1949e2 * pow(sin(latInRadians), 2))))
var x_cart = v * cos(latInRadians) * cos(lngInRadians)
var y_cart = v * cos(latInRadians) * sin(lngInRadians)
var z_cart = (v * (1 - NZGD1949e2)) * sin(latInRadians)
x_cart += differenceX
y_cart += differenceY
z_cart += differenceZ
print(x_cart)
print(y_cart)
print(z_cart)
let p = sqrt(pow(x_cart, 2) + pow(y_cart, 2))
let r = sqrt(pow(p, 2) + pow(z_cart, 2))
let mu = atan((z_cart / p) * ((1 - NZGD2000f) + ((NZGD2000e2 * NZGD2000a) / r)))
let num = (z_cart * (1 - NZGD2000f)) + (NZGD2000e2 * NZGD2000a * pow(sin(mu), 3))
let denom = (1 - NZGD2000f) * (p - (NZGD2000e2 * NZGD2000a * pow(cos(mu), 3)))
var lat = atan(num / denom)
var long = atan(y_cart / x_cart)
print(long)
lat = (180 / M_PI) * lat
long = 180 + (180 / M_PI) * long
return (latitude: lat, longitude: long)
}
}
}

Proj4j provides this functionality:
It is one of the few universal coordinate conversions libs.
http://trac.osgeo.org/proj4j/
Then you have to find the proj4 string that defines the new zeal and datum description:
Depending if you need th eold or the new grid, you need the new zealand datum shift grids,+
which are included in proj4.
As a first step find exactly the name of the datum that you need:
NZGD49 or NZGD2000 ?