bool Snake::move(Bonus & mBonus)
{
timeSinceLastMove += clock.restart();
while (timeSinceLastMove > timePerMove)
{
timeSinceLastMove -= timePerMove;
sf::Vector2i new_pos = sf::Vector2i(head_pos.x + direction.x, head_pos.y + direction.y);
if (wentOutOfTheMap())
{
return false;
}
if (mBonus.getPos() == new_pos)
{
eat(head_pos, mBonus);
}
else
{
moveBody(head_pos);
for (int i = 0; i<segments.size(); i++)
{
if (new_pos == segments[i])
return false;
}
}
head_pos = new_pos;
}
return true;
}
// move detection
void move(int direction){
switch(direction)
{
case 1:
moveBody(snake->head, snake->head->points->x+1, snake->head->points->y, snake->head->points->z);
break;
case 2:
moveBody(snake->head, snake->head->points->x-1, snake->head->points->y, snake->head->points->z);
break;
case 3:
moveBody(snake->head, snake->head->points->x, snake->head->points->y+1, snake->head->points->z);
break;
case 4:
moveBody(snake->head, snake->head->points->x, snake->head->points->y-1, snake->head->points->z);
break;
}
}
void UserController::choice_movement(std::string room)
{
int task_right = 0;
int task_left = 0;
int task_head = 0;
int task_body = 0;
if (room == "lobby")
{
task_left = 0;
task_right = 1;
task_head = 2;
task_body = 0;
}
else if (room == "kitchen")
{
task_left = 2;
task_right = 0;
task_head = 3;
task_body = 0;
}
else if (room == "living room")
{
task_left = 1;
task_right = 0;
task_head = 4;
task_body = 0;
}
else if (room == "bed room")
{
task_left = 0;
task_right = 2;
task_head = 1;
task_body = 0;
}
else if (room == "finish")
{
task_left = 0;
task_right = 0;
task_head = 0;
task_body = 1;
}
else
{
task_left = 0;
task_right = 0;
task_head = 0;
task_body = 0;
}
moveLeftArm(task_left);
moveRightArm(task_right);
moveHead(task_head);
moveBody(task_body);
}
// move snake body
void moveBody(Body* value, float x, float y, float z){
if(value !=NULL)
{
float tmpx = value->points->x;
float tmpy = value->points->y;
float tmpz = value->points->z;
value->points->x = x;
value->points->y = y;
value->points->z = z;
moveBody(value->next, tmpx, tmpy, tmpz);
}
}
/**
* Steps the world.
*
* This function is not complete, may need some rework.
*/
void DYN_stepWorld(DYN_Context *context)
{
int i, j;
double remainingTime = 1;
double nearestPoints[6];
int stuckCtr = 0;
if (simStopped) return;
while (remainingTime > 0)
{
char wasCollision = 0;
// STEP 1: Moving bodies
for (i = 0; i < context->bodyCount; i++)
{
moveBody(&context->bodies[i], remainingTime);
}
// STEP 2: Collision detection
for (i = 0; i < context->bodyCount; i++)
{
context->bodies[i].colliding = 0;
context->bodies[i].inContact = 0;
}
for (i = 0; i < context->bodyCount; i++)
{
for (j = i + 1; j < context->bodyCount; j++)
{
double nearestPoints[6];
int tmp[2];
tmp[0] = i;
tmp[1] = j;
if (AVL_find(&context->ncPairIndex, tmp))
{
continue;
}
if (COL_collide(&context->bodies[i], &context->bodies[j], nearestPoints, 0))
{
addCollidingPair(context, i, j);
wasCollision = 1;
}
}
}
if (wasCollision)
{
double currentImpulseVector[3];
double currentCollisionPoint[3];
double minTime = 1;
int earlyAIndex = -1, earlyBIndex = -1;
// There was collision
// STEP 3: Find earliest collision
int i;
int cpCount = DYNA_getLength(&context->collidingPairs);
int (*collidingPairs)[2] = DYNA_getStorage(&context->collidingPairs);
// Find the earliest collision.
assert(cpCount);
for (i = 0; i < cpCount; i++)
{
DYN_Body *a = &context->bodies[collidingPairs[i][0]];
DYN_Body *b = &context->bodies[collidingPairs[i][1]];
double lower = 0;
double upper = remainingTime;
int stuckCtr2 = 0;
char nearestSet = 0;
for(;;stuckCtr2++)
{
double middle = (lower + upper) * 0.5;
revertMovement(a);
revertMovement(b);
moveBody(a, middle);
moveBody(b, middle);
if (COL_collide(a, b, nearestPoints, 0))
{
upper = middle;
}
else
{
nearestSet = 1;
lower = middle;
}
assert(lower != upper);
if (nearestSet)
{
double tmp[3];
ALG_getPointToPointVector(tmp, nearestPoints, &nearestPoints[3]);
if ((ALG_dotProduct(tmp, tmp) < 1e-6) || (fabs(upper-lower) < 1e-6))
{
// The two bodies are near each other.
if (middle <= minTime)
{
minTime = middle;
earlyAIndex = collidingPairs[i][0];
earlyBIndex = collidingPairs[i][1];
memcpy(currentImpulseVector, tmp, sizeof(currentImpulseVector));
memcpy(currentCollisionPoint, nearestPoints, sizeof(currentCollisionPoint));
}
break;
}
}
}
}
{
DYN_Body *a = &context->bodies[earlyAIndex];
DYN_Body *b = &context->bodies[earlyBIndex];
fprintf(DYN_log, "Collision!\n");
fprintf(DYN_log, "Body index A: %d\n", earlyAIndex);
fprintf(DYN_log, "Body index B: %d\n", earlyBIndex);
fprintf(DYN_log, "Velocity A [%g, %g, %g] %g\n", a->velocity[0], a->velocity[1], a->velocity[2], ALG_getVectorLength(a->velocity));
fprintf(DYN_log, "Velocity B [%g, %g, %g] %g\n", b->velocity[0], b->velocity[1], b->velocity[2], ALG_getVectorLength(b->velocity));
fprintf(DYN_log, "Collision point [%g, %g, %g]\n", currentCollisionPoint[0], currentCollisionPoint[1], currentCollisionPoint[2]);
fprintf(
DYN_log,
"Impulse vector [%g, %g, %g] %g\n",
currentImpulseVector[0],
currentImpulseVector[1],
currentImpulseVector[2],
ALG_getVectorLength(currentImpulseVector)
);
fprintf(DYN_log, "Elapsed time: %g\n", context->elapsedTime + context->timeStep * (1 - remainingTime));
}
// Revert and move all bodies to the collision moment.
for (i = 0; i < context->bodyCount; i++)
{
DYN_Body *current = &context->bodies[i];
revertMovement(current);
moveBody(current, minTime);
}
assert(earlyAIndex >= 0);
assert(earlyBIndex >= 0);
// Resolve the collision
{
CollisionResult result = resolveCollision(
context,
&context->bodies[earlyAIndex],
&context->bodies[earlyBIndex],
currentCollisionPoint,
currentImpulseVector
);
if (result == DYN_STICKEDTOGETHER)
{
addNonCollidingPair(context, earlyAIndex, earlyBIndex);
context->bodies[earlyAIndex].inContact = 1;
context->bodies[earlyBIndex].inContact = 1;
}
}
memcpy(DYN_lastCollisionPoint, currentCollisionPoint, sizeof(DYN_lastCollisionPoint));
memcpy(DYN_lastImpulse, currentImpulseVector, sizeof(DYN_lastImpulse));
fprintf(DYN_log, "==============================\n");
//
clearCollidingPairs(context);
// Continue the simulation
remainingTime -= minTime;
}
else
{
// No collision, step finished
break;
}
stuckCtr++;
if (stuckCtr >= 100)
{
simStopped = 1;
return;
}
}
// STEP 4: repelling non-colliding pairs
{
int i;
int n = DYNA_getLength(&context->nonCollidingPairs);
int (*ncPairs)[2] = (int (*)[2])DYNA_getStorage(&context->nonCollidingPairs);
double lastSimplex[12];
for (i = 0; i < n; i++)
{
DYN_Body *a = &context->bodies[ncPairs[i][0]];
DYN_Body *b = &context->bodies[ncPairs[i][1]];
if (COL_collide(a, b, 0, lastSimplex))
{
double penetrationVector[3];
fprintf(DYN_log, "Sticked and colliding after timestep!\n");
COL_getPenetrationVector(a, b, lastSimplex, penetrationVector);
fprintf(
DYN_log,
"Penetration vector was: [%g, %g, %g]\n",
penetrationVector[0],
penetrationVector[1],
penetrationVector[2]
);
{
// push it out from the body and a little.
double penetrationVectorLength = ALG_getVectorLength(penetrationVector);
double scaleFactor = (penetrationVectorLength + 0.01) / penetrationVectorLength;
ALG_scale(penetrationVector, scaleFactor);
}
ALG_translate(b->position, penetrationVector);
fprintf(DYN_log, "==============================\n");
a->colliding = a->inContact = 0;
b->colliding = a->inContact = 0;
{
// Body b may collide with other bodies. Translate those bodies as well.
DYNA_Array tmp;
int j;
DYNA_initialize(&tmp, sizeof(DYN_Body*));
DYNA_add(&tmp, b);
for (j = 0; j < DYNA_getLength(&tmp); j++)
{
DYN_Body **storage = DYNA_getStorage(&tmp);
DYN_Body *current = storage[j];
int k = 0;
for (k = 0; k < context->bodyCount; k++)
{
DYN_Body *other = &context->bodies[k];
if (other == current) continue;
if (COL_collide(current, other, 0, 0))
{
ALG_translate(other->position, penetrationVector);
current->colliding = 0;
other->colliding = 0;
DYNA_add(&tmp, other);
}
}
}
DYNA_deinitialize(&tmp);
}
}
}
}
clearNonCollidingPairs(context);
context->elapsedTime += context->timeStep;
}
