void Astronomy::convertEquatorialToHorizontal ( double jday, double longitude, double latitude, double rasc, double decl, double &azimuth, double &altitude) { double d = jday - 2451543.5; double w = double (282.9404 + 4.70935E-5 * d); double M = double (356.0470 + 0.9856002585 * d); // Sun's mean longitude double L = w + M; // Universal time of day in degrees. double UT = double(fmod(d, 1) * 360); double hourAngle = longitude + L + double (180) + UT - rasc; double x = cosDeg (hourAngle) * cosDeg (decl); double y = sinDeg (hourAngle) * cosDeg (decl); double z = sinDeg (decl); double xhor = x * sinDeg (latitude) - z * cosDeg (latitude); double yhor = y; double zhor = x * cosDeg (latitude) + z * sinDeg (latitude); azimuth = atan2Deg (yhor, xhor) + double (180); altitude = atan2Deg (zhor, sqrt (xhor * xhor + yhor * yhor)); }
void Astronomy::convertEquatorialToHorizontal ( LongReal jday, LongReal longitude, LongReal latitude, LongReal rasc, LongReal decl, LongReal &azimuth, LongReal &altitude) { LongReal d = jday - 2451543.5; LongReal w = LongReal (282.9404 + 4.70935E-5 * d); LongReal M = LongReal (356.0470 + 0.9856002585 * d); // Sun's mean longitude LongReal L = w + M; // Universal time of day in degrees. LongReal UT = LongReal(fmod(d, 1) * 360); LongReal hourAngle = longitude + L + LongReal (180) + UT - rasc; LongReal x = cosDeg (hourAngle) * cosDeg (decl); LongReal y = sinDeg (hourAngle) * cosDeg (decl); LongReal z = sinDeg (decl); LongReal xhor = x * sinDeg (latitude) - z * cosDeg (latitude); LongReal yhor = y; LongReal zhor = x * cosDeg (latitude) + z * sinDeg (latitude); azimuth = atan2Deg (yhor, xhor) + LongReal (180); altitude = atan2Deg (zhor, sqrt (xhor * xhor + yhor * yhor)); }
void Astronomy::convertRectangularToSpherical ( double x, double y, double z, double &rasc, double &decl, double &dist) { dist = sqrt (x * x + y * y + z * z); rasc = atan2Deg (y, x); decl = atan2Deg (z, sqrt (x * x + y * y)); }
void Astronomy::convertRectangularToSpherical ( LongReal x, LongReal y, LongReal z, LongReal &rasc, LongReal &decl, LongReal &dist) { dist = sqrt (x * x + y * y + z * z); rasc = atan2Deg (y, x); decl = atan2Deg (z, sqrt (x * x + y * y)); }
/*! 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 ) ); }
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax ){ // separate sine and cosine part to circumvent boundary problem //return Vector2D::normalizeAngle( return normalizeTo180Deg( //wenns changed atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0, (cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) ); }
void Astronomy::getHorizontalSunPosition ( double jday, double longitude, double latitude, double &azimuth, double &altitude) { // 2451544.5 == Astronomy::getJulianDayFromGregorianDateTime(2000, 1, 1, 0, 0, 0)); // 2451543.5 == Astronomy::getJulianDayFromGregorianDateTime(1999, 12, 31, 0, 0, 0)); double d = jday - 2451543.5; // Sun's Orbital elements: // argument of perihelion double w = double (282.9404 + 4.70935E-5 * d); // eccentricity (0=circle, 0-1=ellipse, 1=parabola) double e = 0.016709 - 1.151E-9 * d; // mean anomaly (0 at perihelion; increases uniformly with time) double M = double(356.0470 + 0.9856002585 * d); // Obliquity of the ecliptic. //double oblecl = double (23.4393 - 3.563E-7 * d); // Eccentric anomaly double E = M + radToDeg(e * sinDeg (M) * (1 + e * cosDeg (M))); // Sun's Distance(R) and true longitude(L) double xv = cosDeg (E) - e; double yv = sinDeg (E) * sqrt (1 - e * e); //double r = sqrt (xv * xv + yv * yv); double lon = atan2Deg (yv, xv) + w; double lat = 0; double lambda = degToRad(lon); double beta = degToRad(lat); double rasc, decl; convertEclipticToEquatorialRad (lambda, beta, rasc, decl); rasc = radToDeg(rasc); decl = radToDeg(decl); // Horizontal spherical. Astronomy::convertEquatorialToHorizontal ( jday, longitude, latitude, rasc, decl, azimuth, altitude); }
shared_ptr<Action> Player::goTo(const Vector2f& destPos, AngDeg bodyDir, bool avoidBall) { Vector2f relTarget; //Vector2f tmpDestPos=Vector2f(-destPos.y(),destPos.x());//test by dpf float turnAng; //dpf change , comment: dpf want to use new transGlobalPosToRelPos() but it seems bad. //dpf once want to change myPos to torso's global pos, but failed, because we use myPos before // to test the FAT, now we must use it too, though it is good to undstand to use torso's global pos Vector2f myPos=WM.getMyGlobalPos2D(); Vector2f destRelPos=WM.transGlobalPosToRelPos(destPos); // Vector2f destRelPos = WM.transGlobalPosToRelPos(WM.getMyGlobalPos2D(), destPos); if (destRelPos.length() > 0.2f) //far enough, so don't care about body direction, just turn to destination { turnAng = -atan2Deg(destRelPos.x(), destRelPos.y()); if (fabs(turnAng) > 15.0f) relTarget = Vector2f(0, 0); else relTarget = destRelPos; } else { relTarget = destRelPos; turnAng = normalizeAngle(bodyDir - WM.getMyBodyDirection()); /////////////////////////////// } return goToAvoidBlocks(relTarget, turnAng, avoidBall); }
/*!得到矢量的水平角度 \param v 要得到水平角度的矢量 \return 水平角度*/ AngDeg getVector3fHorizontalAng( Vector3f v ) { return atan2Deg( v.y(), v.x() ); }
} i++; } } ////////////////////////////////////////////// //motionNum=7;//////////////////////////////////////test //calculate pos and dir to go const Vector2f& ballPos = WM.getBallGlobalPos2D(); Vector2f myDesiredRelPosToBall=mKickMotionVector[motionNum].myDesiredRelPosToBall; myDesiredRelPosToBall.y()+=-0.0278;//0.155*sinDeg(-10);//a patch, old my glbal pos is eye pos, but now is torso.0.155 is length from eye to torso, -10 is bodyAng Vector2f myDesiredPos = WM.transRelPosToGlobalPos(ballPos,myDesiredRelPosToBall); //Vector2f myDesiredPos = WM.transRelPosToGlobalPos(ballPos, mKickMotionVector[motionNum].myDesiredRelPosToBall); //rewrited by dpf, just for test AngDeg myDesiredBodyDir = (goal - ballPos).angle() + atan2Deg(mKickMotionVector[motionNum].kickTargetRel.x(), mKickMotionVector[motionNum].kickTargetRel.y()); *pPos = myDesiredPos; *pDir = myDesiredBodyDir; *pRelPosToStopWalk = mKickMotionVector[motionNum].relPosToBallToStopWalk; return shared_ptr<KickTask > (new KickTask(mKickMotionVector[motionNum].firstTaskName)); } boost::shared_ptr<action::Action> Player::dribbleToDir(AngDeg angC, AngDeg angL, AngDeg angR) { const Vector2f& ballPos = WM.getBallGlobalPos2D(); const Vector2f& myPos = WM.getMyGlobalPos2D(); AngDeg myDirToBall = (ballPos - myPos).angle();