void PlanningProblem::solveInvalidGoalState()
{
// TODO : make a strategy for handling this situation
// strategy is to just displace goal point temporally to a valid point
for(uint i=0; i<stat_obstacles.size(); i++) {
Obstacle* ob = stat_obstacles[i];
if(ob == NULL) continue;
if(hasCollision(goal.goal_point, *ob)) {
Vector2D diff = goal.goal_point.getPosition().to2D() - Vector2D(ob->transform.p);
float displacement_ = agent->radius() + ob->shape->m_radius - diff.lenght();
if(displacement_ > 0)
goal.goal_point.setPosition( goal.goal_point.getPosition() +
(diff.normalized() * displacement_ * 1.1).to3D());
break;
}
}
}
void PlanningProblem::solveInvalidInitialState()
{
cerr << "Warning: the initial state is drawn in an obstacle" << endl;
for(uint i=0; i < stat_obstacles.size(); i++) {
Obstacle* ob = stat_obstacles[i];
if(ob==NULL) continue;
if(hasCollision(initialState, *ob)) {
Vector2D diff_to_ob(initialState.getPosition().to2D() - Vector2D(ob->transform.p));
diff_to_ob.normalize();
Station new_station;
new_station.setPosition(initialState.getPosition() + (diff_to_ob * ob->shape->m_radius).to3D());
trajec.clear();
trajec.appendState(initialState);
trajec.appendState(new_station);
trajec.printToStream(cout);
planningResult = true;
return;
}
}
}
// this function returns -1 if two objects has collision
// And otherwise returns the distance
float PlanningProblem::distToObstacle(Station A, const Obstacle &ob, b2Vec2& A_point, b2Vec2& ob_point)
{
b2DistanceProxy state_proxy, ob_proxy;
state_proxy.Set(agent->shape, 0);
ob_proxy.Set(ob.shape, 1);
b2DistanceInput dist_in;
dist_in.proxyA = state_proxy;
dist_in.proxyB = ob_proxy;
dist_in.transformA = b2Transform(A.getPosition().toB2vec2(),
b2Rot(A.getPosition().Teta()));
dist_in.transformB = ob.transform;
b2SimplexCache dist_cache;
dist_cache.count = 0;
b2DistanceOutput dis_out;
b2Distance(&dis_out, &dist_cache, &dist_in);
A_point = dis_out.pointA;
ob_point = dis_out.pointB;
if(hasCollision(A, ob)) {
return -1;
}
return dis_out.distance;
}
bool PlanningProblem::CheckValidity(const Station &A)
{
return ( ! hasCollision(A, this->stat_obstacles) );
}
void FallDown::update( float elapsedTime )
{
speed = speed + 0.002f * elapsedTime * 60;
float offsetDiff = glm::min(125.0f, glm::pow(speed * 6, 0.6f) * 2) * elapsedTime * 60;
offset += offsetDiff;
if (offset > 128)
{
offset -= 128;
std::vector<bool> line;
for (int ii = 0; ii < levelWidth; ii++)
line.push_back(!generateEmptyLine);
if (!generateEmptyLine)
{
line[1 + rand()%(line.size() - 2)] = false;
if(blib::math::randomDouble() < 0.5)
line[1 + rand()%(line.size() - 2)] = false;
}
line[0] = true;
line[line.size() - 1] = true;
level.erase(level.begin());
level.push_back(line);
generateEmptyLine = !generateEmptyLine;
}
scrollBack -= 0.5f*offsetDiff;
while (scrollBack < 0)
scrollBack += backSprite->originalHeight;
for(auto p : players)
{
if (!p->alive)
{
p->position.y -= 3 * glm::pow(speed*6, 0.6f);
continue;
}
float fac = 8*glm::pow(speed, 0.6f);
fac = glm::sqrt(fac);
for (int ii = 0; ii < glm::ceil(fac*fac); ii++)
{
p->position.y += settings->scale * (float)fac / 4 * elapsedTime * 60;
if (p->position.y > 1080 - playerSprite->originalHeight)
p->position.y = (float)(1080 - playerSprite->originalHeight);
blib::math::Rectangle playerRect(glm::floor(p->position), playerSprite->originalWidth, playerSprite->originalHeight);
if (hasCollision(playerRect))
p->position.y = 128 * glm::round((p->position.y + offset - 64) / 128) + 64 - offset;
float oldPos = p->position.x;
p->position.x += fac * p->joystick.leftStick.x * elapsedTime * 60;
playerRect = blib::math::Rectangle(glm::floor(p->position), playerSprite->originalWidth, playerSprite->originalHeight);
if (hasCollision(playerRect))
p->position.x = 64 * glm::round(oldPos / 64);
oldPos = p->position.x;
p->position.x += fac * p->joystick.rightStick.x * 0.5f * elapsedTime * 60;
playerRect = blib::math::Rectangle(glm::floor(p->position), playerSprite->originalWidth, playerSprite->originalHeight);
if (hasCollision(playerRect))
p->position.x = 64 * glm::round(oldPos / 64);
}
if (p->position.y < 32 || p->position.x < 32 || p->position.x > 1920-32)
{
for (int ii = glm::max(0, (int)(p->position.x / 100) - 1); ii <= glm::min(29, (int)(p->position.x / 100) + 1); ii++)
touchedBlades[ii] = true;
p->alive = false;
}
}
}
void BattleCharacter::updateMovement( BattleLevel* level, const std::vector<BattleCharacter*> &players, float elapsedTime )
{
if (!alive)
return;
BattleEnemy* enemy = dynamic_cast<BattleEnemy*>(this);
if (enemy == NULL || !enemy->upsidedown)
{
if (isOnBlock(level))
{
speed.x *= 0.7f;
speed.x += 4*directionalStick.x;
speed.x = glm::clamp(speed.x, -8.0, 8.0);
}
else
{
speed.x *= 0.9f;
speed.x += 0.9f*directionalStick.x;
speed.x = glm::clamp(speed.x, -8.0, 8.0);
}
}
double beforeX = position.x;
position.x += speed.x * elapsedTime * 60;
if (position.x < -64)
position.x += (1920 + 64);
if (position.x > 1920)
position.x -= (1920 + 64);
if (hasCollision(level))
{
position.x = beforeX;
speed.x = 0;
}
for (auto pp : players)
{
if (pp == this || !pp->alive)
continue;
if (glm::abs(pp->position.x - position.x) < 64 && position.y > pp->position.y - 64 && position.y < pp->position.y + 64/*player jumped on height*/)
{//collision between p and pp;
position.x = beforeX;
//if still colliding, then one is probably jumping on the other
if (glm::abs(pp->position.x - position.x) < 64 && position.y > pp->position.y - 64 && position.y < pp->position.y + 64)
{
if (position.y < pp->position.y)
{
position.y -= speed.y*elapsedTime*60;
speed.y = -14;
position.y += speed.y*elapsedTime*60;
if (pp->isOnBlock(level))
upcount = 1;
BattlePlayer* player = dynamic_cast<BattlePlayer*>(this);
BattleEnemy* otherEnemy = dynamic_cast<BattleEnemy*>(pp);
pp->speed.y = 14;
pp->hit(this);
}
else
{
pp->position.y -= pp->speed.y*elapsedTime*60;
pp->speed.y = -14;
pp->position.y += pp->speed.y*elapsedTime*60;
if (isOnBlock(level))
pp->upcount = 1;
BattlePlayer* player = dynamic_cast<BattlePlayer*>(pp);
BattleEnemy* otherEnemy = dynamic_cast<BattleEnemy*>(this);
speed.y = 14;
hit(pp);
}
}
else
{//horizontal collision. If one of 2 is a player, he should die. If one is an upsidedown enemy, he should be shot
if (dynamic_cast<BattleEnemy*>(pp) != NULL)
{
if (dynamic_cast<BattleEnemy*>(pp)->upsidedown)
{
if (pp->speed.x == 0)
{
pp->speed.x = 10;
if (position.x > pp->position.x)
pp->speed.x = -10;
pp->position.x += pp->speed.x*elapsedTime*60;
}
else
{
hit(pp);
pp->speed.x = -pp->speed.x;
pp->position.x += pp->speed.x*elapsedTime*60;
}
}
else
{
if (dynamic_cast<BattlePlayer*>(this) != NULL)
hit(pp);
speed.x = -speed.x;
directionalStick = glm::vec2(-directionalStick.x, directionalStick.y);
}
}
else if (dynamic_cast<BattleEnemy*>(this) != NULL)
{
if (dynamic_cast<BattleEnemy*>(this)->upsidedown)
{
if (speed.x == 0)
{
speed.x = 10;
if (pp->position.x > position.x)
speed.x = -10;
position.x += pp->speed.x*elapsedTime*60;
}
else
{
pp->hit(this);
speed.x = -speed.x;
position.x += speed.x*elapsedTime*60;
}
}
else
{
if (dynamic_cast<BattlePlayer*>(pp))
pp->hit(this);
pp->speed.x = -pp->speed.x;
pp->directionalStick = glm::vec2(-pp->directionalStick.x, pp->directionalStick.y);
}
}
else
{
speed.x = 0;
pp->speed.x = 0;
pp->directionalStick = glm::vec2(-pp->directionalStick.x, pp->directionalStick.y);
}
}
}
}
if (wantsToJump)
{
printf("%f, %f\n", speed.y, upcount);
printf("On block: %s\n", isOnBlock(level) ? "yes" : "no");
}
if (wantsToJump && (isOnBlock(level) || (upcount > 0 && upcount < 14)) && !hasCollision(level))
{
speed.y = -14;
upcount+=elapsedTime*60;
}
else if (!wantsToJump)
upcount = 0;
bool onBlockBefore = hasCollision(level);
position.y += speed.y*elapsedTime*60;
if (speed.y < 0 && hasCollision(level) && !onBlockBefore) // ceiling collision
{
position.y -= speed.y*elapsedTime*60;
speed.y = 0.5f * -speed.y;
}
speed.y += 1*elapsedTime*60;
if (isOnBlock(level))
{
position.y = 64 * glm::round(position.y / 64);
speed.y = 0;
}
}
void functionKeys(int key, int x, int y){
double xtmp, ztmp, xnew, znew;
//int currObj = 0;
if (currentAction == NAVIGATE && !personBody->turnHead){
switch (key){
case GLUT_KEY_UP:
personBody->translation.x -= 0.2 * sin(degToRad(personBody->angles.y));
personBody->translation.z -= 0.2 * cos(degToRad(personBody->angles.y));
if (hasCollision(personBody, boundaryWalls, innerWalls, doors, collision_offset, numBoundaryWalls, numInnerWalls, numDoors) || (objCollision && hasObjCollision(personBody, cobj, 12)) ){
personBody->translation.x += 0.2 * sin(degToRad(personBody->angles.y));
personBody->translation.z += 0.2 * cos(degToRad(personBody->angles.y));
}
break;
case GLUT_KEY_DOWN:
personBody->translation.x += 0.2 * sin (degToRad(personBody->angles.y));
personBody->translation.z += 0.2 * cos (degToRad(personBody->angles.y));
if (hasCollision(personBody, boundaryWalls, innerWalls, doors, collision_offset, numBoundaryWalls, numInnerWalls, numDoors) || (objCollision && hasObjCollision(personBody, cobj, 12)) ){
personBody->translation.x -= 0.2 * sin(degToRad(personBody->angles.y));
personBody->translation.z -= 0.2 * cos(degToRad(personBody->angles.y));
}
break;
case GLUT_KEY_LEFT:
personBody->angles.y += 2.0;
if (hasCollision(personBody, boundaryWalls, innerWalls, doors, collision_offset, numBoundaryWalls, numInnerWalls, numDoors) || (objCollision && hasObjCollision(personBody, cobj, 12)) ){
personBody->angles.y -= 2.0;
}
break;
case GLUT_KEY_RIGHT:
personBody->angles.y -= 2.0;
if (hasCollision(personBody, boundaryWalls, innerWalls, doors, collision_offset, numBoundaryWalls, numInnerWalls, numDoors) || (objCollision && hasObjCollision(personBody, cobj, 12)) ){
personBody->angles.y += 2.0;
}
break;
}
ztmp = personBody->scaleFactor.z+0.1;
xnew = ztmp * sin (degToRad(personBody->angles.y));
znew = ztmp * cos (degToRad(personBody->angles.y));
lookFromx = personBody->translation.x;
lookFromy = personBody->scaleFactor.y + 0.7;
lookFromz = personBody->translation.z;
lookAtx = personBody->translation.x - 2*xnew;
lookAty = lookFromy;
lookAtz = personBody->translation.z - 2*znew;
}
else if (currentAction == NAVIGATE && personBody->turnHead){
double ynew, ytmp;
double xzAngle;
switch (key){
case GLUT_KEY_UP: // 't' resets head position to 0.0
personBody->headAngle.x -= 2.0;
if (personBody->headAngle.x < -45) personBody->headAngle.x = -45;
break;
case GLUT_KEY_DOWN: // 't' resets head position to 0.0
personBody->headAngle.x += 2.0;
if (personBody->headAngle.x > 60) personBody->headAngle.x = 60;
break;
case GLUT_KEY_LEFT:
personBody->headAngle.y += 2.0;
if (personBody->headAngle.y > 60) personBody->headAngle.y = 60;
break;
case GLUT_KEY_RIGHT:
personBody->headAngle.y -= 2.0;
if (personBody->headAngle.y < -60) personBody->headAngle.y = -60;
break;
}
ztmp = personBody->scaleFactor.z + 0.8;
xzAngle = degToRad(personBody->angles.y + personBody->headAngle.y);
xnew = ztmp * sin(xzAngle);
znew = ztmp * cos(xzAngle);
ytmp = 0.64; // = 1.6 * 0.4
ynew = ytmp * sin(degToRad(personBody->headAngle.x));
lookFromx = personBody->translation.x + ynew * sin(xzAngle);
lookFromy = personBody->scaleFactor.y + 0.7;
lookFromz = personBody->translation.z + ynew * cos(xzAngle);
lookAtx = personBody->translation.x - 2 * xnew;
lookAty = lookFromy + ynew;
lookAtz = personBody->translation.z - 2 * znew;
}
else if (currentAction == TRANSLATE){
switch (key){
case GLUT_KEY_UP:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.z -= trsize;
}
}
break;
case GLUT_KEY_DOWN:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.z += trsize;
}
}
break;
case GLUT_KEY_LEFT:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.x -= trsize;
}
}
break;
case GLUT_KEY_RIGHT:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.x += trsize;
}
}
break;
}
}
else if (currentAction == ROTATE){
switch (key){
case GLUT_KEY_UP:
break;
case GLUT_KEY_DOWN:
break;
case GLUT_KEY_LEFT:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->angles.y += rosize;
if (cobj[i]->angles.y > 180.0) cobj[i]->angles.y -= 360.0;
//else if (cobj[i]->angles.y < -180.0) cobj[i]->angles.y += 360.0;
}
}
break;
case GLUT_KEY_RIGHT:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->angles.y -= rosize;
//if (cobj[i]->angles.y > 180.0) cobj[i]->angles.y -= 360.0;
if (cobj[i]->angles.y < -180.0) cobj[i]->angles.y += 360.0;
}
}
break;
}
}
else if (currentAction == RAISE){
switch (key){
case GLUT_KEY_UP:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.y += trsize;
}
}
break;
case GLUT_KEY_DOWN:
for (int i = 0; i < 12; i++){
if (cobj[i]->selected){
cobj[i]->translation.y -= trsize;
}
}
break;
case GLUT_KEY_LEFT:
break;
case GLUT_KEY_RIGHT:
break;
}
}
else if (currentAction == SELECT){
int currObj = 0;
switch (key){
case GLUT_KEY_UP:
break;
case GLUT_KEY_DOWN:
break;
case GLUT_KEY_LEFT:
for (currObj = 11; currObj >= 0; currObj--){ //check if there is at least one item selected
if (cobj[currObj]->selected)
break;
}
if (currObj > 0){ //if there are more objects
cobj[currObj]->selected = false; //deselect the current one
cobj[currObj - 1]->selected = true; //select the next one
for (int j = currObj - 2; j >= 0; j--) //deselect the rest
cobj[j]->selected = false;
}
else if (currObj == 0){ //the last cube is selected
cobj[currObj]->selected = false; //deselect the current one
cobj[11]->selected = true; //wrap around and select the first cube
}
else if (currObj == -1){ //if none are selected
cobj[0]->selected = true;
}
break;
case GLUT_KEY_RIGHT:
for (currObj = 0; currObj <= 11; currObj++){ //check if there is at least one item selected
if (cobj[currObj]->selected)
break;
}
if (currObj < 11){ //if there are more objects
cobj[currObj]->selected = false; //deselect the current one
cobj[currObj + 1]->selected = true; //select the next one
for (int j = currObj + 2; j < 12; j++) //deselect the rest
cobj[j]->selected = false;
}
else if (currObj == 11){ //the last cube is selected
cobj[currObj]->selected = false; //deselect the current one
cobj[0]->selected = true; //wrap around and select the first cube
}
else if (currObj == 12){ //if none are selected
cobj[0]->selected = true;
}
break;
}
}
else if (currentAction == MULTIPLESELECT){
int currObj = 0, firstObj;
switch (key){
case GLUT_KEY_UP:
break;
case GLUT_KEY_DOWN:
break;
case GLUT_KEY_LEFT:
for (currObj = 0; currObj < 12; currObj++){ //check if there is at least one item selected
if (cobj[currObj]->selected)
break;
}
// go backward and select the next unselected object
firstObj = currObj;
do{
if (currObj == 0)
currObj = 11;
else
currObj--;
} while (cobj[currObj]->selected && currObj != firstObj);
if (currObj != firstObj)
cobj[currObj]->selected = true;
break;
case GLUT_KEY_RIGHT:
for (currObj = 11; currObj >= 0; currObj--){ //check if there is at least one item selected
if (cobj[currObj]->selected)
break;
}
// go forward and select the next unselected object
firstObj = currObj;
do{
if (currObj == 11)
currObj = 0;
else
currObj++;
} while (cobj[currObj]->selected && currObj != firstObj);
if (currObj != firstObj)
cobj[currObj]->selected = true;
break;
}
}
}