adding 3d point 
wip A matrix