Fix zero-angle connection insertion

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);

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);

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();

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);

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();

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;

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);

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:

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;

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);
 

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)

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);

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));
+}