diff --git a/source/EngineImpl/EngineWorker.cpp b/source/EngineImpl/EngineWorker.cpp index 6d6396e78..642494bcc 100644 --- a/source/EngineImpl/EngineWorker.cpp +++ b/source/EngineImpl/EngineWorker.cpp @@ -472,6 +472,17 @@ void EngineWorker::testOnly_createConnection(uint64_t objectId1, uint64_t object _simulationCudaFacade->testOnly_createConnection(objectId1, objectId2); } +void EngineWorker::testOnly_createConnectionWithAbsAngle( + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2) +{ + EngineWorkerGuard access(this); + _simulationCudaFacade->testOnly_createConnectionWithAbsAngle(objectId1, objectId2, desiredDistance, desiredAbsAngle1, desiredAbsAngle2); +} + void EngineWorker::testOnly_cleanupAfterTimestep() { EngineWorkerGuard access(this); diff --git a/source/EngineImpl/EngineWorker.h b/source/EngineImpl/EngineWorker.h index dd673a975..79d83d4dc 100644 --- a/source/EngineImpl/EngineWorker.h +++ b/source/EngineImpl/EngineWorker.h @@ -120,6 +120,7 @@ class EngineWorker // Only for tests void testOnly_mutate(uint64_t objectId); void testOnly_createConnection(uint64_t objectId1, uint64_t objectId2); + void testOnly_createConnectionWithAbsAngle(uint64_t objectId1, uint64_t objectId2, float desiredDistance, float desiredAbsAngle1, float desiredAbsAngle2); void testOnly_cleanupAfterTimestep(); void testOnly_cleanupAfterDataManipulation(); void testOnly_resizeArrays(ArraySizesForGpuEntities const& sizeDelta); diff --git a/source/EngineImpl/SimulationCudaFacade.cu b/source/EngineImpl/SimulationCudaFacade.cu index 3b9d15e4a..c828b30bf 100644 --- a/source/EngineImpl/SimulationCudaFacade.cu +++ b/source/EngineImpl/SimulationCudaFacade.cu @@ -595,6 +595,19 @@ void _SimulationCudaFacade::testOnly_createConnection(uint64_t objectId1, uint64 syncAndCheck(); } +void _SimulationCudaFacade::testOnly_createConnectionWithAbsAngle( + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2) +{ + checkAndProcessSimulationParameterChanges(); + TestKernelsService::get().testOnly_createConnectionWithAbsAngle( + _settings.cudaSettings, getSimulationDataPtrCopy(), objectId1, objectId2, desiredDistance, desiredAbsAngle1, desiredAbsAngle2); + syncAndCheck(); +} + void _SimulationCudaFacade::testOnly_cleanupAfterTimestep() { checkAndProcessSimulationParameterChanges(); diff --git a/source/EngineImpl/SimulationCudaFacade.cuh b/source/EngineImpl/SimulationCudaFacade.cuh index da0fcd493..e8b5646bf 100644 --- a/source/EngineImpl/SimulationCudaFacade.cuh +++ b/source/EngineImpl/SimulationCudaFacade.cuh @@ -106,6 +106,7 @@ public: // Only for tests void testOnly_mutate(uint64_t objectId); void testOnly_createConnection(uint64_t objectId1, uint64_t objectId2); + void testOnly_createConnectionWithAbsAngle(uint64_t objectId1, uint64_t objectId2, float desiredDistance, float desiredAbsAngle1, float desiredAbsAngle2); void testOnly_cleanupAfterTimestep(); void testOnly_cleanupAfterDataManipulation(); void testOnly_resizeArrays(ArraySizesForGpuEntities const& sizeDelta); diff --git a/source/EngineImpl/SimulationFacadeImpl.cpp b/source/EngineImpl/SimulationFacadeImpl.cpp index d55b86de9..3aa36e15b 100644 --- a/source/EngineImpl/SimulationFacadeImpl.cpp +++ b/source/EngineImpl/SimulationFacadeImpl.cpp @@ -387,6 +387,16 @@ void _SimulationFacadeImpl::testOnly_createConnection(uint64_t objectId1, uint64 _worker.testOnly_createConnection(objectId1, objectId2); } +void _SimulationFacadeImpl::testOnly_createConnectionWithAbsAngle( + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2) +{ + _worker.testOnly_createConnectionWithAbsAngle(objectId1, objectId2, desiredDistance, desiredAbsAngle1, desiredAbsAngle2); +} + void _SimulationFacadeImpl::testOnly_cleanupAfterTimestep() { _worker.testOnly_cleanupAfterTimestep(); diff --git a/source/EngineImpl/SimulationFacadeImpl.h b/source/EngineImpl/SimulationFacadeImpl.h index ec495c0b6..e53322372 100644 --- a/source/EngineImpl/SimulationFacadeImpl.h +++ b/source/EngineImpl/SimulationFacadeImpl.h @@ -108,6 +108,8 @@ class _SimulationFacadeImpl : public _SimulationFacade // for tests only void testOnly_mutate(uint64_t objectId) override; void testOnly_createConnection(uint64_t objectId1, uint64_t objectId2) override; + void testOnly_createConnectionWithAbsAngle(uint64_t objectId1, uint64_t objectId2, float desiredDistance, float desiredAbsAngle1, float desiredAbsAngle2) + override; void testOnly_cleanupAfterTimestep() override; void testOnly_cleanupAfterDataManipulation() override; void testOnly_resizeArrays(ArraySizesForGpuEntities const& sizeDelta) override; diff --git a/source/EngineImpl/TestKernelsService.cu b/source/EngineImpl/TestKernelsService.cu index c6b8ecc43..95400f17b 100644 --- a/source/EngineImpl/TestKernelsService.cu +++ b/source/EngineImpl/TestKernelsService.cu @@ -25,6 +25,18 @@ void TestKernelsService::testOnly_createConnection(CudaSettings const& gpuSettin KERNEL_CALL_1_1(cudaTestCreateConnection, data, objectId1, objectId2); } +void TestKernelsService::testOnly_createConnectionWithAbsAngle( + CudaSettings const& gpuSettings, + SimulationData const& data, + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2) +{ + KERNEL_CALL_1_1(cudaTestCreateConnectionWithAbsAngle, data, objectId1, objectId2, desiredDistance, desiredAbsAngle1, desiredAbsAngle2); +} + bool TestKernelsService::testOnly_isDataValid(CudaSettings const& gpuSettings, SimulationData const& data) { setValueToDevice(_cudaBoolResult, true); diff --git a/source/EngineImpl/TestKernelsService.cuh b/source/EngineImpl/TestKernelsService.cuh index 798286677..a7ae8d934 100644 --- a/source/EngineImpl/TestKernelsService.cuh +++ b/source/EngineImpl/TestKernelsService.cuh @@ -16,6 +16,14 @@ public: void testOnly_mutate(CudaSettings const& gpuSettings, SimulationData const& data, uint64_t objectId); void testOnly_createConnection(CudaSettings const& gpuSettings, SimulationData const& data, uint64_t objectId1, uint64_t objectId2); + void testOnly_createConnectionWithAbsAngle( + CudaSettings const& gpuSettings, + SimulationData const& data, + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2); bool testOnly_isDataValid(CudaSettings const& gpuSettings, SimulationData const& data); private: diff --git a/source/EngineInterface/SimulationFacade.h b/source/EngineInterface/SimulationFacade.h index 13c40a68c..904d0e214 100644 --- a/source/EngineInterface/SimulationFacade.h +++ b/source/EngineInterface/SimulationFacade.h @@ -122,6 +122,8 @@ class _SimulationFacade //**************** virtual void testOnly_mutate(uint64_t objectId) = 0; virtual void testOnly_createConnection(uint64_t objectId1, uint64_t objectId2) = 0; + virtual void + testOnly_createConnectionWithAbsAngle(uint64_t objectId1, uint64_t objectId2, float desiredDistance, float desiredAbsAngle1, float desiredAbsAngle2) = 0; virtual void testOnly_cleanupAfterTimestep() = 0; virtual void testOnly_cleanupAfterDataManipulation() = 0; virtual void testOnly_resizeArrays(ArraySizesForGpuEntities const& sizeDelta) = 0; diff --git a/source/EngineKernels/ObjectConnectionProcessor.cuh b/source/EngineKernels/ObjectConnectionProcessor.cuh index 69c4343c4..e1fec9756 100644 --- a/source/EngineKernels/ObjectConnectionProcessor.cuh +++ b/source/EngineKernels/ObjectConnectionProcessor.cuh @@ -435,14 +435,19 @@ ObjectConnectionProcessor::tryAddConnectionWithAbsAngle_oneWay(Object* object1, auto insertIndex = 0; auto summedAngle = 0.0f; desiredAbsAngle = Math::getNormalizedAngle(desiredAbsAngle, 0.0f); + if (desiredAbsAngle < NEAR_ZERO) { + desiredAbsAngle = 360.0f; + } for (int i = 1; i <= n; ++i) { auto const& angleFromPrevious = object1->getConnection(i).angleFromPrevious; + auto nextSummedAngle = summedAngle + angleFromPrevious; - if (desiredAbsAngle >= summedAngle - NEAR_ZERO && desiredAbsAngle < summedAngle + angleFromPrevious) { + if (desiredAbsAngle > summedAngle + NEAR_ZERO && desiredAbsAngle <= nextSummedAngle + NEAR_ZERO) { insertIndex = i; + desiredAbsAngle = min(desiredAbsAngle, nextSummedAngle); break; } - summedAngle += angleFromPrevious; + summedAngle = nextSummedAngle; } DEVICE_CHECK(insertIndex > 0); diff --git a/source/EngineKernels/TestKernels.cu b/source/EngineKernels/TestKernels.cu index 4fa95167b..4e66d3991 100644 --- a/source/EngineKernels/TestKernels.cu +++ b/source/EngineKernels/TestKernels.cu @@ -71,6 +71,35 @@ __global__ void cudaTestCreateConnection(SimulationData data, uint64_t objectId1 } } +__global__ void cudaTestCreateConnectionWithAbsAngle( + SimulationData data, + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2) +{ + DEVICE_CHECK(blockDim.x == 1 && gridDim.x == 1); + + auto& objects = data.entities.objects; + auto partition = calcSystemThreadPartition(objects.getNumEntries()); + Object* object1 = nullptr; + Object* object2 = nullptr; + for (int index = partition.startIndex; index <= partition.endIndex; index += partition.step) { + auto& object = objects.at(index); + if (object->id == objectId1) { + object1 = object; + } + if (object->id == objectId2) { + object2 = object; + } + } + + if (object1 != nullptr && object2 != nullptr) { + ObjectConnectionProcessor::tryAddConnectionWithAbsAngle(data, object1, object2, desiredDistance, desiredAbsAngle1, desiredAbsAngle2); + } +} + namespace { __device__ bool isEnergyValid(float energy) diff --git a/source/EngineKernels/TestKernels.cuh b/source/EngineKernels/TestKernels.cuh index 6196f013e..b980330e5 100644 --- a/source/EngineKernels/TestKernels.cuh +++ b/source/EngineKernels/TestKernels.cuh @@ -7,4 +7,11 @@ __global__ void cudaTestMutate(SimulationData data, uint64_t objectId); __global__ void cudaTestCreateConnection(SimulationData data, uint64_t objectId1, uint64_t objectId2); +__global__ void cudaTestCreateConnectionWithAbsAngle( + SimulationData data, + uint64_t objectId1, + uint64_t objectId2, + float desiredDistance, + float desiredAbsAngle1, + float desiredAbsAngle2); __global__ void cudaTestIsDataValid(SimulationData data, bool* result); diff --git a/source/EngineTests/ObjectConnectionTests.cpp b/source/EngineTests/ObjectConnectionTests.cpp index 794d618b2..eb7a430cd 100644 --- a/source/EngineTests/ObjectConnectionTests.cpp +++ b/source/EngineTests/ObjectConnectionTests.cpp @@ -149,3 +149,38 @@ TEST_F(ObjectConnectionTests, addThirdConnection2) EXPECT_TRUE(approxCompare(1.0f, connection3._distance)); EXPECT_TRUE(approxCompare(90.0f, connection3._angleFromPrevious)); } + +TEST_F(ObjectConnectionTests, addConnectionWithZeroAbsAngleInsertsBeforeReferenceConnection) +{ + auto data = Desc().objects({ + ObjectDesc().id(1).pos({0, 0}).type(SolidDesc()), + ObjectDesc().id(2).pos({1, 0}).type(SolidDesc()), + ObjectDesc().id(3).pos({0, 1}).type(SolidDesc()), + ObjectDesc().id(4).pos({-1, 0}).type(SolidDesc()), + }); + data.addConnection(1, 2); + data.addConnection(1, 3); + _simulationFacade->setSimulationData(data); + _simulationFacade->testOnly_createConnectionWithAbsAngle(1, 4, 1.0f, 0.0f, 0.0f); + + auto actualData = _simulationFacade->getSimulationData(); + ASSERT_EQ(4, actualData._objects.size()); + + auto object = actualData.getObjectRef(1); + ASSERT_EQ(3, object._connections.size()); + + auto connection1 = object._connections.at(0); + EXPECT_EQ(2, connection1._objectId); + EXPECT_TRUE(approxCompare(1.0f, connection1._distance)); + EXPECT_TRUE(approxCompare(0.0f, connection1._angleFromPrevious)); + + auto connection2 = object._connections.at(1); + EXPECT_EQ(3, connection2._objectId); + EXPECT_TRUE(approxCompare(1.0f, connection2._distance)); + EXPECT_TRUE(approxCompare(90.0f, connection2._angleFromPrevious)); + + auto connection3 = object._connections.at(2); + EXPECT_EQ(4, connection3._objectId); + EXPECT_TRUE(approxCompare(1.0f, connection3._distance)); + EXPECT_TRUE(approxCompare(270.0f, connection3._angleFromPrevious)); +}