From 772bc70e341b3d8a5ddfc8e12b5099bdfe35b424 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 8 Feb 2024 09:55:18 -0500 Subject: [PATCH] --set stage ID to 0; --- examples/viewer.py | 2 +- src/esp/physics/RigidStage.cpp | 2 +- src/esp/physics/bullet/BulletPhysicsManager.cpp | 6 +++--- src/tests/PhysicsTest.cpp | 8 ++++---- tests/test_physics.py | 10 ++++++---- tests/test_simulator.py | 2 +- 6 files changed, 16 insertions(+), 14 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 0439e76c45..866718987d 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -723,7 +723,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: hit_object, ao_link = -1, -1 hit_info = raycast_results.hits[0] - if hit_info.object_id >= 0: + if hit_info.object_id >= 1: # we hit an non-staged collision object ro_mngr = self.sim.get_rigid_object_manager() ao_mngr = self.sim.get_articulated_object_manager() diff --git a/src/esp/physics/RigidStage.cpp b/src/esp/physics/RigidStage.cpp index 2a1d22f3d8..b3aff26ca6 100644 --- a/src/esp/physics/RigidStage.cpp +++ b/src/esp/physics/RigidStage.cpp @@ -9,7 +9,7 @@ namespace physics { RigidStage::RigidStage(scene::SceneNode* rigidBodyNode, const assets::ResourceManager& resMgr) - : RigidBase(rigidBodyNode, ID_UNDEFINED, resMgr) {} + : RigidBase(rigidBodyNode, 0, resMgr) {} bool RigidStage::initialize( metadata::attributes::AbstractObjectAttributes::ptr initAttributes) { diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index 7ec6bec42c..5c4bb22465 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -524,7 +524,7 @@ void BulletPhysicsManager::lookUpObjectIdAndLinkId( *linkId = -1; // If the lookup fails, default to the stage. TODO: better error-handling. - *objectId = -1; + *objectId = 0; auto rawColObjIdIter = collisionObjToObjIds_->find(colObj); if (rawColObjIdIter != collisionObjToObjIds_->end()) { int rawObjectId = rawColObjIdIter->second; @@ -558,8 +558,8 @@ std::vector BulletPhysicsManager::getContactPoints() const { const btPersistentManifold* manifold = dispatcher->getInternalManifoldPointer()[i]; - int objectIdA = -2; // stage is -1 - int objectIdB = -2; + int objectIdA = -1; // stage is 0 + int objectIdB = -1; int linkIndexA = -1; // -1 if not a multibody int linkIndexB = -1; diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index c16837f894..ca154e068c 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -726,7 +726,7 @@ void PhysicsTest::testNumActiveContactPoints() { CORRADE_COMPARE(physicsManager_->getNumActiveOverlappingPairs(), 2); CORRADE_COMPARE( physicsManager_->getStepCollisionSummary(), - "[RigidObject, cubeSolid, id 0] vs [Stage, subpart 6], 4 points\n"); + "[RigidObject, cubeSolid, id 1] vs [Stage, subpart 6], 4 points\n"); float totalNormalForce = 0; for (auto& cp : allContactPoints) { @@ -736,9 +736,9 @@ void PhysicsTest::testNumActiveContactPoints() { CORRADE_COMPARE_AS( (cp.contactNormalOnBInWS - Magnum::Vector3{0.0, 1.0, 0.0}).length(), 1.0e-4, Cr::TestSuite::Compare::LessOrEqual); - // one object is the cube (0), other is the stage (-1) - CORRADE_COMPARE(cp.objectIdA, 0); - CORRADE_COMPARE(cp.objectIdB, -1); + // one object is the cube (1), other is the stage (0) + CORRADE_COMPARE(cp.objectIdA, 1); + CORRADE_COMPARE(cp.objectIdB, 0); // accumulate the normal force totalNormalForce += cp.normalForce; // solver should keep the cube at the contact boundary (~0 penetration) diff --git a/tests/test_physics.py b/tests/test_physics.py index 4ec9b9b8d5..b6c8da4a1f 100644 --- a/tests/test_physics.py +++ b/tests/test_physics.py @@ -543,7 +543,8 @@ def test_raycast(): atol=0.07, ) assert abs(raycast_results.hits[0].ray_distance - 6.831) < 0.001 - assert raycast_results.hits[0].object_id == -1 + # hit stage + assert raycast_results.hits[0].object_id == 0 # add a primitive object to the world and test a ray away from the origin cube_prim_handle = obj_template_mgr.get_template_handles("cube")[0] @@ -906,7 +907,7 @@ def test_articulated_object_add_remove(): robot = art_obj_mgr.add_articulated_object_from_urdf(filepath=robot_file) assert robot assert robot.is_alive - assert robot.object_id == 0 # first robot added + assert robot.object_id == 1 # first robot added # add a second robot robot2 = art_obj_mgr.add_articulated_object_from_urdf( @@ -1859,7 +1860,8 @@ def test_rigid_constraints(): assert robot.translation[1] < -3 # hang AO from the world with P2P - constraint_settings_2.object_id_b = -1 + # object_id_b is stage + constraint_settings_2.object_id_b = 0 constraint_settings_2.constraint_type = ( habitat_sim.physics.RigidConstraintType.PointToPoint ) @@ -2032,7 +2034,7 @@ def test_bullet_collision_helper(): assert sim.get_physics_num_active_overlapping_pairs() == 1 assert ( sim.get_physics_step_collision_summary() - == "[RigidObject, cubeSolid, id 0] vs [Stage, subpart 0], 4 points\n" + == "[RigidObject, cubeSolid, id 1] vs [Stage, subpart 0], 4 points\n" ) sim.step_physics(3.0) diff --git a/tests/test_simulator.py b/tests/test_simulator.py index 6e4b3c2d9d..2186616bd2 100644 --- a/tests/test_simulator.py +++ b/tests/test_simulator.py @@ -247,7 +247,7 @@ def test_object_template_editing(): # test adding a new object obj = rigid_obj_mgr.add_object_by_template_id(template_ids[0]) - assert obj.object_id != -1 + assert obj.object_id != 0 # test getting initialization templates stage_init_template = sim.get_stage_initialization_template()