OpenCNCPilot.GCode.HeightMap.InterpolateZ C# (CSharp) Method

		public double InterpolateZ(double x, double y)
		{
			if (x > Max.X || x < Min.X || y > Max.Y || y < Min.Y)
				return MaxHeight;

			x -= Min.X;
			y -= Min.Y;

			x /= GridX;
			y /= GridY;

			int iLX = (int)Math.Floor(x);   //lower integer part
			int iLY = (int)Math.Floor(y);

			int iHX = (int)Math.Ceiling(x); //upper integer part
			int iHY = (int)Math.Ceiling(y);

			double fX = x - iLX;             //fractional part
			double fY = y - iLY;

			double linUpper = Points[iHX, iHY].Value * fX + Points[iLX, iHY].Value * (1 - fX);       //linear immediates
			double linLower = Points[iHX, iLY].Value * fX + Points[iLX, iLY].Value * (1 - fX);

			return linUpper * fY + linLower * (1 - fY);     //bilinear result
		}

        public GCodeFile ApplyHeightMap(HeightMap map)
        {
            double segmentLength = Math.Min(map.GridX, map.GridY);

            List <Command> newToolPath = new List <Command>();

            foreach (Command command in Toolpath)
            {
                if (command is Motion)
                {
                    Motion m = (Motion)command;

                    foreach (Motion subMotion in m.Split(segmentLength))
                    {
                        subMotion.Start.Z += map.InterpolateZ(subMotion.Start.X, subMotion.Start.Y);
                        subMotion.End.Z   += map.InterpolateZ(subMotion.End.X, subMotion.End.Y);

                        newToolPath.Add(subMotion);
                    }
                }
                else
                {
                    newToolPath.Add(command);
                    continue;
                }
            }

            return(new GCodeFile(newToolPath));
        }