3D rigid bodies

For the case of a 3D rigid body to which any transformation in $SE(3)$ may be applied, the same general principles apply. The quaternion parameterization once again becomes the right way to represent $SO(3)$ because using (4.20) avoids all trigonometric functions in the same way that (4.18) avoided them for $SO(2)$. Unfortunately, (4.20) is not linear in the configuration variables, as it was for (4.18), but it is at least polynomial. This enables semi-algebraic models to be formed for ${\cal C}_{obs}$. Type FV, VF, and EE contacts arise for the $SE(3)$ case. From all of the contact conditions, polynomials that correspond to each patch of ${\cal C}_{obs}$ can be made. These patches are polynomials in seven variables: $x_t$, $y_t$, $z_t$, $a$, $b$, $c$, and $d$. Once again, a special primitive must be intersected with all others; here, it enforces the constraint that unit quaternions are used. This reduces the dimension from $7$ back down to $6$. Also, constraints should be added to throw away half of ${\mathbb{S}}^3$, which is redundant because of the identification of antipodal points on ${\mathbb{S}}^3$.