[SquarePeg3D] To be continued