particlePaths/robot.cpp at main · aneangel/particlePaths · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#include "robot.h"
#include <cmath>
#include <iostream>
#include <cstdlib>

void initializeRobotAndGoal(SimulationState &simState)
{
    if (!simState.initialAgitationDone)
        return;

    if (simState.robotParticle >= 0 && simState.goalParticle >= 0)
        return;

    int numParticles = (int)simState.particles.size();
    float minSeparation = boxsize * 0.5f;

    int attempts = 0;
    int maxAttempts = 100;

    while (attempts < maxAttempts)
    {
        int robotIdx = rand() % numParticles;
        int goalIdx = rand() % numParticles;

        if (robotIdx == goalIdx)
        {
            attempts++;
            continue;
        }

        if (!simState.particles[robotIdx].active || !simState.particles[goalIdx].active)
        {
            attempts++;
            continue;
        }

        Particle &r = simState.particles[robotIdx];
        Particle &g = simState.particles[goalIdx];

        float dx = r.x - g.x;
        float dy = r.y - g.y;
        float dz = r.z - g.z;
        float dist = sqrt(dx * dx + dy * dy + dz * dz);

        if (dist >= minSeparation)
        {
            simState.robotParticle = robotIdx;
            simState.goalParticle = goalIdx;
            std::cout << "Robot initialized at particle index " << robotIdx << std::endl;
            std::cout << "Goal initialized at particle index " << goalIdx << std::endl;
            std::cout << "Initial separation: " << dist << std::endl;
            return;
        }

        attempts++;
    }

    simState.robotParticle = rand() % numParticles;
    do
    {
        simState.goalParticle = rand() % numParticles;
    } while (simState.goalParticle == simState.robotParticle);

    std::cout << "Robot initialized at particle index " << simState.robotParticle << " (fallback)" << std::endl;
    std::cout << "Goal initialized at particle index " << simState.goalParticle << " (fallback)" << std::endl;
    // aiv
}

void steerTowardsTarget(Particle &robot, float targetX, float targetY, float targetZ)
{
    float dx = targetX - robot.x;
    float dy = targetY - robot.y;
    float dz = targetZ - robot.z;

    float dist = sqrt(dx * dx + dy * dy + dz * dz);
    if (dist < 0.001f)
        return;

    float ndx = dx / dist;
    float ndy = dy / dist;
    float ndz = dz / dist;

    float desiredVx = ndx * robotMaxSpeed;
    float desiredVy = ndy * robotMaxSpeed;
    float desiredVz = ndz * robotMaxSpeed;

    robot.vx += (desiredVx - robot.vx) * robotSteerForce * timeStep;
    robot.vy += (desiredVy - robot.vy) * robotSteerForce * timeStep;
    robot.vz += (desiredVz - robot.vz) * robotSteerForce * timeStep;
}

void updateRobotControl(SimulationState &simState)
{
    if (simState.robotParticle < 0 || simState.goalParticle < 0)
        return;

    if (!simState.particles[simState.robotParticle].active)
        return;

    Particle &robot = simState.particles[simState.robotParticle];
    Particle &goal = simState.particles[simState.goalParticle];

    float dx = robot.x - goal.x;
    float dy = robot.y - goal.y;
    float dz = robot.z - goal.z;
    float distToGoal = sqrt(dx * dx + dy * dy + dz * dz);

    if (distToGoal < particleRad * 4.0f)
    {
        simState.goalReached = true;
        std::cout << "Robot reached goal!" << std::endl;
        return;
    }

    if (simState.currentPath.empty())
        return;

    static int currentWaypoint = 0;
    static int lastPathCounter = -1;

    if (simState.pathUpdateCounter != lastPathCounter)
    {
        currentWaypoint = 0;
        lastPathCounter = simState.pathUpdateCounter;
    }

    if (currentWaypoint >= (int)simState.currentPath.size())
    {
        currentWaypoint = (int)simState.currentPath.size() - 1;
    }

    if (currentWaypoint < 0)
    {
        currentWaypoint = 0;
    }

    float targetX = simState.currentPath[currentWaypoint][0];
    float targetY = simState.currentPath[currentWaypoint][1];
    float targetZ = simState.currentPath[currentWaypoint][2];

    float dwx = robot.x - targetX;
    float dwy = robot.y - targetY;
    float dwz = robot.z - targetZ;
    float distToWaypoint = sqrt(dwx * dwx + dwy * dwy + dwz * dwz);

    if (distToWaypoint < waypointReachDist)
    {
        currentWaypoint++;
        if (currentWaypoint < (int)simState.currentPath.size())
        {
            targetX = simState.currentPath[currentWaypoint][0];
            targetY = simState.currentPath[currentWaypoint][1];
            targetZ = simState.currentPath[currentWaypoint][2];
        }
    }

    steerTowardsTarget(robot, targetX, targetY, targetZ);
}