// 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);
}
