Везде, где есть вычисления вида A=SQR(B*B-C*C) (там где вычисляется Z, С*С=X*X+Y*Y), формулу надо поправить на: A=2(B-C)-SQR(B*B-C*C)

Правда подозреваю, что надо не везде, а только во втором вычислении.