A homogeneous matrix approach to 3D kinematics and dynamics — I. Theory