--[BE] - Baselink ID init via constant (#2465)

* --system-wide constant for baseLink ID
* --use BASELINK_ID instead of magic number -1 to denote baseLink
* --Use ID_UNDEFINED for rigid object link ids.
* --more fixes

src/esp/core/Esp.h

@@ -98,6 +98,9 @@ constexpr int ID_UNDEFINED = -1;
 /** @brief Object ID of the rigid stage.*/
 constexpr int RIGID_STAGE_ID = 0;
 
+/** @brief Link ID of the baseLink for articulated objects */
+constexpr int BASELINK_ID = -1;
+
 static const double NO_TIME = 0.0;
 
 /**

src/esp/physics/ArticulatedObject.h

@@ -327,11 +327,11 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   /**
    * @brief Get a const reference to an ArticulatedLink SceneNode for
    * info query purposes.
-   * @param linkId The ArticulatedLink ID or -1 for the baseLink.
+   * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_.
    * @return Const reference to the SceneNode.
    */
-  const scene::SceneNode& getLinkSceneNode(int linkId = -1) const {
-    if (linkId == ID_UNDEFINED) {
+  const scene::SceneNode& getLinkSceneNode(int linkId = BASELINK_ID) const {
+    if (linkId == BASELINK_ID) {
       // base link
       return baseLink_->node();
     }
@@ -345,12 +345,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
 
   /**
    * @brief Get pointers to a link's visual SceneNodes.
-   * @param linkId The ArticulatedLink ID or -1 for the baseLink.
+   * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_.
    * @return vector of pointers to the link's visual scene nodes.
    */
   std::vector<scene::SceneNode*> getLinkVisualSceneNodes(
-      int linkId = -1) const {
-    if (linkId == ID_UNDEFINED) {
+      int linkId = BASELINK_ID) const {
+    if (linkId == BASELINK_ID) {
       // base link
       return baseLink_->visualNodes_;
     }
@@ -404,12 +404,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   /**
    * @brief Get a link by index.
    *
-   * @param id The id of the desired link. -1 for base link.
+   * @param id The id of the desired link. @ref BASELINK_ID for the @ref baseLink_.
    * @return The desired link.
    */
   ArticulatedLink& getLink(int id) {
-    // option to get the baseLink_ with id=-1
-    if (id == -1) {
+    // option to get the baseLink_ with id=BASELINK_ID
+    if (id == BASELINK_ID) {
       return *baseLink_.get();
     }
 
@@ -420,14 +420,16 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   }
 
   /**
-   * @brief Get the number of links for this object (not including the base).
+   * @brief Get the number of links for this object (not including the @ref baseLink_
+   * == @ref BASELINK_ID.).
    *
    * @return The number of non-base links.
    */
   int getNumLinks() const { return links_.size(); }
 
   /**
-   * @brief Get a list of link ids, not including the base (-1).
+   * @brief Get a list of link ids, not including the @ref baseLink_
+   * == @ref BASELINK_ID.
    *
    * @return A list of link ids for this object.
    */
@@ -441,14 +443,15 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   }
 
   /**
-   * @brief Get a list of link ids including the base (-1).
+   * @brief Get a list of link ids including the @ref baseLink_
+   * == @ref BASELINK_ID.
    *
    * @return A list of link ids for this object.
    */
   std::vector<int> getLinkIdsWithBase() const {
     std::vector<int> ids;
     ids.reserve(links_.size() + 1);
-    ids.push_back(-1);
+    ids.push_back(BASELINK_ID);
     for (auto it = links_.begin(); it != links_.end(); ++it) {
       ids.push_back(it->first);
     }
@@ -488,14 +491,14 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
    * @brief Given the list of passed points in this object's local space, return
    * those points transformed to world space.
    * @param points vector of points in object local space
-   * @param linkId Internal link index.
+   * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_.
    * @return vector of points transformed into world space
    */
   std::vector<Mn::Vector3> transformLocalPointsToWorld(
       const std::vector<Mn::Vector3>& points,
-      int linkId = -1) const override {
-    if (linkId == -1) {
-      return this->baseLink_->transformLocalPointsToWorld(points, -1);
+      int linkId = BASELINK_ID) const override {
+    if (linkId == BASELINK_ID) {
+      return this->baseLink_->transformLocalPointsToWorld(points, BASELINK_ID);
     }
     auto linkIter = links_.find(linkId);
     ESP_CHECK(linkIter != links_.end(),
@@ -509,14 +512,14 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
    * @brief Given the list of passed points in world space, return
    * those points transformed to this object's local space.
    * @param points vector of points in world space
-   * @param linkId Internal link index.
+   * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_.
    * @return vector of points transformed to be in local space
    */
   std::vector<Mn::Vector3> transformWorldPointsToLocal(
       const std::vector<Mn::Vector3>& points,
-      int linkId = -1) const override {
-    if (linkId == -1) {
-      return this->baseLink_->transformWorldPointsToLocal(points, -1);
+      int linkId = BASELINK_ID) const override {
+    if (linkId == BASELINK_ID) {
+      return this->baseLink_->transformWorldPointsToLocal(points, BASELINK_ID);
     }
     auto linkIter = links_.find(linkId);
     ESP_CHECK(linkIter != links_.end(),
@@ -557,7 +560,7 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
         int linkId = getLinkIdFromName(linkName);
         // locally access the unique pointer's payload
         const esp::physics::ArticulatedLink* aoLink = nullptr;
-        if (linkId == -1) {
+        if (linkId == BASELINK_ID) {
           aoLink = baseLink_.get();
         } else {
           auto linkIter = links_.find(linkId);
@@ -760,11 +763,11 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   /**
    * @brief Get the name of the link.
    *
-   * @param linkId The link's index. -1 for base link.
+   * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_.
    * @return The link's name.
    */
   virtual std::string getLinkName(int linkId) const {
-    if (linkId == -1) {
+    if (linkId == BASELINK_ID) {
       return baseLink_->linkName;
     }
 
@@ -779,15 +782,17 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
    * @brief Get the starting position for this link's parent joint in the global
    * DoFs array.
    *
-   * @param linkId The link's index.
+   * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_.
    * @return The link's starting DoF index.
    */
-  virtual int getLinkDoFOffset(CORRADE_UNUSED int linkId) const { return -1; }
+  virtual int getLinkDoFOffset(CORRADE_UNUSED int linkId) const {
+    return ID_UNDEFINED;
+  }
 
   /**
    * @brief Get the number of DoFs for this link's parent joint.
    *
-   * @param linkId The link's index.
+   * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_.
    * @return The number of DoFs for this link's parent joint.
    */
   virtual int getLinkNumDoFs(CORRADE_UNUSED int linkId) const { return 0; }
@@ -796,17 +801,17 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
    * @brief Get the starting position for this link's parent joint in the global
    * positions array.
    *
-   * @param linkId The link's index.
+   * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_.
    * @return The link's starting position index.
    */
   virtual int getLinkJointPosOffset(CORRADE_UNUSED int linkId) const {
-    return -1;
+    return ID_UNDEFINED;
   }
 
   /**
    * @brief Get the number of positions for this link's parent joint.
    *
-   * @param linkId The link's index.
+   * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_.
    * @return The number of positions for this link's parent joint.
    */
   virtual int getLinkNumJointPos(CORRADE_UNUSED int linkId) const { return 0; }

src/esp/physics/PhysicsManager.h

@@ -92,10 +92,13 @@ struct RaycastResults {
  * @brief based on Bullet b3ContactPointData
  */
 struct ContactPointData {
-  int objectIdA = -2;  // stage is -1
-  int objectIdB = -2;
-  int linkIndexA = -1;  // -1 if not a multibody
-  int linkIndexB = -1;
+  // Initialize to safe, appropriate values
+  // stage will be lowest object ID in system
+  int objectIdA = RIGID_STAGE_ID - 1;
+  int objectIdB = RIGID_STAGE_ID - 1;
+  // assume not a multibody
+  int linkIndexA = ID_UNDEFINED;
+  int linkIndexB = ID_UNDEFINED;
 
   Magnum::Vector3 positionOnAInWS;  // contact point location on object A, in
                                     // world space coordinates
@@ -160,12 +163,11 @@ struct RigidConstraintSettings {
   /** @brief objectIdB == ID_UNDEFINED indicates "world". */
   int objectIdB = ID_UNDEFINED;
 
-  /** @brief  link of objectA if articulated. ID_UNDEFINED(-1) refers to base.
-   */
-  int linkIdA = ID_UNDEFINED;
+  /** @brief  link of objectA if articulated. @ref BASELINK_ID refers to base.*/
+  int linkIdA = BASELINK_ID;
 
-  /** @brief link of objectB if articulated. ID_UNDEFINED(-1) refers to base.*/
-  int linkIdB = ID_UNDEFINED;
+  /** @brief link of objectB if articulated.  @ref BASELINK_ID refers to base.*/
+  int linkIdB = BASELINK_ID;
 
   /** @brief constraint point in local space of respective objects*/
   Mn::Vector3 pivotA{}, pivotB{};

src/esp/physics/PhysicsObjectBase.h

@@ -229,7 +229,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
    */
   virtual std::vector<Mn::Vector3> transformLocalPointsToWorld(
       const std::vector<Mn::Vector3>& points,
-      CORRADE_UNUSED int linkID = -1) const {
+      CORRADE_UNUSED int linkID = ID_UNDEFINED) const {
     std::vector<Mn::Vector3> wsPoints;
     wsPoints.reserve(points.size());
     Mn::Vector3 objScale = getScale();
@@ -249,7 +249,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
    */
   virtual std::vector<Mn::Vector3> transformWorldPointsToLocal(
       const std::vector<Mn::Vector3>& points,
-      CORRADE_UNUSED int linkID = -1) const {
+      CORRADE_UNUSED int linkID = ID_UNDEFINED) const {
     std::vector<Mn::Vector3> lsPoints;
     lsPoints.reserve(points.size());
     Mn::Vector3 objScale = getScale();
@@ -584,7 +584,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
         for (const auto& markersEntry : linkEntry.second) {
           const std::string markersName = markersEntry.first;
           perLinkMap[markersName] =
-              transformLocalPointsToWorld(markersEntry.second, -1);
+              transformLocalPointsToWorld(markersEntry.second, ID_UNDEFINED);
         }
         perTaskMap[linkName] = perLinkMap;
       }

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -162,7 +162,7 @@ int BulletPhysicsManager::addArticulatedObjectInternal(
   }
 
   // allocate ids for links
-  ArticulatedLink& rootObject = articulatedObject->getLink(-1);
+  ArticulatedLink& rootObject = articulatedObject->getLink(BASELINK_ID);
   rootObject.node().setBaseObjectId(articulatedObject->getObjectID());
   for (int linkIx = 0; linkIx < articulatedObject->btMultiBody_->getNumLinks();
        ++linkIx) {
@@ -536,7 +536,7 @@ void BulletPhysicsManager::lookUpObjectIdAndLinkId(
   CORRADE_INTERNAL_ASSERT(objectId);
   CORRADE_INTERNAL_ASSERT(linkId);
 
-  *linkId = -1;
+  *linkId = ID_UNDEFINED;
   // If the lookup fails, default to the stage. TODO: better error-handling.
   *objectId = RIGID_STAGE_ID;
   auto rawColObjIdIter = collisionObjToObjIds_->find(colObj);
@@ -572,10 +572,10 @@ std::vector<ContactPointData> BulletPhysicsManager::getContactPoints() const {
     const btPersistentManifold* manifold =
         dispatcher->getInternalManifoldPointer()[i];
 
-    int objectIdA = ID_UNDEFINED;
-    int objectIdB = ID_UNDEFINED;
-    int linkIndexA = -1;  // -1 if not a multibody
-    int linkIndexB = -1;
+    int objectIdA = RIGID_STAGE_ID - 1;
+    int objectIdB = RIGID_STAGE_ID - 1;
+    int linkIndexA = ID_UNDEFINED;  // -1 if not a multibody
+    int linkIndexB = ID_UNDEFINED;
 
     const btCollisionObject* colObj0 = manifold->getBody0();
     const btCollisionObject* colObj1 = manifold->getBody1();

src/esp/physics/bullet/BulletURDFImporter.cpp

@@ -194,8 +194,9 @@ void BulletURDFImporter::getAllIndices(
   int mbIndex = cache->getMbIndexFromUrdfIndex(urdfLinkIndex);
   cp.m_mbIndex = mbIndex;
   cp.m_parentIndex = parentIndex;
-  int parentMbIndex =
-      parentIndex >= 0 ? cache->getMbIndexFromUrdfIndex(parentIndex) : -1;
+  int parentMbIndex = parentIndex >= 0
+                          ? cache->getMbIndexFromUrdfIndex(parentIndex)
+                          : BASELINK_ID;
   cp.m_parentMBIndex = parentMbIndex;
 
   allIndices.emplace_back(std::move(cp));
@@ -255,9 +256,8 @@ void BulletURDFImporter::initURDFToBulletCache(
     cache->m_urdfLinkIndices2BulletLinkIndices.resize(
         numTotalLinksIncludingBase);
     cache->m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
-
-    cache->m_currentMultiBodyLinkIndex =
-        -1;  // multi body base has 'link' index -1
+    // multi body base has 'link' index BASELINK_ID
+    cache->m_currentMultiBodyLinkIndex = BASELINK_ID;
 
     bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0;
     if (maintainLinkOrder) {
@@ -327,7 +327,7 @@ void BulletURDFImporter::convertURDFToBullet(
     parentTransforms[urdfLinkIndex] = parentTransformInWorldSpace;
     std::vector<childParentIndex> allIndices;
 
-    getAllIndices(urdfLinkIndex, -1, allIndices);
+    getAllIndices(urdfLinkIndex, BASELINK_ID, allIndices);
     std::sort(allIndices.begin(), allIndices.end(),
               [](const childParentIndex& a, const childParentIndex& b) {
                 return a.m_index < b.m_index;

src/esp/physics/bullet/BulletURDFImporter.h

@@ -54,7 +54,7 @@ struct URDFToBulletCached {
   std::vector<int> m_urdfLinkIndices2BulletLinkIndices;
   std::vector<btTransform> m_urdfLinkLocalInertialFrames;
 
-  int m_currentMultiBodyLinkIndex{-1};
+  int m_currentMultiBodyLinkIndex{BASELINK_ID};
 
   class btMultiBody* m_bulletMultiBody{nullptr};
 

src/esp/physics/objectWrappers/ManagedArticulatedObject.h

@@ -48,15 +48,15 @@ class ManagedArticulatedObject
     return 1.0;
   }
 
-  scene::SceneNode* getLinkSceneNode(int linkId = -1) const {
+  scene::SceneNode* getLinkSceneNode(int linkId = BASELINK_ID) const {
     if (auto sp = getObjectReference()) {
       return &const_cast<scene::SceneNode&>(sp->getLinkSceneNode(linkId));
     }
     return nullptr;
   }
 
   std::vector<scene::SceneNode*> getLinkVisualSceneNodes(
-      int linkId = -1) const {
+      int linkId = BASELINK_ID) const {
     if (auto sp = getObjectReference()) {
       return sp->getLinkVisualSceneNodes(linkId);
     }
@@ -73,7 +73,7 @@ class ManagedArticulatedObject
     if (auto sp = getObjectReference()) {
       return sp->getNumLinks();
     }
-    return -1;
+    return ID_UNDEFINED;
   }
 
   std::vector<int> getLinkIds() const {
@@ -94,7 +94,7 @@ class ManagedArticulatedObject
     if (auto sp = getObjectReference()) {
       return sp->getLinkIdFromName(_name);
     }
-    return -1;
+    return ID_UNDEFINED;
   }
 
   std::unordered_map<int, int> getLinkObjectIds() const {
@@ -237,7 +237,7 @@ class ManagedArticulatedObject
     if (auto sp = getObjectReference()) {
       return sp->getLinkDoFOffset(linkId);
     }
-    return -1;
+    return ID_UNDEFINED;
   }
 
   int getLinkNumDoFs(int linkId) const {
@@ -251,7 +251,7 @@ class ManagedArticulatedObject
     if (auto sp = getObjectReference()) {
       return sp->getLinkJointPosOffset(linkId);
     }
-    return -1;
+    return ID_UNDEFINED;
   }
 
   int getLinkNumJointPos(int linkId) const {