void ai::Herd::calculateV(v2<float> &velocity, Object *sheep, const int leader, const float distance) {
bool was_stopped = velocity.is0();
velocity.clear();
std::set<const Object *> o_set;
World->enumerate_objects(o_set, sheep, distance, NULL);
int n = 0;
for(std::set<const Object *>::iterator i = o_set.begin(); i != o_set.end(); ++i) {
const Object *o = *i;
if (o->impassability == 0 || (leader && o->get_summoner() != leader))
continue;
int cd = getComfortDistance(o);
if (cd == -1)
continue;
v2<float> pos = sheep->get_relative_position(o);
float r = pos.length();
if (r < 0.001) r = 0.001;
if (pos.quick_length() < cd * cd)
velocity -= pos / r;
else
velocity += pos / r;
++n;
}
const v2<int> tile_size = Map->getPathTileSize();
v2<int> pos = sheep->get_center_position().convert<int>() / tile_size;
const Matrix<int> &hint = Map->getAreaMatrix(sheep->registered_name);
GET_CONFIG_VALUE("objects.ai.hint-gravity", float, hgc, 10.0f);
v2<int> size = v2<int>((int)distance, (int)distance * 4 / 3) / tile_size / 2;
for(int y = -size.y; y <= size.y; ++y)
for(int x = -size.x; x < size.x; ++x) {
if (hint.get(pos.y + y, pos.x + x)) {
v2<float> dpos(x, y);
//LOG_DEBUG(("%d:%s %g %g, %g", sheep->get_id(), sheep->registered_name.c_str(), dpos.x, dpos.y, dpos.length()));
float r = dpos.normalize();
velocity += dpos * hgc / r;
}
}
const Object * o = leader?World->getObjectByID(leader): NULL;
if (o != NULL && !ZBox::sameBox(o->get_z(), sheep->get_z()))
o = NULL;
if (o != NULL) {
//LOG_DEBUG(("leader: %p", o));
v2<float> pos = sheep->get_relative_position(o);
int cd = getComfortDistance(NULL);
if (pos.quick_length() < cd * cd)
velocity -= pos;
else
velocity += pos * n;
}
float v = velocity.normalize();
if (v < (was_stopped?0.5:0.0001))
velocity.clear();
}
v1 ublas_elementwise_div(const v1& a, const v2& b)
{
typedef typename v1::value_type D;
v1 c(a.size());
std::transform(a.begin(),a.end(),b.begin(),c.begin(),std::divides<D>());
return c;
}
static v3 getstangent(v2 A, v3 B, v3 N, v2 S)
{
A.normalize();
// Vector2 S(1.0, 0.0); // Os teksturnih koordinat OX
float sina = A.pseudoscalarProduct(S); // Vect mull
float cosa = A.dotProduct(S); // skalarnoe
// a - ugon mezhdu S i A
// esli v normalnom prostranstve povernut' vektor B na ugol -a (minus a), to mipolu4im vektor napravlennij v storonu uveli4enija koordinati S (sTangent)
// return B.rotate(N, sina, cosa);
matrix33 M;
M.setRotationAxis(sina, cosa, v3(N.x, N.y, N.z)); // povernut' na ang v ploskoti perpedukularnoj N
v3 V(B.x, B.y, B.z), R;
R = M*V;
v3 Res(R.x, R.y, R.z);
Res.normalize();
return Res;
}
double v2::dot(const v2& p) const
{
return internal_x*p.x()+internal_y*p.y();
}
v2 v2::product_compontentwise(const v2& p) const
{
return v2(internal_x*p.x(),internal_y*p.y());
}
v2 operator*(const double& s,const v2& p)
{
return v2(p.x()*s,p.y()*s);
}
v2 operator-(const double& s,const v2& p)
{
return v2(s-p.x(),s-p.y());
}
v2 v2::operator-(const v2& p2) const
{
return v2(x()-p2.x(),y()-p2.y());
}
v2 operator+(const double& s,const v2& p)
{
return v2(s+p.x(),s+p.y());
}
