In addition to what @Image Analyst has written, I would advise turning this into a function with p1, p2, p3 as input arguments and d, delta as output arguments.
Also, the C code appears to have bugs. These lines:
should from context probably be this instead:
The p2=p3 expression is an assignment in C that is then tested for non-zero, not an equality test, so this expression will be true whenever p3 is non-zero. You should double check your algorithm to confirm this is a bug. The MATLAB code for an equality test should use the == operator also.