have a look at 3D polynomial interpolation in google, it'll give you some maths to convert into code