Implemente screw matrix implemente

part of the Screw matrix project code using C++ to implemented.