More generic transform3DPoints
Example of 3D transformation using forward kinematic
Added more transform3DPoints() functions