Vector2D Knowledge::PredictDestination(Vector2D sourcePos, Vector2D targetPos, double sourceSpeed, Vector2D targetSpeed)
{
double factor = _wm->var[3] / 250.0;
if(factor < 0)
{
factor = 0;
}
double Vm = sourceSpeed;
double k = Vm / targetSpeed.length();
double gamaT = targetSpeed.dir().radian();
Vector2D delta;
delta = targetPos - sourcePos;
double landa = atan2(delta.y, delta.x);
double theta = gamaT - landa;
if (theta > AngleDeg::PI)
{
theta -= 2 * AngleDeg::PI;
}
if (theta < - AngleDeg::PI)
{
theta += 2 * AngleDeg::PI;
}
double dlta = 0;
if(k != 0 && fabs(sin(theta) / k) < 1)
{
dlta = asin(sin(theta)/k);
}
// No solution.
else
{
//qDebug() << "Prediction: No solution.";
return targetPos;//Vector2D::INVALIDATED;
}
double tf = factor * (delta.length() / 1000) / (Vm*cos(dlta) - targetSpeed.length() * cos(theta));
// No solution.
if(tf < 0)
{
//qDebug() << "Prediction: No solution.";
return targetPos; //Vector2D(0,0); //INVALIDATED;
}
double catchDist = targetSpeed.length() * tf * 1000;
Vector2D catchDiff(catchDist * cos(gamaT), catchDist * sin(gamaT));
Vector2D Pred_pos=targetPos + catchDiff;
//_wm->predict_pos.append(Pred_pos);
return Pred_pos;
}
DelayCoeff PlaneWave::calcDelayCoeff( const Speaker& speaker, const Angle& ang )
{
Vector2D diff = speaker.getPos() - position.getCurrentValue();
float cospsi = ( ang.getNormal() * diff ) / diff.length();
float delay = diff.length() * cospsi;
// factor is determined by angle between speaker normal and direction of the plane wave
float factor = speaker.getNormal() * ang.getNormal();
if( factor <= 0.0 )
return DelayCoeff( 0.0, 0.0 );
else
return DelayCoeff( delay, factor * speaker.getCosAlpha() * twonderConf->planeComp );
}
Vector2D force(Vector2D f, double t){
if(f.x == 0 || f.y == 0)
return Vector2D(0,0);
double fireTemp = 1;
double fireFalloff = 1;
double r = radius(t);
double distFromCircle = f.length() - r;
return f / f.length() * fireTemp * exp(-fireFalloff * distFromCircle);
}
std::vector<RobotState> RobotNeighbours::operator()(RobotState s)
{
static std::vector<Vector2D> lookUp = accs(constraints.maxAcc);
std::vector<RobotState> result;
Vector2D newPos = s.pos.add(s.speed);
unsigned int newTime = s.elapsedTime + 1;
Environment currentEnv = env.getAfterTime(newTime);
if(!currentEnv.isSound(Circle(newPos,constraints.r)))
{
return result;
}
for(auto dV : lookUp)
{
Vector2D newV = s.speed.add(dV);
if(newV.length() <= constraints.maxV)
{
RobotState newState;
newState.pos = newPos;
newState.speed = newV;
newState.elapsedTime = newTime;
result.push_back(newState);
}
}
return result;
}
void DistanceConstraint::checkInverseCollisions()
{
if ((NULL == node1_) || (NULL == node2_))
return;
if (node1_->isFixed() && node2_->isFixed())
return;
float w1 = node1_->getW();
float w2 = node2_->getW();
Vector2D p1 = node1_->getNewPosition();
Vector2D p2 = node2_->getNewPosition();
Vector2D direction = p1 - p2;
float distance = direction.length();
direction = direction.normalVec();
float t1 = w1 / (w1 + w2) * (distance - distance_) * stiffness_;
float t2 = w2 / (w1 + w2) * (distance - distance_) * stiffness_;
Vector2D dp1 = direction.invert() * t1;
Vector2D dp2 = direction * t2;
node1_->setNewPosition(p1 + dp1);
node2_->setNewPosition(p2 + dp2);
}
JET_END_TEST_F
JET_BEGIN_TEST_F(FlipSolver2, Rotation) {
// Build solver
auto solver = FlipSolver2::builder()
.withResolution({10, 10})
.withDomainSizeX(1.0)
.makeShared();
solver->setGravity({0, 0});
solver->setPressureSolver(nullptr);
// Build emitter
auto box = Sphere2::builder()
.withCenter({0.5, 0.5})
.withRadius(0.4)
.makeShared();
auto emitter = VolumeParticleEmitter2::builder()
.withSurface(box)
.withSpacing(1.0 / 20.0)
.withIsOneShot(true)
.makeShared();
solver->setParticleEmitter(emitter);
Array1<double> r;
for (Frame frame; frame.index < 360; ++frame) {
auto x = solver->particleSystemData()->positions();
auto v = solver->particleSystemData()->velocities();
r.resize(x.size());
for (size_t i = 0; i < x.size(); ++i) {
r[i] = (x[i] - Vector2D(0.5, 0.5)).length();
}
solver->update(frame);
if (frame.index == 0) {
x = solver->particleSystemData()->positions();
v = solver->particleSystemData()->velocities();
for (size_t i = 0; i < x.size(); ++i) {
Vector2D rp = x[i] - Vector2D(0.5, 0.5);
v[i].x = rp.y;
v[i].y = -rp.x;
}
} else {
for (size_t i = 0; i < x.size(); ++i) {
Vector2D rp = x[i] - Vector2D(0.5, 0.5);
if (rp.lengthSquared() > 0.0) {
double scale = r[i] / rp.length();
x[i] = scale * rp + Vector2D(0.5, 0.5);
}
}
}
saveParticleDataXy(solver->particleSystemData(), frame.index);
}
}
Vector2D
Vector2D::normalised() const {
Vector2D tmp = (*this);
tmp /= tmp.length();
return tmp;
}
double Vector2D::scalarProduct(const Vector2D &to)const
{
double result;
result=acos( (to.x*this->x +to.y*this->y)/(to.length()*this->length()) );
return result;
}
double
Vector2D::angle(const Vector2D &v) const {
double cosine = this->dot(v) / (this->length() * v.length());
if (cosine > 1.0) {
cosine = 1.0;
} else if (cosine < -1.0) {
cosine = -1.0;
}
return acos(cosine);
}
Vector2D Vector2D::normalVec() const
{
Vector2D val = (*this);
float len = val.length();
if (0.0f != len)
{
val.x /= len;
val.y /=len;
}
return val;
}
DelayCoeff PointSource::calcDelayCoeff( const Speaker& speaker, const Vector2D& sourcePos )
{
Vector2D srcToSpkVec = speaker.getPos() - sourcePos;
float normalProjection = srcToSpkVec * speaker.getNormal();
float srcToSpkDistance = srcToSpkVec.length();
float delay = srcToSpkDistance;
float cosphi = normalProjection / srcToSpkDistance;
// define a circular area around the speakers in which we adjust the amplitude factor to get a smooth
// transition when moving through the speakers ( e.g. from focussed to non-focussed sources )
float transitionRadius = twonderConf->speakerDistance * 1.5; // XXX: empirical value
// if source is in front of speaker
if( normalProjection < 0.0 )
{
// don't render this source if it is outside of the defined maximum range
// for focussed sources
if( srcToSpkDistance > twonderConf->focusLimit )
return DelayCoeff( 0.0, 0.0 );
// don't render this source if it in front of a this speaker
// but is not a focussed source
if( ( ! isFocused( sourcePos ) ) && ( srcToSpkDistance > transitionRadius ) )
return DelayCoeff( 0.0, 0.0 );
// if rendering focussed sources we need to use a "negative delay",
// i.e. make use of a certain amount of already added pre delay
// and so we don't get any phase inversion we only use positve numbers
// for our calculations
delay = - delay;
cosphi = - cosphi;
normalProjection = - normalProjection;
}
float amplitudeFactor = 0.0;
// calculate amplitudefactor according to being in- or outside the transition area around the speakers
if( srcToSpkDistance > transitionRadius )
{
amplitudeFactor = ( sqrtf( twonderConf->reference / ( twonderConf->reference + normalProjection ) ) ) * ( cosphi / sqrtf( srcToSpkDistance ) );
}
else
{
float behind = sqrtf( twonderConf->reference / ( twonderConf->reference + transitionRadius ) ) / sqrtf( transitionRadius );
float focuss = sqrtf( twonderConf->reference / ( twonderConf->reference - transitionRadius ) ) / sqrtf( transitionRadius );
amplitudeFactor = behind + ( transitionRadius - normalProjection ) / ( 2 * transitionRadius) * ( focuss - behind );
}
return DelayCoeff( delay, amplitudeFactor * speaker.getCosAlpha() );
}
double Vector2D::angleTo(const Vector2D & to) const
{
double result;
result=acos( (to.x*this->x +to.y*this->y)/(to.length()*this->length()) );
double z=this->x*to.y-this->y*to.x;
if(z>=0)
return result;
else return -result;
}
Vector2D Game::getCameraPosition() {
Vector2D move = currentLevel->getPlayerObject()->getPosition() - oldCameraPosition;
if (move.length() > cam_radius) {
oldCameraPosition += move.normalized() * (move.length()-cam_radius);
}
if (oldCameraPosition.getX() + 320 > currentLevel->getWidth()) {
oldCameraPosition.setX(currentLevel->getWidth() - 320);
}
if (oldCameraPosition.getX() < 320) {
oldCameraPosition.setX(320);
}
if (oldCameraPosition.getY() + 240 > currentLevel->getHeight()) {
oldCameraPosition.setY(currentLevel->getHeight() - 240);
}
if (oldCameraPosition.getY() < 240) {
oldCameraPosition.setY(240);
}
// TODO use Display size etc
return oldCameraPosition;
}
Vector2D Vector2D :: getRandomVector ( float len )
{
Vector2D v;
for ( ; ; )
{
v.x = rnd ();
v.y = rnd ();
if ( v.lengthSq () < EPS )
continue;
v *= len / v.length ();
return v;
}
}
TacticTest::TacticTest(WorldModel *worldmodel, QObject *parent) :
Tactic("TacticTest", worldmodel, parent)
{
circularBorder.assign(Vector2D(1500,0),1700/2);
circularBorderOut.assign(Vector2D(1500,0),2100/2);// a circle may use to push balls with some risks
circularMid.assign(Vector2D(1500,0),720); // a circle which is between holes and border circle
hole1.assign(Vector2D(1500,1700/4),250);
hole2.assign(Vector2D(1500,-1700/4),250);
Vector2D Cdist = (hole1.center() - circularBorder.center());
double deltaAngle=1.1*asin(hole1.radius()/(Cdist.length())); // 1.1 is safety factor
Uangle1=Cdist.dir().radian() + deltaAngle ;
Dangle1=Uangle1 - 2*deltaAngle;
Cdist = (hole2.center() - circularBorder.center());
Uangle2=Cdist.dir().radian() + deltaAngle ;
Dangle2=Uangle2 - 2*deltaAngle;
state=0;
}
void Chaser::update()
{
if (m_playerPos != nullptr)
{
Vector2D diff = *m_playerPos - m_position;
// Check if player is inside Chaser field of vision
if (diff.length() <= m_viewDistance)
{
if (abs(diff.x()) < m_treshold)
{
// Player caught in x axis, stop
m_velocity.x(0);
}
else
{
if (diff.x() < 0)
{
// x speed can be negative so we have to take abs value
m_velocity.x(-abs(m_xSpeed));
}
else if (diff.x() > 0)
{
//m_velocity.x(m_xSpeed);
m_velocity.x(abs(m_xSpeed));
}
}
}
else
{
// Player is outside Chaser's vision camp
m_velocity.x(0);
}
m_velocity.y(m_ySpeed);
handleMovement(m_velocity);
}
handleAnimation();
}
RobotCommand TacticPush2Goal::getCommand()
{
oppIsValid = wm->ourRobot[0].isValid;//wm->theirRobot.IsValid;// opposite robot
addData();
sortData();
if(!oppIsValid) opp = Vector2D(1000000,1000000);
opp = wm->ourRobot[0].pos.loc;//wm->theirRobot.position;//wm->ourRobot[8].pos.loc;
OppIsKhoraak=!circularBorder2.contains(opp);//out of his field
bool reach=false;
Avoided=false;
AllIn=true;
AnyIn=false;
AllInCorner=true;
AllUnAccessible=true;
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
rc.fin_pos.loc=Vector2D(400,0);//circularBorder.center();
rc.maxSpeed = 1.2;
index=-1;
int tah=balls.size()-1;
if(tah != -1)
{
rc.fin_pos.loc=circularBorder2.nearestpoint(balls.at(tah)->pos.loc);
rc.fin_pos.dir=(circularBorder.center()-rc.fin_pos.loc).dir().radian();
}
FindBall(); // find approtiate ball
if( index != -1 )
{
point2 = balls.at(index)->pos.loc;
FindHole();
Vector2D nrstpnt = (circularBorder.center()-point2); //nearest Point
nrstpnt=nrstpnt.setLength(nrstpnt.length()-circularBorder2.radius());
diff2 = nrstpnt;
nrstpnt=point2+nrstpnt;
// state2=0;
switch(state)
{
case 0:{ //Go Behind the Object
vec2goal.setLength(250);
// qDebug()<<"VEC 2 GOAL LENGTH = " << vec2goal.length();
rc.maxSpeed=1.3;
rc.useNav = false;//true;
rc.isBallObs = false;//true;
rc.isKickObs = false;//true;
rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 - vec2goal,
rc.maxSpeed,balls.at(index)->vel.loc);//point2 - vec2goal;
// wm->kn->PredictDestination()
int rad = 150+ROBOT_RADIUS;
Circle2D c(point2,rad);
rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// if(Avoided) rc.maxSpeed=rc.maxSpeed;
rc.fin_pos.dir=vec2goal.dir().radian();
// KeepInField(rc);
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
if(reach && !Avoided) state = 1;
}
break;
case 1:{//Push
rc.useNav = false;
rc.isBallObs = false;
rc.isKickObs = false;
rc.maxSpeed=1.1;
vec2goal.setLength(100);
rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 + vec2goal,
rc.maxSpeed,balls.at(index)->vel.loc);
rc.fin_pos.dir=vec2goal.dir().radian();
KeepInField(rc);
if(((wm->ourRobot[id].pos.loc-point2).length())>800) state=0;
if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
if(hole1.contains(rc.fin_pos.loc) || hole2.contains(rc.fin_pos.loc))
{
// vec2goal.setLength(100);
rc.fin_pos.loc=point2 ;//+ vec2goal;
// rc.fin_pos.loc=point2;
}
// reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,40);
// if(reach)
// state2 = 0;
}
break;
}
int mrgn=200;
Vector2D dlta;
if(IsInmargins(point2,mrgn))
{
int side = ((point2.x-mean_x)/abs(point2.x-mean_x))*((point2.y-mean_y)/abs(point2.y-mean_y));
if(point2.x > MAX_X-mrgn || point2.x < MIN_X+mrgn) {
side *= ((point2.y-mean_y)/abs(point2.y-mean_y));
dlta=Vector2D(0,side*(ROBOT_RADIUS+20));}
else if(point2.y > MAX_Y-mrgn || point2.y < MIN_Y+mrgn) {
side *=((point2.x-mean_x)/abs(point2.x-mean_x));
dlta=Vector2D(side*(ROBOT_RADIUS+20),0);}
switch(statemargin)
{
case 0:{
rc.fin_pos.loc=point2+dlta;
int rad = 70+ROBOT_RADIUS;
Circle2D c(point2,rad);
rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
qDebug()<< "In Margins Pos : ball = ( " << point2.x << ","<< point2.y << ")";
qDebug()<< "In Margins Pos : delta = ( " << dlta.x << ","<< dlta.y << ")";
qDebug()<< "In Margins Pos : fin_pos = ( " << rc.fin_pos.loc.x << ","<<rc.fin_pos.loc.y << ")";
qDebug()<< "In Margins Pos : Robot = ( " << wm->ourRobot[id].pos.loc.x << ","<<wm->ourRobot[id].pos.loc.y << ")";
rc.fin_pos.dir=dlta.dir().radian()-side*M_PI/2;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,7);
// wm->ourRobot[id].pos.loc,rc.fin_pos.loc,200);
qDebug() << "dist To final Pos : " << (wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length();
qDebug() << " Avoided : " << Avoided << " reach" << reach;
if(reach) statemargin = 1;
}
break;
case 1:{
rc.fin_pos.dir = dlta.dir().radian() - side*0.9*M_PI ;
rc.fin_pos.loc=point2-dlta;
qDebug() << "Fin_POS . dir = " << AngleDeg::rad2deg(rc.fin_pos.dir) << " ROBOT . dir = " << AngleDeg::rad2deg(wm->ourRobot[id].pos.dir);
if(((wm->ourRobot[id].pos.loc-point2).length())>300) statemargin=0;
double delta_ang=wm->ourRobot[id].pos.dir-rc.fin_pos.dir;
if (delta_ang > M_PI) delta_ang -= (M_PI * 2);
if (delta_ang < -M_PI) delta_ang += (M_PI * 2);
if(fabs(delta_ang) < AngleDeg::deg2rad(10)) statemargin=0;
rc.maxSpeed=1.7;
// bool chighz=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,10);
// if(chighz) statemargin=0;
// if((wm->ourRobot[id].pos.loc.dir()-rc.fin_pos.dir)<AngleDeg::deg2rad(10)) statemargin=0;
// if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
}
break;
}
}
}
// if(DontEnterCircle && circularBorderDANGER.contains(wm->ourRobot[id].pos.loc) && circularBorder2.contains(point2))//circularBorderDANGER.contains(rc.fin_pos.loc))
// {
// rc.fin_pos.loc=circularBorderDANGER.nearestpoint(point2);//wm->ourRobot[id].pos.loc;//AvoidtoEnterCircle(circularBorderDANGER,point2);
// rc.maxSpeed=1.1;
// }
// qDebug()<< "BEFOR ANY CHANGE " << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2;
// qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y;
// qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length();
rc.fin_pos.loc=KeepInField(rc);
if(/*((wm->ourRobot[id].pos.loc-opp).length() < 700)*/!OppIsKhoraak && oppIsValid )
{
rc.fin_pos.loc=AvoidtoEnterCircle(circularBorderDANGER,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);//rc.fin_pos.loc);
}
rc.fin_pos.loc=AvoidtoEnterCircle(hole1_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
rc.fin_pos.loc=AvoidtoEnterCircle(hole2_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
if(oppIsValid && OppIsKhoraak) rc.fin_pos.loc=opp;
qDebug() << "State Margin = " << statemargin;
// if(!OppIsKhoraak)
// {
// Circle2D c(opp,2*ROBOT_RADIUS+350);
// rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// }
// bool rich = wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,10,5);
// if(rich) rc.fin_pos.loc=wm->ourRobot[id].pos.loc;
// rc.fin_pos.loc=Vector2D(60000,60000);
// rc.fin_pos.loc=KeepInField(rc);
// rc.fin_pos.loc=Vector2D(MAX_X,MAX_Y);
// qDebug() << " DIRECTION ERROR = " << (rc.fin_pos.dir-wm->ourRobot[id].pos.dir)*(180/3.14);
//rc.useNav = true;
// rc.maxSpeed=1.8*rc.maxSpeed;
// rc.isBallObs = false;
// rc.isKickObs = false;
qDebug()<< "INDEX = " << index << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2;
//qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y;
//qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length();
// qDebug() << " IS INSIDE = " << IsInside << " UN ACCESSIBLE :" << unAccessible;
// qDebug() << " BALL SIZE : " << wm->balls.size();
// qDebug() << " ROBOT IS IN CORNER " << IsInCorner(wm->ourRobot[id].pos.loc,70) << " Robot Is In Margin : " << IsInmargins(wm->ourRobot[id].pos.loc,70) ;
rc.maxSpeed=1.21*rc.maxSpeed;
return rc;
}
float Vector2D::distance(const Vector2D& rhs) const
{
Vector2D val = (*this);
val -= rhs;
return val.length();
}
void Physics2D::resolveCollisionsByBouncing()
{
vector<Body2D*>& b = universe.bodies;
// detect collisions
for(unsigned i = 0; i < universe.bodies.size(); i++)
for(unsigned j = 0; j < i; j++)
{
const double distance = b[i]->position.distance(b[j]->position), radiiSum=0.5*(b[i]->diameter + b[j]->diameter);
if(distance < radiiSum)
{
// ease formula
const Vector2D diff_i = b[i]->position - b[j]->position, diff_j = b[j]->position - b[i]->position;
//computing new velocities
const Vector2D vi_2 = b[i]->velocity - diff_i*((b[i]->velocity-b[j]->velocity)^diff_i)*(1.0/pow(diff_i.length(), 2))*(2.0*b[j]->mass/(b[i]->mass+b[j]->mass));
const Vector2D vj_2 = b[j]->velocity - diff_j*((b[j]->velocity-b[i]->velocity)^diff_j)*(1.0/pow(diff_j.length(), 2))*(2.0*b[i]->mass/(b[i]->mass+b[j]->mass));
b[i]->velocity = vi_2;
b[j]->velocity = vj_2;
//make bodies not overlap
b[i]->position += b[i]->velocity.unit()*(radiiSum-distance)*(b[j]->mass/(b[i]->mass+b[j]->mass));
b[j]->position += b[j]->velocity.unit()*(radiiSum-distance)*(b[i]->mass/(b[i]->mass+b[j]->mass));
}
}
}
double dist(const Vector2D& v) const {
return fabs(dir.cross(v-p0)) / dir.length();
}
