From 54455e5fcb85d84abc73201d38e2bb0a497c7cc7 Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 26 Jul 2023 11:52:00 +0200 Subject: [PATCH 1/7] Query ExtraContactData in gz-sim 2 gz-physics interface Now the EntityContactMap contains a deque of pairs of ContactPoint and ExtraContactData. That way the messages can be populated with Normals, Forces, and Depth. The force on body 2 is equal and opposite to the force on body 1. Fixes Issue #2037 Signed-off-by: Anton Bredenbeck --- src/systems/physics/Physics.cc | 58 +++++++++++++++++++++++++++------- 1 file changed, 46 insertions(+), 12 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index b81f17330ac..44cca9bad5e 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -3742,12 +3743,21 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) return; } + // Using ExtraContactData to expose contact Norm, Force & Depth + using Policy = physics::FeaturePolicy3d; + using GCFeature = physics::GetContactsFromLastStepFeature; + using ExtraContactData = GCFeature::ExtraContactDataT; + + // A contact is described by a contact_point and the corresponding + // extraContactData which we bundle in a pair data structure + using ContactData = std::pair; // Each contact object we get from gz-physics contains the EntityPtrs of the // two colliding entities and other data about the contact such as the - // position. This map groups contacts so that it is easy to query all the + // position and extra contact date (wrench, normal and penetration depth). + // This map groups contacts so that it is easy to query all the // contacts of one entity. - using EntityContactMap = std::unordered_map>; + using EntityContactMap = std::unordered_map>; // This data structure is essentially a mapping between a pair of entities and // a list of pointers to their contact object. We use a map inside a map to @@ -3761,16 +3771,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contact = contactComposite.Get(); + const auto &contact_point = contactComposite.Get(); + const auto &extra_contact_data = + contactComposite.Query(); auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); + this->entityCollisionMap.GetByPhysicsId(contact_point.collision1->EntityID()); auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); + this->entityCollisionMap.GetByPhysicsId(contact_point.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - entityContactMap[coll1Entity][coll2Entity].push_back(&contact); - entityContactMap[coll2Entity][coll1Entity].push_back(&contact); + ContactData data = std::make_pair(&contact_point, extra_contact_data); + entityContactMap[coll1Entity][coll2Entity].push_back(data); + entityContactMap[coll2Entity][coll1Entity].push_back(data); } } @@ -3808,12 +3821,33 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) contactMsg->mutable_collision2()->set_name( removeParentScope(scopedName(collEntity2, _ecm, "::", 0), "::")); } - for (const auto &contact : contactData) + for (const auto &contact_data : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + position->set_x(contact_data.first->point.x()); + position->set_y(contact_data.first->point.y()); + position->set_z(contact_data.first->point.z()); + + auto *normal = contactMsg->add_normal(); + normal->set_x(contact_data.second->normal.x()); + normal->set_y(contact_data.second->normal.y()); + normal->set_z(contact_data.second->normal.z()); + + auto *wrench = contactMsg->add_wrench(); + auto *body1_wrench = wrench->mutable_body_1_wrench(); + auto *body1_force = body1_wrench->mutable_force(); + body1_force->set_x(contact_data.second->force.x()); + body1_force->set_y(contact_data.second->force.y()); + body1_force->set_z(contact_data.second->force.z()); + + // The force on the second body is equal and opposite + auto *body2_wrench = wrench->mutable_body_2_wrench(); + auto *body2_force = body2_wrench->mutable_force(); + body2_force->set_x(-contact_data.second->force.x()); + body2_force->set_y(-contact_data.second->force.y()); + body2_force->set_z(-contact_data.second->force.z()); + + contactMsg->add_depth(contact_data.second->depth); } } From 631f7e81cdb744a086b0a2beb969a577aa2aec89 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 31 Jul 2023 12:08:12 +0200 Subject: [PATCH 2/7] Address points from code-review - snake_case -> camelCase - check whether contactExtraData is not nullptr before accessing Signed-off-by: Anton Bredenbeck --- src/systems/physics/Physics.cc | 68 ++++++++++++++++++---------------- 1 file changed, 37 insertions(+), 31 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 44cca9bad5e..3088804ed04 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3748,7 +3748,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) using GCFeature = physics::GetContactsFromLastStepFeature; using ExtraContactData = GCFeature::ExtraContactDataT; - // A contact is described by a contact_point and the corresponding + // A contact is described by a contactPoint and the corresponding // extraContactData which we bundle in a pair data structure using ContactData = std::pair; @@ -3771,17 +3771,17 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contact_point = contactComposite.Get(); - const auto &extra_contact_data = + const auto &contactPoint = contactComposite.Get(); + const auto &extraContactData = contactComposite.Query(); auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contact_point.collision1->EntityID()); + this->entityCollisionMap.GetByPhysicsId(contactPoint.collision1->EntityID()); auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contact_point.collision2->EntityID()); + this->entityCollisionMap.GetByPhysicsId(contactPoint.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - ContactData data = std::make_pair(&contact_point, extra_contact_data); + ContactData data = std::make_pair(&contactPoint, extraContactData); entityContactMap[coll1Entity][coll2Entity].push_back(data); entityContactMap[coll2Entity][coll1Entity].push_back(data); } @@ -3821,33 +3821,39 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) contactMsg->mutable_collision2()->set_name( removeParentScope(scopedName(collEntity2, _ecm, "::", 0), "::")); } - for (const auto &contact_data : contactData) + for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact_data.first->point.x()); - position->set_y(contact_data.first->point.y()); - position->set_z(contact_data.first->point.z()); - - auto *normal = contactMsg->add_normal(); - normal->set_x(contact_data.second->normal.x()); - normal->set_y(contact_data.second->normal.y()); - normal->set_z(contact_data.second->normal.z()); - - auto *wrench = contactMsg->add_wrench(); - auto *body1_wrench = wrench->mutable_body_1_wrench(); - auto *body1_force = body1_wrench->mutable_force(); - body1_force->set_x(contact_data.second->force.x()); - body1_force->set_y(contact_data.second->force.y()); - body1_force->set_z(contact_data.second->force.z()); - - // The force on the second body is equal and opposite - auto *body2_wrench = wrench->mutable_body_2_wrench(); - auto *body2_force = body2_wrench->mutable_force(); - body2_force->set_x(-contact_data.second->force.x()); - body2_force->set_y(-contact_data.second->force.y()); - body2_force->set_z(-contact_data.second->force.z()); - - contactMsg->add_depth(contact_data.second->depth); + position->set_x(contact.first->point.x()); + position->set_y(contact.first->point.y()); + position->set_z(contact.first->point.z()); + + // Check if the extra contact data exists, + // since not all physics engines support it. + // Then, fill the msg with extra data. + if(contact.second != nullptr) + { + auto *normal = contactMsg->add_normal(); + normal->set_x(contact.second->normal.x()); + normal->set_y(contact.second->normal.y()); + normal->set_z(contact.second->normal.z()); + + auto *wrench = contactMsg->add_wrench(); + auto *body1_wrench = wrench->mutable_body_1_wrench(); + auto *body1_force = body1_wrench->mutable_force(); + body1_force->set_x(contact.second->force.x()); + body1_force->set_y(contact.second->force.y()); + body1_force->set_z(contact.second->force.z()); + + // The force on the second body is equal and opposite + auto *body2_wrench = wrench->mutable_body_2_wrench(); + auto *body2_force = body2_wrench->mutable_force(); + body2_force->set_x(-contact.second->force.x()); + body2_force->set_y(-contact.second->force.y()); + body2_force->set_z(-contact.second->force.z()); + + contactMsg->add_depth(contact.second->depth); + } } } From d67d41ad29af07ae5e074e48bb630fcb59597026 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 31 Jul 2023 15:20:03 +0200 Subject: [PATCH 3/7] Add world for testing extraContactData Signed-off-by: Anton Bredenbeck --- test/worlds/contact_extra_data.sdf | 53 ++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 test/worlds/contact_extra_data.sdf diff --git a/test/worlds/contact_extra_data.sdf b/test/worlds/contact_extra_data.sdf new file mode 100644 index 00000000000..4f5ccc9d920 --- /dev/null +++ b/test/worlds/contact_extra_data.sdf @@ -0,0 +1,53 @@ + + + + + 0 + + + + + + + + + 0 0 0.5 0 0.0 0 + + 1.0 + + 0.4 + 0.4 + 0.4 + + + + + + + 1.0 + + + + + + + 1.0 + + + + + + collision_sphere + /test_extra_collision_data + + 1 + 1000 + + + + + From 8e0916eb41261adeb0957c451290ed139968252e Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 1 Aug 2023 17:51:25 +0200 Subject: [PATCH 4/7] Add ground plane to extra data test world Signed-off-by: Anton Bredenbeck --- test/worlds/contact_extra_data.sdf | 52 ++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/test/worlds/contact_extra_data.sdf b/test/worlds/contact_extra_data.sdf index 4f5ccc9d920..def44661df4 100644 --- a/test/worlds/contact_extra_data.sdf +++ b/test/worlds/contact_extra_data.sdf @@ -14,6 +14,58 @@ name="gz::sim::systems::Contact"> + 0 0 -9.81 + + + true + + + + + 0 0 1 + 1 1 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 -0 0 + false + + 0 0 0.5 0 0.0 0 From 9c157becffa2c55ff6857e14626b23fe7bd7aa27 Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 1 Aug 2023 17:51:44 +0200 Subject: [PATCH 5/7] Add test for extraContactData transport Signed-off-by: Anton Bredenbeck --- test/integration/contact_system.cc | 81 ++++++++++++++++++++++++++++++ 1 file changed, 81 insertions(+) diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index 10431e496eb..a4363699042 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -272,6 +272,87 @@ TEST_F(ContactSystemTest, } } +///////////////////////////////////////////////// +// The test checks that contacts are published with +// the correct extraContactData +TEST_F(ContactSystemTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(ExtraContactData)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/contact_extra_data.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::mutex contactMutex; + std::vector contactMsgs; + + auto contactCb = [&](const msgs::Contacts &_msg) -> void + { + std::lock_guard lock(contactMutex); + contactMsgs.push_back(_msg); + }; + + // subscribe to contacts topic + transport::Node node; + // Have to create an lvalue here for Node::Subscribe to work. + auto callbackFunc = std::function(contactCb); + node.Subscribe("/test_extra_collision_data", callbackFunc); + + // Run server for 10 iters to ensure + // contact solver has converged + size_t iters = 10; + server.Run(true, iters, false); + { + std::lock_guard lock(contactMutex); + ASSERT_GE(contactMsgs.size(), 1u); + } + + { + std::lock_guard lock(contactMutex); + const auto &lastContacts = contactMsgs.back(); + EXPECT_EQ(1, lastContacts.contact_size()); + + // The sphere weighs 1kg and gravity is set to 9.81 m/s^2 + // Hence the contact force should be m*g = 9.81 N + // along the z-axis. The force on body 2 should be equal + // and opposite. + // All torques should be zero. + // The normal should align with the world z-axis + for (const auto &contact : lastContacts.contact()) + { + ASSERT_EQ(1, contact.wrench_size()); + ASSERT_EQ(1, contact.normal_size()); + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().x(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().y(), 5e-2); + EXPECT_NEAR(9.81, contact.wrench().Get(0).body_1_wrench().force().z(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().x(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().y(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().z(), 5e-2); + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().x(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().y(), 5e-2); + EXPECT_NEAR(-9.81, contact.wrench().Get(0).body_2_wrench().force().z(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().x(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().y(), 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().z(), 5e-2); + + EXPECT_NEAR(0.0, contact.normal().Get(0).x(), 5e-2); + EXPECT_NEAR(0.0, contact.normal().Get(0).y(), 5e-2); + EXPECT_NEAR(1.0, contact.normal().Get(0).z(), 5e-2); + + } + } +} + TEST_F(ContactSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RemoveContactSensor)) { From 064e9bcc695d5a5a29f0f02158c3e2bfde3c30ed Mon Sep 17 00:00:00 2001 From: Anton Bredenbeck Date: Tue, 1 Aug 2023 23:14:30 +0200 Subject: [PATCH 6/7] Fixed some more snake_case to camelCase style issues Signed-off-by: Anton Bredenbeck --- src/systems/physics/Physics.cc | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 3088804ed04..49e3eb442ae 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3839,18 +3839,18 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) normal->set_z(contact.second->normal.z()); auto *wrench = contactMsg->add_wrench(); - auto *body1_wrench = wrench->mutable_body_1_wrench(); - auto *body1_force = body1_wrench->mutable_force(); - body1_force->set_x(contact.second->force.x()); - body1_force->set_y(contact.second->force.y()); - body1_force->set_z(contact.second->force.z()); + auto *body1Wrench = wrench->mutable_body_1_wrench(); + auto *body1Force = body1Wrench->mutable_force(); + body1Force->set_x(contact.second->force.x()); + body1Force->set_y(contact.second->force.y()); + body1Force->set_z(contact.second->force.z()); // The force on the second body is equal and opposite - auto *body2_wrench = wrench->mutable_body_2_wrench(); - auto *body2_force = body2_wrench->mutable_force(); - body2_force->set_x(-contact.second->force.x()); - body2_force->set_y(-contact.second->force.y()); - body2_force->set_z(-contact.second->force.z()); + auto *body2Wrench = wrench->mutable_body_2_wrench(); + auto *body2Force = body2Wrench->mutable_force(); + body2Force->set_x(-contact.second->force.x()); + body2Force->set_y(-contact.second->force.y()); + body2Force->set_z(-contact.second->force.z()); contactMsg->add_depth(contact.second->depth); } From ad6b339ab27804113b8535f51cb516b65bca986d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 3 Aug 2023 02:05:18 -0500 Subject: [PATCH 7/7] Fix codecheck errors Signed-off-by: Addisu Z. Taddese --- src/systems/physics/Physics.cc | 18 +++++------ test/integration/contact_system.cc | 50 ++++++++++++++++++------------ 2 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 49e3eb442ae..8cd8991eed6 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3746,7 +3746,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) // Using ExtraContactData to expose contact Norm, Force & Depth using Policy = physics::FeaturePolicy3d; using GCFeature = physics::GetContactsFromLastStepFeature; - using ExtraContactData = GCFeature::ExtraContactDataT; + using ExtraContactData = GCFeature::ExtraContactDataT; // A contact is described by a contactPoint and the corresponding // extraContactData which we bundle in a pair data structure @@ -3771,17 +3771,17 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contactPoint = contactComposite.Get(); - const auto &extraContactData = - contactComposite.Query(); - auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contactPoint.collision1->EntityID()); - auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contactPoint.collision2->EntityID()); + const auto &contactPoint = + contactComposite.Get(); + const auto &extraContactData = contactComposite.Query(); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - ContactData data = std::make_pair(&contactPoint, extraContactData); + ContactData data = std::make_pair(&contactPoint, extraContactData); entityContactMap[coll1Entity][coll2Entity].push_back(data); entityContactMap[coll2Entity][coll1Entity].push_back(data); } diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index a4363699042..2ac7694388d 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -273,7 +273,7 @@ TEST_F(ContactSystemTest, } ///////////////////////////////////////////////// -// The test checks that contacts are published with +// The test checks that contacts are published with // the correct extraContactData TEST_F(ContactSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ExtraContactData)) @@ -321,34 +321,46 @@ TEST_F(ContactSystemTest, EXPECT_EQ(1, lastContacts.contact_size()); // The sphere weighs 1kg and gravity is set to 9.81 m/s^2 - // Hence the contact force should be m*g = 9.81 N + // Hence the contact force should be m*g = 9.81 N // along the z-axis. The force on body 2 should be equal - // and opposite. + // and opposite. // All torques should be zero. // The normal should align with the world z-axis for (const auto &contact : lastContacts.contact()) { ASSERT_EQ(1, contact.wrench_size()); ASSERT_EQ(1, contact.normal_size()); - - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().x(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().y(), 5e-2); - EXPECT_NEAR(9.81, contact.wrench().Get(0).body_1_wrench().force().z(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().x(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().y(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().z(), 5e-2); - - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().x(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().y(), 5e-2); - EXPECT_NEAR(-9.81, contact.wrench().Get(0).body_2_wrench().force().z(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().x(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().y(), 5e-2); - EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().z(), 5e-2); - + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().y(), + 5e-2); + EXPECT_NEAR(9.81, contact.wrench().Get(0).body_1_wrench().force().z(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().y(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().z(), + 5e-2); + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().y(), + 5e-2); + EXPECT_NEAR(-9.81, contact.wrench().Get(0).body_2_wrench().force().z(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().y(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().z(), + 5e-2); + EXPECT_NEAR(0.0, contact.normal().Get(0).x(), 5e-2); EXPECT_NEAR(0.0, contact.normal().Get(0).y(), 5e-2); EXPECT_NEAR(1.0, contact.normal().Get(0).z(), 5e-2); - + } } }