From 166a5aaba37dee467ce11efc3eb8b8fd754d0061 Mon Sep 17 00:00:00 2001 From: Erik Ogenvik Date: Fri, 23 Mar 2018 19:22:35 +0100 Subject: [PATCH] Initial work on planting entities. --- rulesets/PhysicalDomain.cpp | 213 ++++++++++++++++++++++-- rulesets/PhysicalDomain.h | 3 +- server/CorePropertyManager.cpp | 2 + tests/PhysicalDomainIntegrationTest.cpp | 139 ++++++++++++++++ 4 files changed, 344 insertions(+), 13 deletions(-) diff --git a/rulesets/PhysicalDomain.cpp b/rulesets/PhysicalDomain.cpp index 60b0415c8..1a9be1b90 100644 --- a/rulesets/PhysicalDomain.cpp +++ b/rulesets/PhysicalDomain.cpp @@ -39,6 +39,7 @@ #include "common/TypeNode.h" #include "common/Update.h" #include "common/BaseWorld.h" +#include "EntityProperty.h" #include #include @@ -59,6 +60,7 @@ #include #include #include +#include static const bool debug_flag = false; @@ -255,8 +257,6 @@ PhysicalDomain::PhysicalDomain(LocatedEntity& entity) : auto propellingEntries = static_cast*>(world->getWorldUserInfo()); for (auto& entry : *propellingEntries) { -// const btVector3& velocity = entry.second.bulletEntry->collisionObject->getLinearVelocity(); -// entry.second.bulletEntry->collisionObject->setLinearVelocity(btVector3(entry.second.velocity.x(), velocity.y()+ (world->getGravity().y() * timeStep), entry.second.velocity.z())); float verticalVelocity = entry.second.rigidBody->getLinearVelocity().y(); @@ -754,11 +754,9 @@ void PhysicalDomain::addEntity(LocatedEntity& entity) mass = .0f; } - calculatePositionForEntity(mode, entity, entity.m_location.m_pos); btQuaternion orientation = entity.m_location.m_orientation.isValid() ? Convert::toBullet(entity.m_location.m_orientation) : btQuaternion::getIdentity(); - btVector3 pos = entity.m_location.m_pos.isValid() ? Convert::toBullet(entity.m_location.m_pos) : btVector3(0, 0, 0); short collisionMask; short collisionGroup; @@ -782,8 +780,13 @@ void PhysicalDomain::addEntity(LocatedEntity& entity) entry->centerOfMassOffset = btVector3(0, 0, 0); } entry->collisionObject->setCollisionShape(entry->collisionShape); - entry->collisionObject->setWorldTransform(btTransform(orientation, pos) * btTransform(btQuaternion::getIdentity(), entry->centerOfMassOffset).inverse()); entry->collisionObject->setCollisionFlags(entry->collisionObject->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); + + calculatePositionForEntity(mode, entry, entity.m_location.m_pos); + + entry->collisionObject->setWorldTransform(btTransform(orientation, Convert::toBullet(entity.m_location.m_pos)) + * btTransform(btQuaternion::getIdentity(), entry->centerOfMassOffset).inverse()); + m_dynamicsWorld->addCollisionObject(entry->collisionObject, collisionGroup, collisionMask); m_waterBodies.emplace_back(ghostObject); ghostObject->activate(); @@ -825,8 +828,11 @@ void PhysicalDomain::addEntity(LocatedEntity& entity) btRigidBody* rigidBody = new btRigidBody(rigidBodyCI); entry->collisionObject = rigidBody; + + calculatePositionForEntity(mode, entry, entity.m_location.m_pos); + entry->motionState = new PhysicalMotionState(*entry, *rigidBody, *this, - btTransform(orientation, pos), + btTransform(orientation, Convert::toBullet(entity.m_location.m_pos)), btTransform(btQuaternion::getIdentity(), entry->centerOfMassOffset)); rigidBody->setMotionState(entry->motionState); rigidBody->setAngularFactor(angularFactor); @@ -884,10 +890,10 @@ void PhysicalDomain::addEntity(LocatedEntity& entity) btCollisionObject* visObject = new btCollisionObject(); visObject->setCollisionShape(visSphere); - visObject->setWorldTransform(btTransform(btQuaternion::getIdentity(), pos / VISIBILITY_SCALING_FACTOR)); visObject->setUserPointer(entry); entry->visibilitySphere = visObject; if (entity.m_location.m_pos.isValid()) { + visObject->setWorldTransform(btTransform(btQuaternion::getIdentity(), Convert::toBullet(entity.m_location.m_pos) / VISIBILITY_SCALING_FACTOR)); m_visibilityWorld->addCollisionObject(visObject, VISIBILITY_MASK_OBSERVER, VISIBILITY_MASK_OBSERVABLE); } } @@ -895,11 +901,11 @@ void PhysicalDomain::addEntity(LocatedEntity& entity) btSphereShape* viewSphere = new btSphereShape(0.5f / VISIBILITY_SCALING_FACTOR); btCollisionObject* visObject = new btCollisionObject(); visObject->setCollisionShape(viewSphere); - visObject->setWorldTransform(btTransform(btQuaternion::getIdentity(), pos / VISIBILITY_SCALING_FACTOR)); visObject->setUserPointer(entry); entry->viewSphere = visObject; mContainingEntityEntry.observingThis.insert(entry); if (entity.m_location.m_pos.isValid()) { + visObject->setWorldTransform(btTransform(btQuaternion::getIdentity(), Convert::toBullet(entity.m_location.m_pos) / VISIBILITY_SCALING_FACTOR)); m_visibilityWorld->addCollisionObject(visObject, VISIBILITY_MASK_OBSERVABLE, VISIBILITY_MASK_OBSERVER); } } @@ -1372,7 +1378,8 @@ void PhysicalDomain::entityPropertyApplied(const std::string& name, PropertyBase for (auto& entry : m_terrainSegments) { entry.second.rigidBody->setFriction(static_cast(frictionProp->data())); } - } else if (name == "friction_roll") { + } else if (name == "frid .." + "ction_roll") { auto frictionRollingProp = dynamic_cast*>(&prop); for (auto& entry : m_terrainSegments) { entry.second.rigidBody->setRollingFriction(static_cast(frictionRollingProp->data())); @@ -1394,14 +1401,195 @@ void PhysicalDomain::entityPropertyApplied(const std::string& name, PropertyBase } } -void PhysicalDomain::calculatePositionForEntity(ModeProperty::Mode mode, LocatedEntity& entity, WFMath::Point<3>& pos) + +void PhysicalDomain::calculatePositionForEntity(ModeProperty::Mode mode, PhysicalDomain::BulletEntry* entry, WFMath::Point<3>& pos) { + struct PlantedOnCallback : public btCollisionWorld::ContactResultCallback + { + btVector3 highestPoint; + bool hadHit = false; + const btCollisionObject* highestObject = nullptr; + + explicit PlantedOnCallback(btVector3 highestPoint) + : btCollisionWorld::ContactResultCallback(), highestPoint(highestPoint) + { + } + + bool needsCollision(btBroadphaseProxy* proxy0) const override + { + return true; + } + + btScalar addSingleResult(btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0, int partId0, int index0, + const btCollisionObjectWrapper* colObj1, int partId1, int index1) override + { + + //B will be the existing planted object, A will be the object being planted. + btVector3 point = cp.getPositionWorldOnB(); + + if (point.y() > highestPoint.y()) { + highestPoint = point; + highestObject = colObj1->m_collisionObject; + hadHit = true; + } + + //Returned result is ignored. + return 0; + } + }; + + auto& entity = *entry->entity; + if (mode == ModeProperty::Mode::Planted || mode == ModeProperty::Mode::Free || mode == ModeProperty::Mode::Submerged) { float h = pos.y(); getTerrainHeight(pos.x(), pos.z(), h); if (mode == ModeProperty::Mode::Planted) { - pos.y() = h; + + bool plantedOn = false; + if (entry->collisionObject) { + auto plantedOnProp = entity.getPropertyClass("planted_on"); + if (plantedOnProp) { + const auto& plantedOnEntityRef = plantedOnProp->data(); + if (plantedOnEntityRef && plantedOnEntityRef->getIntId() != m_entity.getIntId()) { + auto I = m_entries.find(plantedOnEntityRef->getIntId()); + if (I != m_entries.end()) { + + BulletEntry* plantedOnBulletEntry = I->second; + + btVector3 aabbMin, aabbMax; + entry->collisionObject->getCollisionShape()->getAabb(entry->collisionObject->getWorldTransform(), aabbMin, aabbMax); + float height = aabbMax.y() - aabbMin.y(); + + float yPos = pos.y(); + + btQuaternion orientation = entity.m_location.m_orientation.isValid() ? Convert::toBullet(entity.m_location.m_orientation) : btQuaternion::getIdentity(); + btTransform transform(orientation, Convert::toBullet(entry->entity->m_location.pos())); + transform *= btTransform(btQuaternion::getIdentity(), entry->centerOfMassOffset).inverse(); + + //auto originalTransform = entry->collisionObject->getWorldTransform(); + entry->collisionObject->setWorldTransform(transform); + + while (yPos > h) { + PlantedOnCallback callback(btVector3(pos.x(), h, pos.z())); + + //Test if the shape collides, otherwise move it downwards until it reaches the ground. + entry->collisionObject->getWorldTransform().getOrigin().setY(yPos); + m_dynamicsWorld->contactPairTest(entry->collisionObject, plantedOnBulletEntry->collisionObject, callback); + + if (callback.hadHit) { + pos.y() = std::max(callback.highestPoint.y(), h); + plantedOn = true; + break; + } + + yPos -= height; + } + + + +// btTransform from(btQuaternion::getIdentity(), Convert::toBullet(entity.m_location.m_pos)); +// btTransform to(from); +// to.getOrigin().setY(h); + +// btCollisionWorld::ClosestRayResultCallback callback(from.getOrigin(), to.getOrigin()); +// rayTestSingle(from, to, plantedOnBulletEntry->collisionObject, +// plantedOnBulletEntry->collisionShape, +// plantedOnBulletEntry->collisionObject->getWorldTransform(), +// callback); +// +// if (callback.hasHit()) { +// pos.y() = std::max(callback.m_hitPointWorld.y(), h); +// plantedOn = true; +// } + + +// BulletEntry* plantedOnBulletEntry = I->second; +// +// +// auto size = entity.m_location.bBox().highCorner() - entity.m_location.bBox().lowCorner(); +// auto offset = entity.m_location.bBox().getCenter(); +// +// auto halfSize = size / 2.0; +// +// float testHeight = pos.y() + entity.m_location.bBox().highCorner().y() - h; +// +// +// +// btCylinderShape testShape(btVector3(halfSize.x(), testHeight / 2, halfSize.z())); +// testShape.setMargin(0); +// btCollisionObject testCollisionObject{}; +// testCollisionObject.setCollisionShape(&testShape); +//// testCollisionObject.setWorldTransform(btTransform(Convert::toBullet(entity.m_location.orientation())), ) +// testCollisionObject.setWorldTransform(btTransform(btQuaternion::getIdentity(), btVector3(pos.x() + offset.x(), h + (testHeight / 2), pos.z() + offset.z())) ); +// +// PlantedOnCallback callback{}; +// m_dynamicsWorld->contactPairTest(&testCollisionObject, plantedOnBulletEntry->collisionObject, callback); +// +// if (callback.hadHit) { +// pos.y() = std::max(callback.highestPoint.y(), h); +// plantedOn = true; +// } +// +//// btTransform from(btQuaternion::getIdentity(), Convert::toBullet(entity.m_location.m_pos)); +//// btTransform to(from); +//// to.getOrigin().setY(h); +// +//// btCollisionWorld::ClosestRayResultCallback callback(from.getOrigin(), to.getOrigin()); +//// rayTestSingle(from, to, plantedOnBulletEntry->collisionObject, +//// plantedOnBulletEntry->collisionShape, +//// plantedOnBulletEntry->collisionObject->getWorldTransform(), +//// callback); +//// +//// if (callback.hasHit()) { +//// pos.y() = std::max(callback.m_hitPointWorld.y(), h); +//// plantedOn = true; +//// } + } + } + } else { + //Look for closest thing it can be planted on. + btVector3 aabbMin, aabbMax; + entry->collisionObject->getCollisionShape()->getAabb(entry->collisionObject->getWorldTransform(), aabbMin, aabbMax); + float height = aabbMax.y() - aabbMin.y(); + + float yPos = pos.y(); + + btQuaternion orientation = entity.m_location.m_orientation.isValid() ? Convert::toBullet(entity.m_location.m_orientation) : btQuaternion::getIdentity(); + btTransform transform(orientation, Convert::toBullet(entry->entity->m_location.pos())); + transform *= btTransform(btQuaternion::getIdentity(), entry->centerOfMassOffset).inverse(); + + entry->collisionObject->setWorldTransform(transform); + + while (yPos > h) { + PlantedOnCallback callback(btVector3(pos.x(), h, pos.z())); + + //Test if the shape collides, otherwise move it downwards until it reaches the ground. + entry->collisionObject->getWorldTransform().getOrigin().setY(yPos); + m_dynamicsWorld->contactTest(entry->collisionObject, callback); + + if (callback.hadHit) { + auto plantedOnEntry = static_cast(callback.highestObject->getUserPointer()); + if (plantedOnEntry) { + pos.y() = std::max(callback.highestPoint.y(), h); + plantedOn = true; + + auto newPlantedOnProp = entry->entity->requirePropertyClass("planted_on"); + newPlantedOnProp->data() = EntityRef(plantedOnEntry->entity); + + break; + } + } + + yPos -= height; + } + } + } + + if (!plantedOn) { + pos.y() = h; + } auto plantedOffsetProp = entity.getPropertyType("planted-offset"); if (plantedOffsetProp) { @@ -1450,7 +1638,7 @@ void PhysicalDomain::applyNewPositionForEntity(BulletEntry* entry, const WFMath: WFMath::Point<3> newPos = pos; - calculatePositionForEntity(mode, entity, newPos); + calculatePositionForEntity(mode, entry, newPos); entity.m_location.m_pos = newPos; @@ -2117,3 +2305,4 @@ void PhysicalDomain::transformRestingEntities(PhysicalDomain::BulletEntry* entry } } + diff --git a/rulesets/PhysicalDomain.h b/rulesets/PhysicalDomain.h index 92c4c558f..175972301 100644 --- a/rulesets/PhysicalDomain.h +++ b/rulesets/PhysicalDomain.h @@ -278,7 +278,7 @@ class PhysicalDomain : public Domain void applyVelocity(BulletEntry& entry, const WFMath::Vector<3>& velocity); - void calculatePositionForEntity(ModeProperty::Mode mode, LocatedEntity& entity, WFMath::Point<3>& pos); + void calculatePositionForEntity(ModeProperty::Mode mode, BulletEntry* entry, WFMath::Point<3>& pos); /** * Called each tick to process any bodies that are moving in water. @@ -296,6 +296,7 @@ class PhysicalDomain : public Domain * @param transformedEntities */ void transformRestingEntities(BulletEntry* entry, const WFMath::Vector<3>& posTransform, std::set& transformedEntities); + }; #endif /* PHYSICALDOMAIN_H_ */ diff --git a/server/CorePropertyManager.cpp b/server/CorePropertyManager.cpp index 694e01f6a..b49a8bfb2 100644 --- a/server/CorePropertyManager.cpp +++ b/server/CorePropertyManager.cpp @@ -172,6 +172,8 @@ CorePropertyManager::CorePropertyManager() installProperty(); installProperty(); + installProperty("planted_on", "string"); + /** * Vertical offset to use when entity is planted, and adjusted to the height of the terrain. */ diff --git a/tests/PhysicalDomainIntegrationTest.cpp b/tests/PhysicalDomainIntegrationTest.cpp index 030645dbb..537a0c299 100644 --- a/tests/PhysicalDomainIntegrationTest.cpp +++ b/tests/PhysicalDomainIntegrationTest.cpp @@ -54,6 +54,7 @@ #include #include +#include using Atlas::Message::Element; using Atlas::Message::ListType; @@ -97,6 +98,8 @@ class PhysicalDomainIntegrationTest : public Cyphesis::TestBase void test_convert(); + void test_plantedOn(); + void test_terrainMods(); void test_lake_rotated(); @@ -138,6 +141,7 @@ long PhysicalDomainIntegrationTest::m_id_counter = 0L; PhysicalDomainIntegrationTest::PhysicalDomainIntegrationTest() { + ADD_TEST(PhysicalDomainIntegrationTest::test_plantedOn); ADD_TEST(PhysicalDomainIntegrationTest::test_terrainMods); ADD_TEST(PhysicalDomainIntegrationTest::test_lake_rotated); ADD_TEST(PhysicalDomainIntegrationTest::test_lake); @@ -171,6 +175,141 @@ void PhysicalDomainIntegrationTest::teardown() { } +#define ASSERT_FUZZY_EQUAL_FN(_lval, _rval, _epsilon, _fn) {\ + if (this->assertFuzzyEqual(#_lval, _lval, #_rval, _rval, #_epsilon, _epsilon, __PRETTY_FUNCTION__,\ + __FILE__, __LINE__) != 0) {_fn(); return;}\ +} + +void PhysicalDomainIntegrationTest::test_plantedOn() +{ + + class TestPhysicalDomain : public PhysicalDomain + { + public: + explicit TestPhysicalDomain(LocatedEntity& entity) : PhysicalDomain(entity) + { + } + + btDiscreteDynamicsWorld* test_getBulletWorld() + { + return m_dynamicsWorld; + } + + + btRigidBody* test_getRigidBody(long id) + { + return btRigidBody::upcast(m_entries.find(id)->second->collisionObject); + } + }; + + std::vector shapes{"box", "cylinder-x", "cylinder-y", "cylinder-z", "capsule-x", "capsule-y", "capsule-z"}; + + for (auto plantedShape : shapes) { + for (auto plantedOnTopShape : shapes) { + GeometryProperty* plantedGeometryProperty = new GeometryProperty(); + plantedGeometryProperty->set(MapType{{"type", plantedShape}}); + + auto id = newId(); + Entity* rootEntity = new Entity(std::to_string(id), id); + TerrainProperty* terrainProperty = new TerrainProperty(); + Mercator::Terrain& terrain = terrainProperty->getData(); + terrain.setBasePoint(0, 0, Mercator::BasePoint(10)); + terrain.setBasePoint(0, 1, Mercator::BasePoint(10)); + terrain.setBasePoint(1, 0, Mercator::BasePoint(10)); + terrain.setBasePoint(1, 1, Mercator::BasePoint(10)); + rootEntity->setProperty("terrain", terrainProperty); + rootEntity->m_location.m_pos = WFMath::Point<3>::ZERO(); + rootEntity->m_location.setBBox({{-64, -64, -64}, + {64, 64, 64}}); + TestPhysicalDomain* domain = new TestPhysicalDomain(*rootEntity); + + ModeProperty* modeProperty = new ModeProperty(); + modeProperty->set("planted"); + + + id = newId(); + Entity* planted1 = new Entity(std::to_string(id), id); + planted1->m_location.m_pos = WFMath::Point<3>(0, 10, 0); + planted1->m_location.setBBox({{-1, -1, -1}, + {1, 1, 1}}); + planted1->setProperty(ModeProperty::property_name, modeProperty); + planted1->setProperty(GeometryProperty::property_name, plantedGeometryProperty); + + domain->addEntity(*planted1); + + OpVector res; + domain->tick(0, res); + + ASSERT_FUZZY_EQUAL(10, planted1->m_location.m_pos.y(), 0.1); + { + auto* planted1RigidBody = domain->test_getRigidBody(planted1->getIntId()); + btVector3 aabbMin, aabbMax; + planted1RigidBody->getAabb(aabbMin, aabbMax); + ASSERT_FUZZY_EQUAL_FN(aabbMin.y(), 9, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1'.", plantedShape)); }); + ASSERT_FUZZY_EQUAL_FN(aabbMax.y(), 11, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1'.", plantedShape)); }); + + } + + id = newId(); + Entity* planted2 = new Entity(std::to_string(id), id); + planted2->m_location.m_pos = WFMath::Point<3>(0, 15, 0); + planted2->m_location.setBBox({{-1, -1, -1}, + {1, 1, 1}}); + planted2->setProperty(ModeProperty::property_name, modeProperty); + planted2->setProperty(GeometryProperty::property_name, plantedGeometryProperty); + + domain->addEntity(*planted2); + + domain->tick(0, res); + + ASSERT_TRUE(planted2->getPropertyClass("planted_on")->data()); + ASSERT_EQUAL(planted1->getIntId(), planted2->getPropertyClass("planted_on")->data()->getIntId()); + ASSERT_FUZZY_EQUAL(11, planted2->m_location.m_pos.y(), 0.1); + { + auto* planted2RigidBody = domain->test_getRigidBody(planted2->getIntId()); + btVector3 aabbMin, aabbMax; + planted2RigidBody->getAabb(aabbMin, aabbMax); + ASSERT_FUZZY_EQUAL_FN(aabbMin.y(), 10, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1'.", plantedShape)); }); + ASSERT_FUZZY_EQUAL_FN(aabbMax.y(), 12, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1'.", plantedShape)); }); + + } + + + + id = newId(); + Entity* plantedOn = new Entity(std::to_string(id), id); + plantedOn->m_location.m_pos = {0, 15, 0}; + plantedOn->setProperty(ModeProperty::property_name, modeProperty); + plantedOn->m_location.setBBox({{-1, 0, -1}, + {1, 1, 1}}); + EntityProperty* plantedOnProperty = new EntityProperty(); + plantedOnProperty->data() = EntityRef(planted1); + plantedOn->setProperty("planted_on", plantedOnProperty); + + GeometryProperty* geometryProperty = new GeometryProperty(); + geometryProperty->set(MapType{{"type", plantedOnTopShape}}); + plantedOn->setProperty(GeometryProperty::property_name, geometryProperty); + + domain->addEntity(*plantedOn); + + + ASSERT_TRUE(plantedOn->getPropertyClass("planted_on")->data()); + ASSERT_EQUAL(planted1->getIntId(), plantedOn->getPropertyClass("planted_on")->data()->getIntId()); + ASSERT_FUZZY_EQUAL_FN(plantedOn->m_location.m_pos.y(), 11, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1' on top of '%2'.", plantedOnTopShape, plantedShape)); }); + { + auto* plantedOnRigidBody = domain->test_getRigidBody(plantedOn->getIntId()); + btVector3 aabbMin, aabbMax; + plantedOnRigidBody->getAabb(aabbMin, aabbMax); + ASSERT_FUZZY_EQUAL_FN(aabbMin.y(), 11, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1' on top of '%2'.", plantedOnTopShape, plantedShape)); }); + ASSERT_FUZZY_EQUAL_FN(aabbMax.y(), 12, 0.1, [&]() { this->addFailure(String::compose("Using shape '%1' on top of '%2'.", plantedOnTopShape, plantedShape)); }); + } + + } + } + +} + + void PhysicalDomainIntegrationTest::test_terrainMods() {