// NOTE: for derivation of the algorithm, see mathlib.doc void operator^=(ANGLE3D &a3dAngles, const DOUBLEmatrix3D &t3dRotation) { ANGLE &h=a3dAngles(1); // heading ANGLE &p=a3dAngles(2); // pitch ANGLE &b=a3dAngles(3); // banking DOUBLE a; // temporary // calculate pitch p = ASin(-t3dRotation(2,3)); a = sqrt(1-t3dRotation(2,3)*t3dRotation(2,3)); // if pitch makes banking beeing the same as heading if (a<0.0001) { // we choose to have banking of 0 b = 0; // and calculate heading for that ASSERT(Abs(t3dRotation(2,3))>0.5); // must be around 1, what is far from 0 h = ATan2(t3dRotation(1,2)/(-t3dRotation(2,3)), t3dRotation(1,1)); // no division by 0 // otherwise } else { // calculate banking and heading normally b = ATan2(t3dRotation(2,1)/a, t3dRotation(2,2)/a); h = ATan2(t3dRotation(1,3)/a, t3dRotation(3,3)/a); } // snap angles to compensate for errors when converting to and from matrix notation Snap(h, ANGLE_SNAP); Snap(p, ANGLE_SNAP); Snap(b, ANGLE_SNAP); }
inline CVector3 CVector3::ToAngle() const { if (x == 0.f && y == 0.f) return CVector3(z > 0.f ? -90.f : 90.f, 0.f, 0.f); return CVector3(-RAD2DEG(ASin(z / (Length() + M_EPSILON))), RAD2DEG(ATan2(y, x)), 0.f); }
/* * Create angles in 3D from direction vector(ignoring banking). */ void DirectionVectorToAnglesNoSnap(const FLOAT3D &vDirection, ANGLE3D &a3dAngles) { // now calculate the angles ANGLE &h = a3dAngles(1); ANGLE &p = a3dAngles(2); ANGLE &b = a3dAngles(3); const FLOAT &x = vDirection(1); const FLOAT &y = vDirection(2); const FLOAT &z = vDirection(3); // banking is always irrelevant b = 0; // calculate pitch p = ASin(y); // if y is near +1 or -1 if (y>0.99 || y<-0.99) { // heading is irrelevant h = 0; // otherwise } else { // calculate heading h = ATan2(-x, -z); } }
// NOTE: for derivation of the algorithm, see mathlib.doc void DecomposeRotationMatrixNoSnap(ANGLE3D &a3dAngles, const FLOATmatrix3D &t3dRotation) { ANGLE &h=a3dAngles(1); // heading ANGLE &p=a3dAngles(2); // pitch ANGLE &b=a3dAngles(3); // banking FLOAT a; // temporary // calculate pitch FLOAT f23 = t3dRotation(2,3); p = ASin(-f23); a = Sqrt(1.0f-f23*f23); // if pitch makes banking beeing the same as heading if (a<0.001) { // we choose to have banking of 0 b = 0; // and calculate heading for that ASSERT(Abs(t3dRotation(2,3))>0.5); // must be around 1, what is far from 0 h = ATan2(t3dRotation(1,2)/(-t3dRotation(2,3)), t3dRotation(1,1)); // no division by 0 // otherwise } else { // calculate banking and heading normally b = ATan2(t3dRotation(2,1), t3dRotation(2,2)); h = ATan2(t3dRotation(1,3), t3dRotation(3,3)); } }
inline Vector3 QuaternionGetEulerAnglesInRadians(const Quaternion &quat) { const float poleTest = 2.f * (quat.y * quat.w - quat.x * quat.z); const float errorMarginTest = 0.0001f; if(IsNear(poleTest, 1.f, errorMarginTest)) { const float x = 0; const float y = QuartTau(); const float z = -2.f * ATan2(quat.x, quat.w); const Vector3 retVec = {x, y, z}; return retVec; } else if(IsNear(poleTest, -1.f, errorMarginTest)) { const float x = 0; const float y = -QuartTau(); const float z = 2.f * ATan2(quat.x, quat.w); const Vector3 retVec = {x, y, z}; return retVec; } else { const float wSq = quat.w * quat.w; const float xSq = quat.x * quat.x; const float ySq = quat.y * quat.y; const float zSq = quat.z * quat.z; const float x = ATan2(2.f * (quat.y * quat.z + quat.x * quat.w), (-xSq - ySq + zSq + wSq)); const float y = ASin(Clamp(poleTest, -1.f, 1.f)); const float z = ATan2(2.f * (quat.x * quat.y + quat.z * quat.w), (xSq - ySq - zSq + wSq)); const Vector3 retVec = {x, y, z}; return retVec; } }
/** 以确定的姿势(指倒着跑和正跑),跑到目标点 * Run to the destination point with a certain posture, forward or backword. * \param agent the agent itself. * \param act an atomic action caculated to go to pos for this cycle. * \param pos the destination point. * \param buffer point buffer, means the real destinantion is a cycle with the center * of pos and radius of buffer. * \param power the intend power to use. * \param inverse true means run backward. * \param turn_first true means that the agent should turn first to adjust the direction. */ void Dasher::GoToPointWithCertainPosture(Agent & agent, AtomicAction & act, Vector target, double buffer, double power, const bool inverse, bool turn_first) { act.Clear(); const Vector & my_pre_pos = agent.Self().GetPredictedPos(1); double dist = my_pre_pos.Dist(target); AngleDeg dirdiff = inverse? GetNormalizeAngleDeg((my_pre_pos - target).Dir() - agent.GetSelf().GetBodyDir()): GetNormalizeAngleDeg((target - my_pre_pos).Dir() - agent.GetSelf().GetBodyDir()); //需要转身的角度 double bufang; buffer = Max(0.05, buffer); //TODO: wait test if (buffer > FLOAT_EPS){ bufang = ASin(buffer / dist / 2) * 2; bufang = Max(10.0, bufang); } else { bufang = 0.0; } if (fabs(dirdiff) > bufang){ TurnDashPlaning(agent, act, target, buffer, power, inverse, turn_first); } else { power = inverse? -fabs(power): fabs(power); power = agent.GetSelf().CorrectDashPowerForStamina(power); if (fabs(power) > FLOAT_EPS){ act.mType = CT_Dash; act.mDashPower = AdjustPowerForDash(agent.Self(), target, buffer, power); act.mDashPower = agent.GetSelf().CorrectDashPowerForStamina(act.mDashPower); if (fabs(act.mDashPower) > FLOAT_EPS){ act.mSucceed = true; } else { act.mType = CT_None; //power为零时就不要执行了,没意义 TurnDashPlaning(agent, act, target, buffer, power, inverse, turn_first);//再次考虑转身,以免漏掉一线好的转身的机会 } } } }
/** * player 以确定得姿势(指倒着跑和正跑),跑到 target 所需要的周期数 * This function returns the minimum cycles for a player to go to a target position with * a certain posture, forward or backward. * @param player the player to caculate. * @param target the target position to go to. * @param inverse true means running backwards. * @param buffer * @return an integer to show the minimum cycles caculated. */ int Dasher::CycleNeedToPointWithCertainPosture(const PlayerState & player, Vector target, const bool inverse, double *buf) { int cycle = 0; //用的周期 const double & decay = player.GetPlayerDecay(); const double & speedmax = player.GetEffectiveSpeedMax(); const double & stamina = player.GetStamina(); const double & stamina_inc_max = player.GetStaminaIncMax(); const double & dash_max = ServerParam::instance().maxDashPower(); const Vector & pos = player.GetPos(); const Vector & vel = player.GetVel(); const double accrate = player.GetDashPowerRate() * player.GetEffort(); double speed = vel.Mod(); const Vector predict_pos_1 = pos + vel; const Vector predict_pos_2 = predict_pos_1 + vel * decay; const double dir = (target - pos).Dir(); double dis = (target - predict_pos_1).Mod(); const double kick_area = player.IsGoalie()? ServerParam::instance().catchAreaLength(): (player.GetKickableArea() - GETBALL_BUFFER); if (dis <= kick_area){ dis = pos.Dist(target); if (buf) *buf = 0; return 1; } double facing; if (player.IsBodyDirValid()) { facing = player.GetBodyDir(); } else if (speed > 0.26){ facing = vel.Dir(); } else { facing = dir; //认为不用转身 } double diffang = fabs(GetNormalizeAngleDeg(dir - facing)); const double oneturnang = player.GetMaxTurnAngle(); const double stamina_recovery_thr = ServerParam::instance().recoverDecThr() * ServerParam::instance().staminaMax(); double angbuf = FLOAT_EPS; angbuf = ASin(kick_area / dis); angbuf = Max(angbuf , 15.0); if (inverse) { diffang = 180.0 - diffang; facing = GetNormalizeAngleDeg(180.0 + facing); } //I 调整阶段 if(diffang <= angbuf){ //不需要转身 target = (target - pos).Rotate(-facing); dis = fabs(target.X()); double y = fabs(target.Y()); if(y < kick_area){ dis -= sqrt(kick_area * kick_area - y * y); } speed *= Cos(vel.Dir() - facing); //身体方向上的投影 } else if(diffang <= oneturnang){ cycle += 1; target -= predict_pos_1; speed *= Cos(vel.Dir() - dir); //取得目标方向的投影 speed *= decay;//进行投影.垂直方向1个周期后衰减到10+厘米了,并且在1turn时可加入考虑修正掉 dis = target.Mod(); dis -= kick_area; } else{ //认为转身两下(不细致) cycle += 2; target -= predict_pos_2; speed *= Cos(vel.Dir() - dir); //取得目标方向的投影 speed *= decay * decay;//进行投影.垂直方向1个周期后衰减到10+厘米了,并且在1turn时可加入考虑修正掉 dis = target.Mod(); dis -= kick_area; } if (dis <= 0){ if(buf != NULL){ *buf = -dis / ( speed / decay); *buf = Min(*buf , 0.99); } return Max(cycle, 0); } //II 加速 & 消耗体力阶段 const double stamina_used_per_cycle = inverse? dash_max * 2.0: dash_max; const int full_cyc = int((stamina - stamina_recovery_thr) / (stamina_used_per_cycle - stamina_inc_max)); //满体力阶段 int acc_cyc = 0;//加速阶段 const double speedmax_thr = speedmax * decay * 0.98; const double accmax = accrate * dash_max; while(acc_cyc < full_cyc && speed < speedmax_thr){ speed += accmax; if(speed > speedmax){ speed = speedmax; } dis -= speed; if(dis <= 0){//还没加速到最大就跑到了... cycle += acc_cyc + 1; if(buf != NULL){ *buf = -dis /( speed / decay ); *buf = Min(*buf , 0.99); } return Max(cycle, 0); } speed *= decay; ++ acc_cyc; } cycle += acc_cyc; //III 满体匀速阶段 int aver_cyc = full_cyc - acc_cyc; double aver_cyc_dis = aver_cyc * speedmax; if(aver_cyc_dis >= dis){ if(buf != NULL){ double realcyc = cycle + dis / speedmax; cycle = int( ceil(realcyc) ); *buf = cycle - realcyc; return Max(cycle, 0); } else{ cycle = int(ceil( cycle + dis / speedmax)); return Max(cycle, 0); } } else{ cycle += aver_cyc; dis -= aver_cyc_dis; } //IV 没体(0消耗)减速阶段 double acc_tired = stamina_inc_max * accrate; double speed_tired = acc_tired / (1 - decay); double speed_tired_thr = speed_tired * decay; speed *= decay; while(dis > 0 && fabs(speed - speed_tired_thr) > 0.004){ speed += acc_tired; dis -= speed; speed *= decay; ++cycle; } if(dis <= 0){ if(buf != NULL){ *buf = -dis / ( speed / decay); *buf = Min(*buf , 0.99); } return Max(cycle, 0); } //V 没体(0消耗)匀速阶段 if( buf != NULL){ double realcyc = cycle + dis / speed_tired; cycle = int( ceil( realcyc ) ); *buf = cycle - realcyc; return Max(cycle, 0); } else{ cycle = cycle + int(ceil ( dis / speed_tired)); return Max(cycle, 0); } }
/** * 考虑转身或直接dash * Planing to turn or to dash. * @param agent the agent itself. * @param act an atomic action to return the caculated decision. * @param target the target position to go to. * @param buffer * @param power the intend power for dash. * @param inverse true means running backwards. * @param turn_first true means turn first manually. */ void Dasher::TurnDashPlaning(Agent & agent, AtomicAction & act, Vector target, double buffer, double power, bool inverse, bool turn_first) { act.Clear(); power = inverse? -fabs(power): fabs(power); power = agent.GetSelf().CorrectDashPowerForStamina(power); PlayerState & self = agent.Self(); const Vector & my_pre_pos = self.GetPredictedPos(1); double target_dist = my_pre_pos.Dist(target); AngleDeg target_ang = inverse? GetNormalizeAngleDeg((my_pre_pos - target).Dir() - agent.GetSelf().GetBodyDir()): GetNormalizeAngleDeg((target - my_pre_pos).Dir() - agent.GetSelf().GetBodyDir()); //需要转身的角度 //DT_none //处理转身问题,可能1dash + 1turn更好 double bufang; if(buffer > FLOAT_EPS){ bufang = ASin(buffer / target_dist / 2) * 2; bufang = Max(10.0, bufang); } else{ bufang = 0.0; } const double & effort = self.GetEffort(); const double & accrate = self.GetDashPowerRate(); const double & inertia_moment = self.GetInertiaMoment(); const double & decay = self.GetPlayerDecay(); const double & speedmax = self.GetEffectiveSpeedMax(); const Vector & vel = self.GetVel(); const double & facing = self.GetBodyDir(); double speed = vel.Mod(); double diffang = fabs(target_ang); double oneturnang = self.GetMaxTurnAngle(); if(buffer < FLOAT_EPS && target_dist > 2.6){ oneturnang += 2.6; } if(turn_first || diffang <= oneturnang || speed < 0.04){ //由于噪声的存在,在非身体方向也有速度,调也调不过来,太小时不如直接转,0.04是一般队员的误差极值 act.mType = CT_Turn; act.mTurnAngle = target_ang; act.mSucceed = true; return; } else { //计算1 dash + 1turn; //下面都是算dash到什么速度最好 double mspd1 = (180.0/ ( diffang + 10 ) - 1.0 ) / inertia_moment; //10是buf,不然容易调整后正好还转不过来 double dash2spd = Min(mspd1 / decay, speedmax) / (1 + ServerParam::instance().playerRand()); if( fabs(GetNormalizeAngleDeg(vel.Dir() - facing)) > 90.0){ speed = -speed; } double spd_inf = Max(speed - accrate * ServerParam::instance().maxDashPower(), -dash2spd); double spd_sup = Min(speed + accrate * ServerParam::instance().maxDashPower(), dash2spd); Ray cur_r(self.GetPos(), facing); Vector pt_inf = cur_r.GetPoint(spd_inf * (1.0 + decay)); Vector pt_sup = cur_r.GetPoint(spd_sup * (1.0 + decay)); Vector pt;//要取得点 bool bnegtive;//要跑的点是不是在身后 Line pdl = Line(cur_r).GetPerpendicular(target);//垂线 if(pdl.IsPointInSameSide(pt_inf, pt_sup)){ if(fabs(GetNormalizeAngleDeg(facing-target_ang)) < 90.0){ pt = pt_sup; bnegtive = (spd_sup<0); } else{ pt = pt_inf; bnegtive = (spd_inf<0); } } else{ double dist; bnegtive = !cur_r.Intersection(pdl, dist); //垂点 pt = cur_r.GetPoint(dist); } double dis = pt.Dist(target);//两个周期后离point的距离,用来compare with 2 turn double twoturnang = oneturnang + 180.0 / (1.0 + inertia_moment * fabs(speed) * decay); if(twoturnang > diffang){ const Vector & pt2 = self.GetPredictedPos(2); double dis2 = pt2.Dist(target); if(dis2 < dis || diffang > 102.6){ //一次转太大,误差也大不如直接2turn act.mType = CT_Turn; act.mTurnAngle = target_ang; act.mSucceed = true; return; } } double spd_need; if(bnegtive){ spd_need = -(pt - self.GetPos()).Mod() / (1.0 + decay); } else{ spd_need = (pt - self.GetPos()).Mod() / (1.0 + decay); } //会总是dash而dash后又转不过来...,看着就像不拿球 double turnang_after_dash = 180.0 / (1.0 + inertia_moment * fabs(spd_need) * decay) - 5.0; if(GetAngleDegDiffer((target - pt).Dir(), facing) > turnang_after_dash ){ act.mType = CT_Turn; act.mTurnAngle = target_ang; act.mSucceed = true; return; } double power = ( spd_need - speed ) / accrate / effort; power = MinMax(-ServerParam::instance().maxDashPower(), power, ServerParam::instance().maxDashPower()); //I over act.mType = CT_Dash; act.mDashPower = AdjustPowerForDash(self, pt, FLOAT_EPS, power); act.mDashPower = agent.GetSelf().CorrectDashPowerForStamina(act.mDashPower); if(fabs(act.mDashPower) > FLOAT_EPS) { act.mSucceed = true; } else { act.mType = CT_Turn; //power为零时就不要执行了,没意义 act.mTurnAngle = 0.0; act.mSucceed = true; } } }
/* we always reason about the right trajectory for the ball leave velocity correction for dokick */ KickToRes turnball_kick(AngleDeg target_dir, TurnDir rotate, Bool StopBall, TurnKickCommand* pCom, float EndDist, float closeMarg, float kickFac) { float dir; float dist; Vector btraj; pCom->time = -1; pCom->turn_neck = FALSE; DebugKick(printf("\nat turnball_kick: target_dir: %f\n", target_dir)); LogAction4(60, "Turnball_kick: targ_dir: %.1f dir: %d", target_dir, (int)rotate); NormalizeAngleDeg(&target_dir); //DebugKick(printf("HERE Time: %d\n", Mem->CurrentTime.t)); /* the pos valid is not really right - if we are turning the ball and didn't actually see it last time, then there's a problem */ if ( !Mem->BallPositionValid() || !Mem->BallKickable() ) { LogAction2(90, "turnball_kick: lost the ball"); return KT_LostBall; } /* if the velocity isn's valid, turn to face ball */ if ( !Mem->BallVelocityValid() ) { float ball_ang_from_body = Mem->BallAngleFromBody(); /* for efficiency */ LogAction2(90, "turnball_kick: vel is not valid, looking at it"); DebugKick(printf("turning to face ball\n")); if (Mem->CanSeeBallWithNeck()) { pCom->time = Mem->CurrentTime; pCom->type = CMD_kick; pCom->angle = ball_ang_from_body + 180; pCom->power = Mem->CP_stop_ball_power; pCom->turn_neck = TRUE; pCom->turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck()); } else { /* turn body to face ball, and turn neck to straight ahead */ pCom->time = Mem->CurrentTime; pCom->type = CMD_turn; pCom->turn_neck = TRUE; if (fabs(ball_ang_from_body) > Mem->MaxEffectiveTurn()) { /* out body can't get to where we want to go */ pCom->angle = 180; /* get our maximum effective turn */ pCom->turn_neck_angle = ball_ang_from_body - signf(ball_ang_from_body)*Mem->MaxEffectiveTurn(); } else { pCom->angle = ball_ang_from_body; pCom->turn_neck_angle = -Mem->MyNeckRelAng(); } } return KT_TurnedToBall; } DebugKick(printf(" ball.dist: %f\t.dir: %f\n", Mem->BallDistance(), Mem->BallAngle())); DebugKick(printf(" HERE ball.vel.x: %f\t.y: %f\tmod: %f\n", Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y, Mem->BallSpeed())); DebugKick(printf(" ball.rpos.x: %f\t.y: %f\n", Mem->BallRelativePosition().x, Mem->BallRelativePosition().y)); DebugKick(printf(" target_dir: %f\n", target_dir)); if ( fabs(GetNormalizeAngleDeg(target_dir - Mem->BallAngleFromBody())) < Mem->CP_KickTo_err) { /* Do something to indicate we are done */ if (!StopBall || Mem->BallSpeed() < Mem->CP_max_ignore_vel) return KT_DidNothing; LogAction2(90, "turnball_kick: we're there, stopping the ball"); DebugKick(printf(" Stop ball kick\n")); dir = 0; dist = 0; PlayerMovementCorrection(&dir, &dist); *pCom = dokick(dir, dist, 1.0); pCom->turn_neck = FALSE; return KT_Success; } if (rotate == TURN_AVOID) { rotate = RotToAvoidOpponent(target_dir + Mem->MyBodyAng()); } if (rotate == TURN_CLOSEST) { rotate = RotClosest(target_dir + Mem->MyBodyAng()); } if (is_straight_kick(target_dir, EndDist, closeMarg)) { float pow; btraj = Polar2Vector(EndDist, target_dir) - Mem->BallRelativeToBodyPosition(); dir = btraj.dir(); dist = btraj.mod(); /* now we're goign to do some distance twiddling to get the ball to get to the right angle and stop */ pow = dist / Mem->BallKickRate(); pow = Min(pow, Mem->CP_max_turn_kick_pow); dist = pow * Mem->BallKickRate(); LogAction4(90, "turnball_kick: striaght kick: dist %f at %f", dist, dir); DebugKick(printf(" Straight kick# dir: %f dist: %f\n", dir, dist)); PlayerMovementCorrection(&dir, &dist); *pCom = dokick(dir, dist, 1.0); pCom->turn_neck = FALSE; } else if (Mem->BallDistance() < closeMarg) { /* ball is too close to do a tangent kick, so do a kick at 90 degrees */ dir = ((int)rotate)*(-90) + Mem->BallAngleFromBody(); dist = 2.0*sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(Mem->BallDistance())); LogAction2(90, "turnball_kick: 90 deg kick"); DebugKick(printf(" Close kick# dir: %f dist: %f\n", dir, dist)); PlayerMovementCorrection(&dir, &dist); *pCom = dokick(dir, dist, kickFac); pCom->turn_neck = FALSE; } else { /* do a turning kick */ /* we make a circle around the player of radius closeMarg and calculate the trajectory that goes in the right direction and is tangent to the circle */ dir = 180 + Mem->BallAngleFromBody() + ((int)rotate)*ASin(closeMarg / Mem->BallDistance()); DebugKick(printf(" ball dist: %f\tclosest_margin: %f\n", Mem->BallDistance(), closeMarg)); dist = sqrt(Sqr(Mem->BallDistance()) - Sqr(closeMarg)); dist += sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(closeMarg)); DebugKick(printf(" Turning ball# dir: %f dist: %f\n", dir, dist)); LogAction2(90, "turnball_kick: turning kick"); PlayerMovementCorrection(&dir, &dist); *pCom = dokick(dir, dist, kickFac); pCom->turn_neck = FALSE; } return KT_DidKick; }
inline Big<Exponent, Mantissa> asin(Big<Exponent, Mantissa> const& v) { return ASin(v); }