In Progress Added in C++ code

This contains files code for motion Solver that is in progress, not
completed yet. Didn't wan to break any of the current working code.