Thanks for your reply, @batmm.
After you have either refined your original solution or developed a new one, please post it for us so we can try it out on various cases.
The p5.js function posted above is similar to your original Java version, but it works properly.