int x1 = (int) Math.floor(pointInMatrix.x);
int x2 = (int) Math.ceil(pointInMatrix.x);
int y1 = (int) Math.floor(pointInMatrix.y);
int y2 = (int) Math.ceil(pointInMatrix.y);
if (pointInMatrix.distance(new Vector2D(x1, y1)) < pointInMatrix.distance(new Vector2D(x2, y2))) {
planePoint1 = new Vector3D(x1, y1, this.heightProfile.get(x1, y1));
} else {
planePoint1 = new Vector3D(x2, y2, this.heightProfile.get(x2, y2));
}
planePoint2 = new Vector3D(x2, y1, this.heightProfile.get(x2, y1));