It will be helpful, if you add comments discussing why certain functions are implemented in that way, and what algorithms (references) is used.
For example, there are some magic numbers without explanation:
if (cap < 1000) {
Have you looked at: https://github.com/davideberly/GeometricTools
Each algorithm is well documented and explained. There is even a short pdf papers for some of the algorithms explainimg why some decisions are taken and what numerical issues may be expected.
It will be helpful, if you add comments discussing why certain functions are implemented in that way, and what algorithms (references) is used. For example, there are some magic numbers without explanation: if (cap < 1000) {
Have you looked at: https://github.com/davideberly/GeometricTools Each algorithm is well documented and explained. There is even a short pdf papers for some of the algorithms explainimg why some decisions are taken and what numerical issues may be expected.