private vector L(node p, node q, vector D_p)
{
vector q_p = new vector(q.x - p.x, q.y - p.y);
if (D_p * q_p >= 0)
return q_p;
return new vector(-q.x + p.x, -q.y + p.y);
}
/// <summary>
/// стоимость ребра от q до r
/// </summary>
/// <param name="p">из вершины p</param>
/// <param name="q">в соседнюю вершину q</param>
/// <returns></returns>
double l(node p, node q)
{
int x = p.x;
int y = p.y;
int x1 = q.x;
int y1 = q.y;
byte f_z = Lapl[x,y];//Лапласиан
double f_g = 1 - Math.Sqrt(Math.Pow(Gx[x,y],2)+Math.Pow(Gy[x,y],2)) / max_G;//величина градиента
if (Math.Abs(x - q.x) + Math.Abs(y - q.y) == 1)//не диагональный элемент
f_g *= 1.0 / Math.Sqrt(2);
vector D_p = new vector(Gy[x, y], -Gx[x, y]);
vector D_q = new vector(Gy[x1,y1], -Gx[x1,y1]);
vector L_p_q = L(p, q, D_p);
double d_p = D_p * L_p_q;
double d_q = L_p_q * D_q;
if (d_p != 0)
d_p = d_p/(D_p.lenght()*L_p_q.lenght());
if (d_q != 0)
d_q = d_q / (D_q.lenght() * L_p_q.lenght());
if (d_q > 1 || d_p > 1)
MessageBox.Show("1");//мало ли
double f_d = (2 / (3 * Math.PI)) * (Math.Acos(d_p) + Math.Acos(d_q));
/////////////////////////////
const double w_z=0.43;
const double w_g = 0.43;
const double w_d = 0.14;
////////////////////////////
return w_z*f_z+w_g*f_g+w_d*f_d;
}
