Simplify Body and Joint private implementation

  - access directly to private types when possible, avoiding costly dynamic_cast
    and confusion.