void GLView::mouseMoveEvent(QMouseEvent* event)
{
if(!mouseDrag)
return;
QPoint current = event->globalPos();
int dx = current.x()-last.x();
int dy = current.y()-last.y();
if(event->buttons() & Qt::LeftButton) {
rotateX += (GLdouble)dy;
if(QApplication::keyboardModifiers() & Qt::ShiftModifier) {
rotateY += (GLdouble)dx;
} else {
rotateZ += (GLdouble)dx;
}
normalizeAngle(rotateX);
normalizeAngle(rotateY);
normalizeAngle(rotateZ);
} else {
if(QApplication::keyboardModifiers() & Qt::ShiftModifier) {
zoomView(-dy);
} else {
viewportX += (GLint)dx;
viewportZ -= (GLint)dy;
}
}
updateGL();
last = current;
}
//###########################################3
void rotateRel_naive(double deltaAngle)
{
double x, y, t;
double targetAngle;
double error;
int cmdVel;
getRobotPos(&x, &y, &t);
targetAngle = normalizeAngle(t + deltaAngle);
error = normalizeAngle(targetAngle - t);
if(error < 0)
cmdVel = -30;
else
cmdVel = 30;
setVel2(-cmdVel, cmdVel);
do
{
getRobotPos(&x, &y, &t);
error = normalizeAngle(targetAngle - t);
} while (fabs(error) > 0.01 && (error * cmdVel) > 0);
setVel2(0, 0);
}
int chooseBestBot(std::list<int>& freeBots, const Tactic::Param* tParam, int prevID) const
{
int minv = *(freeBots.begin());
float angle_difference = firaNormalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos)- state->homeAngle[*(freeBots.begin())]);
int minwt = Vector2D<int>::dist(state->homePos[*(freeBots.begin())],state->ballPos) + angle_difference * ANGLE_TO_DIST;
for (std::list<int>::iterator it = freeBots.begin(); it != freeBots.end(); ++it)
{
// TODO make the bot choosing process more sophisticated, the logic below returns the 1st available bot
float dis_from_ball = Vector2D<int>::dist(state->homePos[*it],state->ballPos);
float botballangle = normalizeAngle(Vector2D<int>::angle(state->ballPos, state->homePos[*it]));
//TODO might require normalization
float botball_orientation_diff = MIN(fabs(botballangle-state->homeAngle[*it]),fabs(botballangle-(state->homeAngle[*it]+PI)));
float finalOrientationDiff = normalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos)-botballangle);
// angle_difference = fabs(firaNormalizeAngle(state->homeAngle[*it]-normalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos))))+ fabs(firaNormalizeAngle((Vector2D<int>::angle(state->homePos[*it],Vector2D<int>(OPP_GOAL_X, 0)))));
angle_difference = botball_orientation_diff + finalOrientationDiff;
//float x_diff = ForwardX(state->ballPos.x)-ForwardX(state->homePos.x);
float weight;
//printf("%d >>>>>>>>>> %f , %f\n", *it,dis_from_ball,angle_difference);
weight = dis_from_ball + ANGLE_TO_DIST * angle_difference;
if(*it == prevID)
weight -= HYSTERESIS;
if(weight < minwt)
{
minwt = dis_from_ball ;
minv = *it;
}
}
//Util::Logger::toStdOut("Selected bot %d\n", minv);
printf(" :: %d ::",minv);
//assert(tParam=0);
return minv;
} // chooseBestBot
int getattackbotID()
{
int minv = 0;
float angle_difference = firaNormalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos)- state->homeAngle[0]);
int minwt = Vector2D<int>::dist(state->homePos[0],state->ballPos) + angle_difference * ANGLE_TO_DIST;
for (int it = 0; it < HomeTeam::SIZE ; ++it)
{
// TODO make the bot choosing process more sophisticated, the logic below returns the 1st available bot
float dis_from_ball = Vector2D<int>::dist(state->homePos[it],state->ballPos);
float botballangle = normalizeAngle(Vector2D<int>::angle(state->ballPos, state->homePos[it]));
//TODO might require normalization
float botball_orientation_diff = MIN(fabs(botballangle-state->homeAngle[it]),fabs(botballangle-(state->homeAngle[it]+PI)));
float finalOrientationDiff = normalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos)-botballangle);
// angle_difference = fabs(firaNormalizeAngle(state->homeAngle[*it]-normalizeAngle(Vector2D<int>::angle(Vector2D<int>(OPP_GOAL_X, 0), state->ballPos))))+ fabs(firaNormalizeAngle((Vector2D<int>::angle(state->homePos[*it],Vector2D<int>(OPP_GOAL_X, 0)))));
angle_difference = botball_orientation_diff + finalOrientationDiff;
//float x_diff = ForwardX(state->ballPos.x)-ForwardX(state->homePos.x);
float weight;
weight = dis_from_ball + ANGLE_TO_DIST * angle_difference;
//if(*it == botID)
// weight -= HYSTERESIS;
if(weight < minwt)
{
minwt = dis_from_ball ;
minv = it;
}
}
//Util::Logger::toStdOut("Selected bot %d\n", minv);
return minv;
}
void KoulesSimulator::updateShip(const oc::Control* control, double t)
{
const double* cval = control->as<KoulesControlSpace::ControlType>()->values;
double v[2] = { cval[0] - qcur_[2], cval[1] - qcur_[3] };
double deltaTheta = normalizeAngle(atan2(v[1], v[0]) - qcur_[4]);
double accel = 0., omega = 0.;
if (std::abs(deltaTheta) < shipEps)
{
if (v[0] * v[0] + v[1] * v[1] > shipDelta * shipDelta)
accel = shipAcceleration;
}
else
omega = deltaTheta > 0. ? shipRotVel : -shipRotVel;
if (accel == 0.)
{
qnext_[0] = qcur_[0] + qcur_[2] * t;
qnext_[1] = qcur_[1] + qcur_[3] * t;
qnext_[2] = qcur_[2];
qnext_[3] = qcur_[3];
qcur_[4] = normalizeAngle(qcur_[4] + omega * t);
}
else // omega == 0.
{
double ax = accel * cos(qcur_[4]);
double ay = accel * sin(qcur_[4]);
double t2 = .5 * t * t;
qnext_[0] = qcur_[0] + qcur_[2] * t + ax * t2;
qnext_[1] = qcur_[1] + qcur_[3] * t + ay * t2;
qnext_[2] = qcur_[2] + ax * t;
qnext_[3] = qcur_[3] + ay * t;
}
}
ReturnMatrix KinematicsSolver::getR(double rx, double ry, double rz) {
rx = normalizeAngle(rx);
ry = normalizeAngle(ry);
rz = normalizeAngle(rz);
double cA = cos(rz);
double sA = sin(rz);
double cB = cos(ry);
double sB = sin(ry);
double cG = cos(rx);
double sG = sin(rx);
Matrix m(3, 3);
m = 0.0;
m.row(1) << cA*cB << cA*sB*sG - sA*cG << cA*sB*cG + sA*sG;
m.row(2) << sA*cB << sA*sB*sG + cA*cG << sA*sB*cG - cA*sG;
m.row(3) << -sB << cB*sG << cB*cG;
m.Release();
return m;
}
/**
* Compute the difference in angle that is normalized in the range of [0, PI].
*/
float Util::diffAngle(float angle1, float angle2, bool absolute) {
if (absolute) {
return fabs(normalizeAngle(angle1 - angle2));
} else {
return normalizeAngle(angle1 - angle2);
}
}
SSL_WrapperPacket* SSLWorld::generatePacket()
{
SSL_WrapperPacket* packet = new SSL_WrapperPacket;
float x,y,z,dir;
ball->getBodyPosition(x,y,z);
packet->mutable_detection()->set_camera_id(0);
packet->mutable_detection()->set_frame_number(framenum);
double t_elapsed = timer->elapsed()/1000.0;
packet->mutable_detection()->set_t_capture(t_elapsed);
packet->mutable_detection()->set_t_sent(t_elapsed);
float dev_x = cfg->noiseDeviation_x();
float dev_y = cfg->noiseDeviation_y();
float dev_a = cfg->noiseDeviation_angle();
if (cfg->noise()==false) {dev_x = 0;dev_y = 0;dev_a = 0;}
if ((cfg->vanishing()==false) || (rand0_1() > cfg->ball_vanishing()))
{
SSL_DetectionBall* vball = packet->mutable_detection()->add_balls();
vball->set_x(randn_notrig(x*1000.0f,dev_x));
vball->set_y(randn_notrig(y*1000.0f,dev_y));
vball->set_z(z*1000.0f);
vball->set_pixel_x(x*1000.0f);
vball->set_pixel_y(y*1000.0f);
vball->set_confidence(0.9 + rand0_1()*0.1);
}
for(int i = 0; i < ROBOT_COUNT; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->blue_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_blue();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
for(int i = ROBOT_COUNT; i < ROBOT_COUNT*2; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->yellow_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_yellow();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i-ROBOT_COUNT);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
return packet;
}
//-----------------------------------------------------------------------------
void CPose3d::operator= ( const CPose3d pose )
{
mX = pose.mX;
mY = pose.mY;
mZ = pose.mZ;
mRoll = normalizeAngle ( pose.mRoll );
mPitch = normalizeAngle ( pose.mPitch );
mYaw = normalizeAngle ( pose.mYaw );
}
//-----------------------------------------------------------------------------
CPose3d::CPose3d ( CPose3d const& pose )
{
mX = pose.mX;
mY = pose.mY;
mZ = pose.mZ;
mRoll = normalizeAngle ( pose.mRoll );
mPitch = normalizeAngle ( pose.mPitch );
mYaw = normalizeAngle ( pose.mYaw );
}
//-----------------------------------------------------------------------------
CPose3d::CPose3d ( double x, double y, double z,
double roll, double pitch, double yaw )
{
mX = x;
mY = y;
mZ = z;
mRoll = normalizeAngle ( roll );
mPitch = normalizeAngle ( pitch );
mYaw = normalizeAngle ( yaw );
}
MotorDir RobotMath::getMotorDirToTarget(double currentAngle, double targetAngle)
{
currentAngle = normalizeAngle(currentAngle);
targetAngle = normalizeAngle(targetAngle);
int retDir = 1 * (std::abs(currentAngle - targetAngle) > 180 ? 1 : -1) * (currentAngle - targetAngle < 0 ? -1 : 1);
//MotorDir is syntactic sugar; could just return retDir
if (currentAngle - targetAngle == 0 || currentAngle - targetAngle == 180) return MotorDir::DIR_NONE;
return (retDir == 1 ? MotorDir::DIR_CW : MotorDir::DIR_CCW);
}
float Util::diffAngle(const QVector3D& dir1, const QVector3D& dir2, bool absolute) {
float ang1 = atan2f(dir1.y(), dir1.x());
float ang2 = atan2f(dir2.y(), dir2.x());
if (absolute) {
return fabs(normalizeAngle(ang1 - ang2));
} else {
return normalizeAngle(ang1 - ang2);
}
}
double RobotMath::angleDistance(double angle1, double angle2)
{
double dist = normalizeAngle(angle2) - normalizeAngle(angle1);
if (std::abs(dist) > 180)
{
double sgnval = sgn(dist);
return -sgnval * (360 - std::abs(dist));
}
return dist;
}
//-----------------------------------------------------------------------------
CPose3d CPose3d::operator+ ( const CPose3d pose )
{
CPose3d newPose;
newPose.mX = mX + pose.mX;
newPose.mY = mY + pose.mY;
newPose.mZ = mZ + pose.mZ;
newPose.mRoll = normalizeAngle ( mRoll + pose.mRoll );
newPose.mPitch = normalizeAngle ( mPitch + pose.mPitch );
newPose.mYaw = normalizeAngle ( mYaw + pose.mYaw );
return newPose;
}
void execute(const Param& tParam)
{
printf("DragtoGoal BotID: %d\n",botID);
static int stt = 0;
// Select the skill to be executed next
Vector2D<int> fastBallPos;
Vector2D<float> origin(0, 0);
fastBallPos = Vector2D<int>((int)state->ballPos.x + 0.3 * state->ballVel.x, (int)state->ballPos.y + 0.3 * state->ballVel.y);
if(state->ballVel.absSq() > MOVING_BALL_VELOCITY * MOVING_BALL_VELOCITY)
{
Util::Logger::toStdOut("Capturing fast Ball %f\n", state->ballVel.absSq());
float captureDist = Vector2D<int>::dist(state->ballPos, state->homePos[botID]);
// float captureAngle = fabs(firaNormalizeAngle(Vector2D<float>::angle(state->ballVel, origin)-Vector2D<int>::angle(state->homePos[botID], state->ballPos)));
// if(captureAngle < PI/3) {
// Util::Logger::toStdOut("Ball now in range\n");
// fastBallPos = Vector2D<int>((int)state->ballPos.x + 0.1*state->ballVel.x, (int)state->ballPos.y + 0.1*state->ballVel.y);
// }
}
Vector2D<int> goalPoint(HALF_FIELD_MAXX, 0);
float theta = normalizeAngle(Vector2D<int>::angle(goalPoint, fastBallPos));
float angleWithBall = normalizeAngle(Vector2D<int>::angle(fastBallPos, state->homePos[botID]));
//Vector2D<int> finalPoint = state->homePos[botID] + Vector2D<int>(2*BOT_BALL_THRESH*cos(theta), 2*BOT_BALL_THRESH*sin(theta));
Vector2D<int> targetPoint = fastBallPos - Vector2D<int>(2 * BOT_BALL_THRESH * cos(theta), 2 * BOT_BALL_THRESH * sin(theta));
float dist = Vector2D<int>::dist(targetPoint, state->homePos[botID]);
//printf("Drag to Goal: %f, %d, %f, \n", dist, 2*BOT_BALL_THRESH, fabs(firaNormalizeAngle(theta)));
float sign = (state->homePos[botID].x - fastBallPos.x) * (state->homePos[botID].x - HALF_FIELD_MAXX);
printf("state: %d\n", stt);
if(stt == 0 && isBallInMyWideAngleRange() && sign > 0)
{
stt = 1;
}
if(stt == 1 && (!isBallInMyLastHopeRange(botID) || sign < 0))
{
stt = 0;
}
if(stt == 1)
{
Util::Logger::toStdOut("Dribbling to goal %d %d", goalPoint.x, goalPoint.y);
gotoPointExact(state->ballPos.x, state->ballPos.y, true, normalizeAngle(Vector2D<int>::angle( goalPoint, fastBallPos )), MAX_BOT_SPEED);
}
else
{
//Util::Logger::toStdOut("Going to ball");
gotoPointExact(targetPoint.x, targetPoint.y, true, Vector2D<int>::angle(goalPoint, state->homePos[botID]), 0.7*MAX_BOT_SPEED);
}
if (state->pr_oppBall || state->pr_looseBall/*||state->pr_goalscored*/)
{
// tState = COMPLETED;
}
}
/*! return the two vector's clip angle
just now only consider in 2D !!!!
\param two vector
\return the clip angle
*/
AngDeg getClipAng(const Vector3f &v1, const Vector3f &v2)
{
// just now only consider in 2D !!!!
AngDeg ang1 = getVector3fHorizontalAng(v1);
AngDeg ang2 = getVector3fHorizontalAng(v2);
return normalizeAngle( ang1 - ang2 );
}
/**\brief Rotates the ship in the given direction (relative angle).
* \param direction Relative angle to rotate by
* \sa Model::GetRotationsperSecond()
*/
void Ship::Rotate( float direction ) {
float rotPerSecond, timerDelta, maxturning;
float angle = GetAngle();
if( !model ) {
LogMsg(WARN, "Attempt to rotate sprite with no model." );
return;
}
// Compute the maximum amount that the ship can turn
rotPerSecond = shipStats.GetRotationsPerSecond();
timerDelta = Timer::GetDelta();
maxturning = static_cast<float>((rotPerSecond * timerDelta) * 360.);
// Cap the ship rotation
if (fabs(direction) > maxturning){
if (direction > 0 )
angle += maxturning;
else
angle -= maxturning;
} else {
angle += direction;
}
// Normalize
angle = normalizeAngle(angle);
SetAngle( angle );
}
CNPositionEgo CNPositionAllo::toEgo(CNPositionAllo& me) const
{
auto ego = CNPositionEgo();
double x = this->x - me.x;
double y = this->y - me.y;
double angle = atan2(y, x) - me.theta;
double dist = sqrt(x * x + y * y);
ego.x = cos(angle) * dist;
ego.y = sin(angle) * dist;
ego.theta = normalizeAngle(this->theta - me.theta);
return ego;
//
// // sub me pos from allo pos -> rel pos with allo orientation
// double relX = this->x - me.x;
// double relY = this->y - me.y;
//
// // rotate rel point around origin -> rel point with ego orientation
// double s = sin(-me.theta);
// double c = cos(-me.theta);
//
// ego.x = c * relX - s * relY;
// ego.y = s * relX + c * relY; // TODO: fix experimentals
//
// // rotate theta
// ego.theta = this->theta - me.theta;
//
// return ego;
}
/*! This method returns the bisector (average) of two angles. It deals
with the boundary problem, thus when 'angMin' equals 170 and 'angMax'
equals -100, -145 is returned.
\param angMin minimum angle [-180,180]
\param angMax maximum angle [-180,180]
\return average of angMin and angMax. */
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax )
{
// separate sine and cosine part to circumvent boundary problem
return normalizeAngle(
atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0,
(cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) );
}
Gosu::Color Gosu::Color::fromAHSV(Channel alpha, double h, double s, double v)
{
if (s == 0)
// Grey.
return Color(alpha, v * 255, v * 255, v * 255);
// Normalize hue
h = normalizeAngle(h);
int sector = h / 60;
double factorial = h / 60 - sector;
double p = v * (1 - s);
double q = v * (1 - s * factorial);
double t = v * (1 - s * (1 - factorial));
switch (sector)
{
case 0:
return Color(alpha, v * 255, t * 255, p * 255);
case 1:
return Color(alpha, q * 255, v * 255, p * 255);
case 2:
return Color(alpha, p * 255, v * 255, t * 255);
case 3:
return Color(alpha, p * 255, q * 255, v * 255);
case 4:
return Color(alpha, t * 255, p * 255, v * 255);
default: // sector 5
return Color(alpha, v * 255, p * 255, q * 255);
}
}
//-----------------------------------------------------------------------------
CPose2d::CPose2d( double x, double y, double yaw, std::string name) : IRapiVar()
{
mName = name;
mX = x;
mY = y;
mYaw = normalizeAngle( yaw );
}
void execute(const Param& tParam)
{
printf("KicktoGoal BotID: %d\n",botID);
Vector2D<int> goal(HALF_FIELD_MAXX, 0);
float finalSlope = Vector2D<int>::angle(goal, state->homePos[botID]);
float turnAngleLeft = normalizeAngle(finalSlope - state->homeAngle[botID]); // Angle left to turn
float dist = Vector2D<int>::dist(state->ballPos, state->homePos[botID]);
if(dist > BOT_BALL_THRESH)
{
float finalSlope = Vector2D<int>::angle(Vector2D<int>(ForwardX(HALF_FIELD_MAXX, 0)), state->ballPos);
captureBall();
return;
}
if(fabs(turnAngleLeft) > 2 * SATISFIABLE_THETA)
{
turnToPoint(goal.x, goal.y);
return;
}
//printf("should kick now %f\n", turnAngleLeft);
kickBallForGoal();
if (state->homePos[botID].absSq() < BOT_BALL_THRESH * BOT_BALL_THRESH)
{
tState = COMPLETED;
}
}
/**\brief Update the Projectile
*
* Projectiles do all the normal Sprite things like moving.
* Projectiles check for collisions with nearby Ships, and if they collide,
* they deal damage to that ship. Note that since each projectile knows which ship fired it and will never collide with them.
*
* Projectiles have a life time limit (in milli-seconds). Each tick they need
* to check if they've lived to long and need to disappear.
*
* Projectiles have the ability to track down a specific target. This only
* means that they will turn slightly to head towards their target.
*/
void Projectile::Update( void ) {
Sprite::Update(); // update momentum and other generic sprite attributes
SpriteManager *sprites = SpriteManager::Instance();
// Check for projectile collisions
Sprite* impact = sprites->GetNearestSprite( (Sprite*)this, 100,DRAW_ORDER_SHIP|DRAW_ORDER_PLAYER );
if( (impact != NULL) && (impact->GetID() != ownerID) && ((this->GetWorldPosition() - impact->GetWorldPosition()).GetMagnitude() < impact->GetRadarSize() )) {
((Ship*)impact)->Damage( (weapon->GetPayload())*damageBoost );
sprites->Delete( (Sprite*)this );
// Create a fire burst where this projectile hit the ship's shields.
// TODO: This shows how much we need to improve our collision detection.
Effect* hit = new Effect(this->GetWorldPosition(), "Resources/Animations/shield.ani", 0);
hit->SetAngle( -this->GetAngle() );
hit->SetMomentum( impact->GetMomentum() );
sprites->Add( hit );
}
// Expire the projectile after a time period
if (( Timer::GetTicks() > secondsOfLife + start )) {
sprites->Delete( (Sprite*)this );
}
// Track the target
Sprite* target = sprites->GetSpriteByID( targetID );
float tracking = weapon->GetTracking();
if( target != NULL && tracking > 0.00000001f ) {
float angleTowards = normalizeAngle( ( target->GetWorldPosition() - this->GetWorldPosition() ).GetAngle() - GetAngle() );
SetMomentum( GetMomentum().RotateBy( angleTowards*tracking ) );
SetAngle( GetMomentum().GetAngle() );
}
}
gr_Robot_Command SkillSet::turnToPoint(const SParam ¶m, const BeliefState &state, int botID)
{
Vector2D<int> point(param.TurnToPointP.x, param.TurnToPointP.y);
Vector2D<int> botPos(state.homePos[botID].x, state.homePos[botID].y);
Vector2D<int> ballPos(state.ballPos.x, state.ballPos.y);
float finalSlope = Vector2D<int>::angle(point, botPos);
float turnAngleLeft = normalizeAngle(finalSlope - state.homePos[botID].theta); // Angle left to turn
float omega = turnAngleLeft * param.TurnToPointP.max_omega / (2 * PI); // Speedup turn
if(omega < MIN_BOT_OMEGA && omega > -MIN_BOT_OMEGA)
{
if(omega < 0) omega = -MIN_BOT_OMEGA;
else omega = MIN_BOT_OMEGA;
}
float v_x = omega*BOT_BALL_THRESH*1.5;
// comm.addCircle(state->homePos[botID].x, state->homePos[botID].y, 50);
float dist = Vector2D<int>::dist(ballPos, botPos);
if(dist < DRIBBLER_BALL_THRESH*1.2)
{
return getRobotCommandMessage(botID, v_x, 0, omega, 0, true);
}
else
{
return getRobotCommandMessage(botID, 0, 0, omega, 0, false);
}
}
void GLWidget::setZRotation(int angle){
normalizeAngle(&angle);
if(angle != zRot){
zRot = angle;
updateGL();
}
}
void VFH::computeCommands(Pose2D_t* robotPose, Position_t* goal, tScan* scan, double* v, double* w)
{
// **** Get desired theta orientation towards the goal
double targetTheta = atan2(goal->y - robotPose->y, goal->x - robotPose->x);
printf("Desired theta: %.3f\n", targetTheta*RAD2DEG);
// **** Analize scan to get available directions
std::vector<double> histogram;
int nOpenings = analyzeScan(scan, targetTheta, histogram);
printf("Number of openings: %d\n", nOpenings);
// **** Get direction that is closest to goal
double commandTheta;
if(nOpenings > 0)
{
commandTheta = computeRobotSteering(histogram); //Global coordinates
printf("Command theta (local): %.3f\n", commandTheta*RAD2DEG);
commandTheta = commandTheta + robotPose->theta;
printf("Command theta (global): %.3f\n", commandTheta*RAD2DEG);
}
else // No opening found => rotate 180ยบ to find other path
{
commandTheta = normalizeAngle(robotPose->theta + M_PI);
}
// **** Compute robot commands
computeRobotCommands(robotPose->theta, commandTheta, v, w);
printf("Robot commands: %.3f, %3f\n", *v, *w);
}
/** Determines the direction of the quad graph when driving to the specified
* successor. It also determines the last graph node that is still following
* the given direction. The computed data is saved in the corresponding
* graph node.
* It compares the lines connecting the center point of node n with n+1 and
* the lines connecting n+1 and n+2 (where n is the current node, and +1 etc.
* specifying the successor). Then it keeps on testing the line from n+2 to
* n+3, n+3 to n+4, ... as long as the turn direction is the same. The last
* value that still has the same direction is then set as the last node
* with the same direction in the specified graph node.
* \param current Index of the graph node with which to start ('n' in the
* description above).
* \param succ_index The successor to be followed from the current node.
* If there should be any other branches later, successor
* 0 will always be tetsed.
*/
void QuadGraph::determineDirection(unsigned int current,
unsigned int succ_index)
{
// The maximum angle which is still considered to be straight
const float max_straight_angle=0.1f;
// Compute the angle from n (=current) to n+1 (=next)
float angle_current = getAngleToNext(current, succ_index);
unsigned int next = getNode(current).getSuccessor(succ_index);
float angle_next = getAngleToNext(next, 0);
float rel_angle = normalizeAngle(angle_next-angle_current);
// Small angles are considered to be straight
if(fabsf(rel_angle)<max_straight_angle)
rel_angle = 0;
next = getNode(next).getSuccessor(0); // next is now n+2
// If the direction is still the same during a lap the last node
// in the same direction is the previous node;
int max_step = (int)m_all_nodes.size()-1;
while(max_step-- != 0)
{
// Now compute the angle from n+1 (new current) to n+2 (new next)
angle_current = angle_next;
angle_next = getAngleToNext(next, 0);
float new_rel_angle = normalizeAngle(angle_next - angle_current);
if(fabsf(new_rel_angle)<max_straight_angle)
new_rel_angle = 0;
// We have reached a different direction if we go from a non-straight
// section to a straight one, from a straight section to a non-
// straight section, or from a left to a right turn (or vice versa)
if( (rel_angle != 0 && new_rel_angle == 0 ) ||
(rel_angle == 0 && new_rel_angle != 0 ) ||
(rel_angle * new_rel_angle < 0 ) )
break;
rel_angle = new_rel_angle;
next = getNode(next).getSuccessor(0);
} // while(1)
GraphNode::DirectionType dir =
rel_angle==0 ? GraphNode::DIR_STRAIGHT
: (rel_angle>0) ? GraphNode::DIR_RIGHT
: GraphNode::DIR_LEFT;
m_all_nodes[current]->setDirectionData(succ_index, dir, next);
} // determineDirection
void Simulator::setYRotation(int angle)
{
normalizeAngle(angle);
if (angle != yRot)
{
yRot = angle;
}
}
void Simulator::setZRotation(int angle)
{
normalizeAngle(angle);
if (angle != zRot)
{
zRot = angle;
}
}
