int16 Map::checkDirectPath(Mult::Mult_Object *obj, int16 x0, int16 y0, int16 x1, int16 y1) {
while (1) {
Direction dir = getDirection(x0, y0, x1, y1);
if (obj) {
// Check for a blocking waypoint
if (obj->nearestWayPoint < obj->nearestDest)
if ((obj->nearestWayPoint + 1) < _wayPointCount)
if (_wayPoints[obj->nearestWayPoint + 1].notWalkable == 1)
return 3;
if (obj->nearestWayPoint > obj->nearestDest)
if (obj->nearestWayPoint > 0)
if (_wayPoints[obj->nearestWayPoint - 1].notWalkable == 1)
return 3;
}
if ((x0 == x1) && (y0 == y1))
// Successfully reached the destination
return 1;
if (dir == kDirNone)
// No way
return 3;
moveDirection(dir, x0, y0);
}
}
bool Game::startGame(int size){
this->length= size;
newBoard(this->length);
bool gameEnd = false;
bool victory = false;
do
{
insertNewNumber();
if (checkIfGameOver() == true){
break;
}
string oldNumber = uniqNumber();
string message = "Press up, down, left, right or q for quit";
while (1){
PrintBoard();
string d = input(message);
if (d == "quit"){
gameEnd=true;
break;
}
moveDirection(d); //left worked
if (oldNumber != uniqNumber()){
break;
}
message = "You can't go there, try another button or q for quit";
}
if (victory==false){
victory = checkIfVictorius();
}
}
while(gameEnd==false);
PrintBoard();
return victory;
}
void NPC::oof()
{
// Oh noes!!! We hit something!
// Abruptly change course
direction = (direction + 1) % 4;
moveDirection();
}
/*
@brief 設定した座標に向かって移動
*/
void MyShipInterface::movePosition( const CCPoint& in_rPos )
{
MyShipObj* pObj = (MyShipObj*)getChildByTag( eTAG_CHILD_OBJ );
CCPoint pos = pObj->getPosition();
CCPoint vec = in_rPos - pos;
moveDirection( vec.normalize() );
}
void atIntersection(int x, int y, int d, char ** maze, int w, int h){
int i;
int a;
int b;
for(i = NORTH; i <= WEST; i++)//iterate through all directions
{
if (findOpposite(i) != d)//ensure that I don't go back in the direction I came from
{
a = x;
b = y;
appendLocation(&a, &b, i);
if (maze[a][b] != 'X')//the direction doesn't lead into a wall
{
moveDirection(x, y, i, maze, w, h);//move in all directions except the the direction I came from
}
}
}
}
void NPC::push()
{
moveTimer.stop(); // Stop the clock!
// If we're just standing here...
if (direction == -1) {
// Move somewhere for a bit
duration = randomDouble(0.5, 4.0);
direction = randomInt(0, 3);
} else { // If we're on the move...
// Take a breather
duration = randomDouble(2.0, 6.0);
direction = -1;
}
moveDirection(); // Set the moving* bools
moveTimer.start(int(duration * 1000.0)); // Start the clock!
}
int16 Map::checkLongPath(int16 x0, int16 y0, int16 x1, int16 y1, int16 i0, int16 i1) {
int16 curX = x0;
int16 curY = y0;
int16 nextLink = 1;
while (1) {
if ((x0 == curX) && (y0 == curY))
nextLink = 1;
if (nextLink != 0) {
if (checkDirectPath(0, x0, y0, x1, y1) == 1)
return 1;
nextLink = 0;
if (i0 > i1) {
curX = _wayPoints[i0].x;
curY = _wayPoints[i0].y;
i0--;
} else if (i0 < i1) {
curX = _wayPoints[i0].x;
curY = _wayPoints[i0].y;
i0++;
} else if (i0 == i1) {
curX = _wayPoints[i0].x;
curY = _wayPoints[i0].y;
}
}
if ((i0 == i1) && (_wayPoints[i0].x == x0) &&
(_wayPoints[i0].y == y0)) {
if (checkDirectPath(0, x0, y0, x1, y1) == 1)
return 1;
return 0;
}
Direction dir = getDirection(x0, y0, curX, curY);
if (dir == kDirNone)
// No way
return 0;
moveDirection(dir, x0, y0);
}
}
void Core::loopBegin(const IGui *gui, Food food, t_key key)
{
int score;
score = 0;
if (gui)
{
displayBegin(gui, food);
while ((key != ESCAPE_KEY) &&
(collisionWithWall(_pos[0].getPosX(), _pos[0].getPosY())) &&
(collisionWithSnake(_pos[0].getPosX(), _pos[0].getPosY())) &&
(posFood(food)))
{
if (getFood())
{
food.putFood(_pos, _width, _height);
gui->displayFood(food.getPosX(), food.getPosY());
score += 100;
}
if (!getFood())
{
gui->displayAfterFood(_pos[_pos.size() - 1].getPosX(), _pos[_pos.size() - 1].getPosY());
_pos.pop_back();
}
key = gui->manageKey();
if (key == ESCAPE_KEY)
std::cout << "You're a poor loser, you leave the game !" << std::endl;
if (key == SPEEDP_KEY || key == SPEEDL_KEY)
setSpeed(key);
else
setDirectionSnake(key);
moveDirection();
gui->displayBody(_pos[0].getPosX(), _pos[0].getPosY());
gui->displayScore(score);
if (_speed <= 0)
_speed += -(_speed);
usleep(_speed);
}
}
}
// This is the superdrive task. If you can think of any better names, please tell me :P
// This task incorporates two modes, regular mecanum driving and free-spinning mode
task superDrive(){
float x1,y1,x2,y2,LF,RF,LB,RB= 0;
int minJoy = 12;
float turning;
float mag; // magnitude of the joystick vector
float initialHeading = radheading; float calcHeading = radheading; // sets a base heading for the
float movementAmount, turningAmount, totalAmount; // for apportioning power to turning and moving
while(true){
// Get joystick values
x1 = joystick.joy1_x1 * .5;y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;y2 = joystick.joy1_y2 * .5;
// function for making new initial heading
if (joy1Btn(5)==1){
initialHeading = radheading;
}
// starts free-spinning mode
if (joy1Btn(6)==1){
// find joystick vector angle
joyAngle = atan2(y1, x1);
// find joystick vector magnitude
mag = sqrt(x1*x1+y1*y1)/2;
if (mag>64)
mag = 64;
// get calculated heading
calcHeading = radheading - initialHeading;
// find the direction needed to move
moveDirection(joyAngle + calcHeading, mag);
// fix movement drifting
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy/*&&abs(joystick.joy1_x2)<minJoy*/){
FLset=0;FRset=0;
}
// find turning magnitude
turning = x2/2;
//fix turning drifting
if (abs(joystick.joy1_x2)<minJoy){
turning = 0;
}
// apportion motor capacity to movement and turning
// TODO make this better... although that will be difficult
totalAmount = 1 + turning + mag*3;
movementAmount = (mag*3)/totalAmount;
turningAmount = turning/totalAmount;
// Apply finished values to motors
motor[FL] = FLset*movementAmount+turning*turningAmount;
motor[FR] = FRset*movementAmount-turning*turningAmount;
motor[BL] = FRset*movementAmount+turning*turningAmount;
motor[BR] = FLset*movementAmount-turning*turningAmount;
} else {
// Resets movement values
LF = 0;RF = 0;LB = 0;RB = 0;
// Get joystick values
x1 = joystick.joy1_x1 * .5;y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;y2 = joystick.joy1_y2 * .5;
// Handle Strafing Movement
LF += x1;RF -= x1;LB -= x1;RB += x1;
// Handle Regular Movement
LF += y1;RF += y1;LB += y1;RB += y1;
// Handle Turning Movement
LF += x2;RF -= x2;LB += x2;RB -= x2;
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy&&abs(joystick.joy1_x2)<minJoy){
LF = 0;RF = 0;LB = 0;RB = 0;
}
// Apply Finished values to motors.
motor[FL] = LF;
motor[FR] = RF;
motor[BL] = LB;
motor[BR] = RB;
}
wait10Msec(1);
}
}
void ISOP2P1::updateSolution()
{
/// 更新插值点.
fem_space_p.updateDofInterpPoint();
fem_space_v.updateDofInterpPoint();
/// 备份数值解.
FEMFunction<double, DIM> _u_h(v_h[0]);
FEMFunction<double, DIM> _v_h(v_h[1]);
FEMFunction<double, DIM> _p_h(p_h);
const double& msl = moveStepLength();
/// 因为有限元空间插值点变化, 重新构造矩阵.
buildMatrixStruct();
buildMatrix();
/// 因为网格移动量为小量, 因此时间步长可以相对取的大些.
double _dt = 0.1;
int n_dof_v = fem_space_v.n_dof();
int n_dof_p = fem_space_p.n_dof();
int n_total_dof = DIM * n_dof_v + n_dof_p;
int n_total_dof_v = DIM * n_dof_v;
/// 一步Euler.
for (int m = 1; m > 0; --m)
{
/// 系数矩阵直接使用 Stokes 矩阵结构.
SparseMatrix<double> mat_moving;
mat_moving.reinit(sp_stokes);
/// (0, 0)
for (int i = 0; i < sp_vxvx.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_vxvx[i]) = mat_v_mass.global_entry(i);
/// (1, 1) 这两个对角块仅有质量块.
for (int i = 0; i < sp_vyvy.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_vyvy[i]) = mat_v_mass.global_entry(i);
/// (0, 2) 这个不是方阵. 在矩阵结构定义的时候已经直接排除了对角元优
/// 先.
for (int i = 0; i < sp_pvx.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_pvx[i]) = (1.0 / m) * mat_pvx_divT.global_entry(i);
/// (1, 2)
for (int i = 0; i < sp_pvy.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_pvy[i]) = (1.0 / m) * mat_pvy_divT.global_entry(i);
/// (2, 0)
for (int i = 0; i < sp_vxp.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_vxp[i]) = (1.0 / m) * mat_vxp_div.global_entry(i);
/// (2, 1) 这四块直接复制散度矩阵.
for (int i = 0; i < sp_vyp.n_nonzero_elements(); ++i)
mat_moving.global_entry(index_vyp[i]) = (1.0 / m) * mat_vyp_div.global_entry(i);
/// 问题的右端项.
Vector<double> rhs_loc(n_total_dof);
// rhs.reinit(n_total_dof);
FEMSpace<double, DIM>::ElementIterator the_element_v = fem_space_v.beginElement();
FEMSpace<double, DIM>::ElementIterator end_element_v = fem_space_v.endElement();
/// 遍历速度单元, 拼装相关系数矩阵和右端项.
for (the_element_v = fem_space_v.beginElement();
the_element_v != end_element_v; ++the_element_v)
{
/// 当前单元信息.
double volume = the_element_v->templateElement().volume();
/// 积分精度至少为2.
const QuadratureInfo<DIM>& quad_info = the_element_v->findQuadratureInfo(4);
std::vector<double> jacobian = the_element_v->local_to_global_jacobian(quad_info.quadraturePoint());
int n_quadrature_point = quad_info.n_quadraturePoint();
std::vector<Point<DIM> > q_point = the_element_v->local_to_global(quad_info.quadraturePoint());
/// 速度单元信息.
std::vector<std::vector<double> > basis_value_v = the_element_v->basis_function_value(q_point);
std::vector<double> vx_value = _u_h.value(q_point, *the_element_v);
std::vector<double> vy_value = _v_h.value(q_point, *the_element_v);
std::vector<std::vector<double> > vx_gradient = v_h[0].gradient(q_point, *the_element_v);
std::vector<std::vector<double> > vy_gradient = v_h[1].gradient(q_point, *the_element_v);
const std::vector<int>& element_dof_v = the_element_v->dof();
int n_element_dof_v = the_element_v->n_dof();
/// 速度积分点上的移动方向, 注意是速度单元的积分点, 在压力单元上的移动方向.
/// 所以下面一行程序, 仔细算过, 没有问题.
std::vector<std::vector<double> > move_vector = moveDirection(q_point, index_v2p[the_element_v->index()]);
for (int l = 0; l < n_quadrature_point; ++l)
{
double Jxw = quad_info.weight(l) * jacobian[l] * volume;
for (int i = 0; i < n_element_dof_v; ++i)
{
double rhs_cont = (vx_value[l] + (1.0 / m) * msl * innerProduct(move_vector[l], vx_gradient[l])) * basis_value_v[i][l];
rhs_cont *= Jxw;
rhs_loc(element_dof_v[i]) += rhs_cont;
rhs_cont = (vy_value[l] + (1.0 / m) * msl * innerProduct(move_vector[l], vy_gradient[l])) * basis_value_v[i][l];
rhs_cont *= Jxw;
rhs_loc(n_dof_v + element_dof_v[i]) += rhs_cont;
}
}
}
/// 构建系数矩阵和右端项.
Vector<double> x(n_total_dof);
// PoiseuilleVx real_vx (-1.0, 1.0);
// PoiseuilleVy real_vy;
// Operator::L2Project(real_vx, v_h[0], Operator::LOCAL_LEAST_SQUARE, 3);
// Operator::L2Project(real_vy, v_h[1], Operator::LOCAL_LEAST_SQUARE, 3);
// /// 边界处理.
const std::size_t * rowstart = sp_stokes.get_rowstart_indices();
const unsigned int * colnum = sp_stokes.get_column_numbers();
/// 遍历全部维度的速度节点.
for (unsigned int i = 0; i < n_total_dof_v; ++i)
{
/// 边界标志.
int bm = -1;
/// 判断一下是 x 方向还是 y 方向. 分别读取标志.
if (i < n_dof_v)
bm = fem_space_v.dofInfo(i).boundary_mark;
else
bm = fem_space_v.dofInfo(i - n_dof_v).boundary_mark;
if (bm == 0)
continue;
/// 方腔流边界条件.
if (bm == 1 || bm == 2 || bm == 4 || bm == 5)
x(i) = 0.0;
if (bm == 3)
if (i < n_dof_v)
{
/// 不包括顶端的两个端点,称为watertight cavity.
// x(i) = scale * 1.0;
Regularized regularize;
x(i) = scale * regularize.value(fem_space_v.dofInfo(i).interp_point);
}
else
x(i) = 0.0;
// /// poiseuille flow边界条件.
// if (bm == 1 || bm == 2 || bm == 4 || bm == 5)
// if (i < n_dof_v)
// x(i) = scale * real_vx.value(fem_space_v.dofInfo(i).interp_point);
// else
// x(i) = scale * real_vy.value(fem_space_v.dofInfo(i - n_dof_v).interp_point);
/// 右端项这样改, 如果该行和列其余元素均为零, 则在迭代中确
/// 保该数值解和边界一致.
if (bm == 1 || bm == 2 || bm == 3 || bm == 4 || bm == 5)
// if (bm == 1 || bm == 2 || bm == 4 || bm == 5)
{
rhs_loc(i) = mat_moving.diag_element(i) * x(i);
/// 遍历 i 行.
for (unsigned int j = rowstart[i] + 1; j < rowstart[i + 1]; ++j)
{
/// 第 j 个元素消成零(不是第 j 列!). 注意避开了对角元.
mat_moving.global_entry(j) -= mat_moving.global_entry(j);
/// 第 j 个元素是第 k 列.
unsigned int k = colnum[j];
/// 看看第 k 行的 i 列是否为零元.
const unsigned int *p = std::find(&colnum[rowstart[k] + 1],
&colnum[rowstart[k + 1]],
i);
/// 如果是非零元. 则需要将这一项移动到右端项. 因为第 i 个未知量已知.
if (p != &colnum[rowstart[k + 1]])
{
/// 计算 k 行 i 列的存储位置.
unsigned int l = p - &colnum[rowstart[0]];
/// 移动到右端项. 等价于 r(k) = r(k) - x(i) * A(k, i).
rhs_loc(k) -= mat_moving.global_entry(l) * x(i);
/// 移完此项自然是零.
mat_moving.global_entry(l) -= mat_moving.global_entry(l);
}
}
}
}
std::cout << "boundary values for updateSolution OK!" << std::endl;
// /// debug 边界条件处理部分,已经测试过, 边界处理正确.
// RegularMesh<DIM> &mesh_v = irregular_mesh_v->regularMesh();
// for (int i = 0; i < mesh_v.n_geometry(0); ++i)
// {
// Point<DIM> &p= mesh_v.point(i);
// if (fabs(1 - p[1]) < eps || fabs(1 - p[0]) < eps || fabs(-1 - p[1]) < eps || fabs(-1 - p[0]) < eps)
// {
// std::cout << "point(" << i << ") = (" << p[0] << "," << p[1] << ");" << std::endl;
// std::cout << "uh(" << i << ") = " << v_h[0](i) << std::endl;
// std::cout << "vh(" << i << ") = " << v_h[1](i) << std::endl;
// }
// }
// getchar();
// /// debug
clock_t t_cost = clock();
/// 预处理矩阵.
/// 不完全LU分解.
dealii::SparseILU <double> preconditioner;
preconditioner.initialize(mat_moving);
/// 矩阵求解.
dealii::SolverControl solver_control (4000000, l_tol, check);
SolverMinRes<Vector<double> > minres (solver_control);
minres.solve (mat_moving, x, rhs_loc, PreconditionIdentity());
t_cost = clock() - t_cost;
std::cout << "time cost: " << (((float)t_cost) / CLOCKS_PER_SEC) << std::endl;
for (int i = 0; i < n_dof_v; ++i)
{
v_h[0](i) = x(i);
v_h[1](i) = x(i + n_dof_v);
}
for (int i = 0; i < n_dof_p; ++i)
p_h(i) = x(i + 2 * n_dof_v);
/// debug
std::ofstream mat_deb;
// rowstart = sp_pvx.get_rowstart_indices();
// colnum = sp_pvx.get_column_numbers();
mat_deb.open("mat.m", std::ofstream::out);
mat_deb.setf(std::ios::fixed);
mat_deb.precision(20);
for (int i = 0; i < n_total_dof; ++i)
{
for (int j = rowstart[i]; j < rowstart[i + 1]; ++j)
{
mat_deb << "A(" << i + 1 << ", " << colnum[j] + 1 << ")="
<< mat_moving.global_entry(j) << ";" << std::endl;
}
mat_deb << "x(" << i + 1<< ") = " << x(i) << ";" << std::endl;
mat_deb << "rhs(" << i + 1 << ") = " << rhs_loc(i) << ";" << std::endl;
}
// for(int i = 0; i < n_dof_p; ++i)
// mat_deb << "x(" << i + 1<< ") = " << p_h(i) << ";" << std::endl;
mat_deb.close();
std::cout << "mat output" << std::endl;
Vector<double> res(n_total_dof);
mat_moving.vmult(res, x);
res *= -1;
res += rhs_loc;
std::cout << "res_l2norm =" << res.l2_norm() << std::endl;
// double error;
// error = Functional::L2Error(v_h[0], real_vx, 3);
// std::cout << "|| u - u_h ||_L2 = " << error << std::endl;
// error = Functional::H1SemiError(v_h[0], real_vx, 3);
// std::cout << "|| u - u_h ||_H1 = " << error << std::endl;
/// debug
RegularMesh<DIM> &mesh_p = irregular_mesh_p->regularMesh();
for (int i = 0; i < mesh_p.n_geometry(0); ++i)
{
(*mesh_p.h_geometry<0>(i))[0] += moveDirection(i)[0];
(*mesh_p.h_geometry<0>(i))[1] += moveDirection(i)[1];
}
/// 输出一下.
outputTecplotP("NS_Euler");
getchar();
}
};
// This is the superdrive task. If you can think of any better names, please tell me :P
// This task incorporates two modes, regular mecanum driving and free-spinning mode
task superDrive(){
while(true)
{
if(isSuper)
{
float x1,y1,x2,y2,LF,RF,LB,RB = 0;
int minJoy = 12;
float turning;
float mag; // magnitude of the joystick vector
float initialHeading = radheading;
float calcHeading = radheading; // sets a base heading for the
float movementAmount, turningAmount, totalAmount; // for apportioning power to turning and moving
while(isSuper) { // while true
// Get joystick values
x1 = joystick.joy1_x1 * .5;
y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;
y2 = joystick.joy1_y2 * .5;
// function for making new initial heading
if (joy1Btn(5) == 1){
initialHeading = radheading;
}
// starts free-spinning mode
if (isSuper) {
// find joystick vector angle
joyAngle = atan2(y1, x1);
// find joystick vector magnitude
mag = sqrt(x1*x1+y1*y1)/2;
if (mag>64)
mag = 64;
// get calculated heading
calcHeading = radheading - initialHeading;
// find the direction needed to move
moveDirection(joyAngle + calcHeading, mag);
// fix movement drifting
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy&&abs(joystick.joy1_x2)<minJoy){
FLset=0;FRset=0;
}
// find turning magnitude
turning = x2 / 2;
//fix turning drifting
if (abs(joystick.joy1_x2) < minJoy) {
turning = 0;
}
// apportion motor capacity to movement and turning
// TODO make this better... although that will be difficult
totalAmount = 1 + turning + mag*3;
movementAmount = (mag*3)/totalAmount;
turningAmount = turning/totalAmount;
// Apply finished values to motors
motor[FL] = 2*FLset*movementAmount+2*turning*turningAmount;
motor[FR] = 2*FRset*movementAmount-2*turning*turningAmount;
motor[BL] = 2*FRset*movementAmount+2*turning*turningAmount;
motor[BR] = 2*FLset*movementAmount-2*turning*turningAmount;
nxtDisplayCenteredTextLine(4, "else block");
} else {
// Resets movement values
LF = 0;
RF = 0;
LB = 0;
RB = 0;
// Get joystick values
x1 = joystick.joy1_x1 * .5;
y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;
y2 = joystick.joy1_y2 * .5;
// Handle Strafing Movement
LF += x1;
RF -= x1;
LB -= x1;
RB += x1;
// Handle Regular Movement
LF += y1;
RF += y1;
LB += y1;
RB += y1;
// Handle Turning Movement
LF += x2;
RF -= x2;
LB += x2;
RB -= x2;
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy&&abs(joystick.joy1_x2)<minJoy){
LF = 0;
RF = 0;
LB = 0;
RB = 0;
}
// Apply Finished values to motors.
nxtDisplayCenteredTextLine(4, "else block");
motor[FL] = LF; // initially no shift
motor[FR] = RF;
motor[BL] = LB;
motor[BR] = RB;
}
wait1Msec(10);
}
}
else
{
// mecanum driving
// 242 could be more precise
// think about implementing dampening and quadratic shifting
while(!isSuper)
{
motor[FL] = (scaledLeftY - scaledRightX + scaledLeftX);
motor[BL] = (scaledLeftY - scaledRightX - scaledLeftX);
motor[FR] = (scaledLeftY + scaledRightX - scaledLeftX);
motor[BR] = (scaledLeftY + scaledRightX + scaledLeftX);
// motor[frontLeft] = (joystickLeftY - joystickRightX + joystickLeftX)*100/127;
// motor[backLeft] = (joystickLeftY - joystickRightX - joystickLeftX)*100/127;
// motor[frontRight] = (joystickLeftY + joystickRightX - joystickLeftX)*100/127;
// motor[backRight] = (joystickLeftY + joystickRightX + joystickLeftX) *100/127;
wait1Msec(waitTime);
}
}
}
}
