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();
