/*
* Move to specified value
* In: Joint, value
* Returns PWM value
*/
u32 moveTo(Joint joint, u32 value) {
//Variables
int diff;
u32 limit;
//Make sure value is smaller than upper limit
limit = getUpperPwmLimit(joint);
if (value > limit) {
value = limit;
}
//Make sure value is bigger than lower limit
limit = getLowerPwmLimit(joint);
if (value < limit) {
value = limit;
}
//Move
do {
diff = moveTowards(joint, value);
} while (diff != 0);
//Return
return value;
}
void CWall::update(float mFT)
{
speed.update(mFT);
curve.update(mFT);
float radius{hexagonGame.getRadius() * 0.65f};
int pointsOnCenter{0};
for(auto& vp : vertexPositions)
{
if(abs(vp.x - centerPos.x) < radius && abs(vp.y - centerPos.y) < radius) pointsOnCenter++;
else
{
moveTowards(vp, centerPos, speed.speed * 5.f * mFT);
rotateRadiansAroundCenter(vp, centerPos, curve.speed / 60.f * mFT);
}
}
if(pointsOnCenter > 3) getEntity().destroy();
}
/**
* @brief AgentCluster::move Takes care of moving an Agent to its next location.
* @param agent Agent to move.
*/
void AgentCluster::move(Agent *agent) {
std::vector<Agent*> neighbors = agentsWithinForagingRange(agent); //bestAgentInRange(agent); //best agent in range.
if (neighbors.size() != 0) { //has neighbors
Agent* bestNeighbor = neighbors[0];
for (unsigned int i = 1; i < neighbors.size(); i++) {
Agent* candidate = neighbors[i];
if (candidate->happiness > bestNeighbor->happiness)
bestNeighbor = candidate;
}
if (bestNeighbor->happiness > agent->happiness) { //found a better neighbor we should move towards
moveTowards(agent, bestNeighbor);
} else { //otherwise, just move randomly :(
moveRandomly(agent);
}
} else { //all alone...
std::vector<ClusterItem*> items = dataWithinForagingRange(agent);
if (items.size() != 0) { //alone, but with data?
//Find the average position vector and move in that direction
double avgX = 0;
double avgY = 0;
for (unsigned int i = 0; i < items.size(); i++) {
ClusterItem* item = items[i];
avgX += item->x - agent->x;
avgY += item->y - agent->y;
}
avgX /= (double)items.size();
avgY /= (double)items.size();
double magnitude = randomDouble(0.1, 1);
agent->x += avgX * magnitude;
agent->y += avgY * magnitude;
} else { //alone AND no data?
moveRandomly(agent);
}
}
}
void Entity::moveTowards(Position pos)
{
moveTowards(pos, speed);
}
void Entity::moveTowards(Direction dir)
{
moveTowards(position.getNeighbor(dir), speed);
}
void SimulatedSub::moveTowards(float targetx, float targety, float targetz) {
QVector3D targetPosition = QVector3D(targetx, targety, targetz);
logger->debug("Moving to position: " + std::to_string(targetx) + ", " + std::to_string(targety) + ", " + std::to_string(targetz));
moveTowards(targetPosition);
}
