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