Consider providing shortcut to get BodyNode from Contact

[jslee02]

In DART 6, it is little verbose in order to get BodyNode from Contact. Here is an example:

BodyNode* bodyNode = nullptr;
auto* shapeFrame = contact.collisionObject1->getShapeFrame();
if (!shapeFrame->isShapeNode())
  return nullptr;
bodyNode = shapeFrame->asShapeNode()->getBodyNodePtr();

It would be nice if we have a shortcut for this in Contact or shapeFrame as:

dynamics::ConstBodyNodePtr Contact::getBodyNodePtr1() const
{
  if (!collisionObject1)
    return nullptr;

  auto* shapeFrame = collisionObject1->getShapeFrame();

  if (!shapeFrame)
    return nullptr;

  if (!shapeFrame->isShapeNode())
    return nullptr;

  return shapeFrame->asShapeNode()->getBodyNodePtr();
}