/// <summary>
/// 防守机器鱼在禁区内犯规时最大停留时间（单位：毫秒）
/// </summary>
#endregion
#region 对抗赛5VS5仿真使命控制规则具体处理过程
#region 进球判断及处理
// Added by renjing 20110310 Modified by LiYoubing 20110515
/// <summary>
/// 进球处理
/// </summary>
private void GoalHandler()
{
MyMission mymission = MyMission.Instance();
int j;
if (CommonPara.IsExchangedHalfCourt == false)
{
RoboFish f1 = Teams[0].Fishes[0];
for (j = 1; j < 4; j++)
{
RoboFish f2 = Teams[1].Fishes[j];
CollisionDetectionResult result = new CollisionDetectionResult();
result = CollisionDetection.DetectCollisionBetweenTwoFishes(ref f1, ref f2);
if (result.Intersect == true && gool[j - 1] == 0)
{
Teams[0].Para.Score++;
gool[j - 1] = 1;
string strMsg = "";
int Shootouttime = CommonPara.TotalSeconds * 1000 - CommonPara.RemainingCycles * mymission.ParasRef.MsPerCycle;
int ShootouttimeSec = Shootouttime / 1000;
ltime = Shootouttime;
strMsg += string.Format("Congradulations!shootouttime is: {1:00}:{2:00}:{3:00}.",
Teams[0].Para.Score, ShootouttimeSec / 60, ShootouttimeSec % 60, Shootouttime % 1000);
MatchHelper.MissionMessageBox(ref MyMission.Instance().MissionRef, strMsg,
"Confirming", MessageBoxButtons.OK, MessageBoxIcon.Warning);
}
}
}
else
{
RoboFish f1 = Teams[1].Fishes[0];
for (j = 1; j < 4; j++)
{
RoboFish f2 = Teams[0].Fishes[j];
CollisionDetectionResult result = new CollisionDetectionResult();
result = CollisionDetection.DetectCollisionBetweenTwoFishes(ref f1, ref f2);
if (result.Intersect == true && goor[j - 1] == 0)
{
Teams[1].Para.Score++;
goor[j - 1] = 1;
string strMsg = "";
int Shootouttime = CommonPara.TotalSeconds * 1000 / 2 - CommonPara.RemainingCycles * mymission.ParasRef.MsPerCycle;
int ShootouttimeSec = Shootouttime / 1000;
rtime = Shootouttime;
strMsg += string.Format("Congradulations!shootouttime is: {1:00}:{2:00}:{3:00}.",
Teams[0].Para.Score, ShootouttimeSec / 60, ShootouttimeSec % 60, Shootouttime % 1000);
MatchHelper.MissionMessageBox(ref MyMission.Instance().MissionRef, strMsg,
"Confirming", MessageBoxButtons.OK, MessageBoxIcon.Warning);
}
}
}
if (CommonPara.IsExchangedHalfCourt == false && Teams[0].Para.Score == 3)
{
CommonPara.RemainingCycles = CommonPara.TotalSeconds * 1000 / CommonPara.MsPerCycle / 2;
}
else if (CommonPara.IsExchangedHalfCourt == true && Teams[1].Para.Score == 3)
{
CommonPara.RemainingCycles = 0;
}
}
/// <summary>
/// Server服务实例向Client服务实例Notify当前周期的Mission对象
/// </summary>
/// <returns></returns>
public IEnumerator <ITask> MissionParaNotification()
{
// Notify all subscribers.
MissionPara announce = new MissionPara();
// 将当前仿真使命的通用Mission对象拷贝一份用于策略调用的参数传递
Mission mission = (Mission)MyMission.Instance().MissionRef.Clone();
for (int i = 0; i < mission.CommonPara.TeamCount; i++)
{// 循环对每条仿真机器鱼施加图像处理干扰 LiYoubing 20110617
for (int j = 0; j < mission.CommonPara.FishCntPerTeam; j++)
{
RoboFish f = mission.TeamsRef[i].Fishes[j];
Interference.ImageProcessInterference(ref f.PositionMm, ref f.BodyDirectionRad,
f.VelocityMmPs, f.AngularVelocityRadPs);
}
}
for (int i = 0; i < mission.EnvRef.Balls.Count; i++)
{// 循环对所有仿真水球施加图像处理干扰 LiYoubing 20110617
Ball b = mission.EnvRef.Balls[i];
float tmp = 0;
Interference.ImageProcessInterference(ref b.PositionMm, ref tmp, b.VelocityMmPs, b.AngularVelocityRadPs);
}
announce.Body = new MissionParaRequest(mission);
//announce.Body = new MissionParaRequest(MyMission.Instance().MissionRef);//modified 20101203
base.SendNotification(_submgrPort, announce);
announce.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// 返回所需转过的角度
/// </summary>
/// <param name="fish"></param>
/// <param name="aimPosition"></param>
/// <returns>返回所需转过的角度 </returns>
public static float Getxzdangle(RoboFish fish, xna.Vector3 aimPosition)
{
xna.Vector3 aimVector;
aimVector.X = aimPosition.X - fish.PolygonVertices[0].X;
aimVector.Z = aimPosition.Z - fish.PolygonVertices[0].Z;
aimVector.Y = 0;
//float aimAngle = StrategyHelper.Helpers.GetAngleDegree(aimVector);
xna.Vector3 fishRad = new xna.Vector3((float)Math.Cos(fish.BodyDirectionRad), 0, (float)Math.Sin(fish.BodyDirectionRad));
//公式：θ=atan2(v2.y,v2.x)−atan2(v1.y,v1.x)
//atan2的取值范围是[−π,π]，在进行相减之后得到的夹角是在[−2π,2π]，
//因此当得到的结果大于π时，对结果减去2π，当结果小于−π时，对结果加上2π
//虽然与一般坐标方向不一致，但是象限都是3 4 1 2的顺序，所以仍然成立
//但是仍需验证
float theta = (float)Math.Atan2(aimVector.Z, aimVector.X) - (float)Math.Atan2(fishRad.Z, fishRad.X);
if (theta > Math.PI)
{
theta -= (float)(2 * Math.PI);
}
else if (theta < -Math.PI)
{
theta += (float)(2 * Math.PI);
}
return(theta);
}
public static void JudgeFish(ref Decision[] decisions, RoboFish fish, xna.Vector3 destPosition, int i)
{
if (GetVectorDistance(fish.PositionMm, destPosition) > 150)
{
timeForPoseToPose[i] = 0;
decisions[i - 1].VCode = 0;
}
}
// Added by LiYoubing 20110515
/// <summary>
/// 判断仿真机器鱼鱼体前方矩形中心点（PositionMm）是否位于左/右球门区域
/// </summary>
/// <param name="fish">仿真机器鱼</param>
/// <param name="bIsLeftHalfCourt">待判断的是否左球门true判断左球门false判断右球门</param>
/// <returns>仿真机器鱼鱼体前方矩形中心点（PositionMm）位于球门内返回true否则返回false</returns>
private bool IsRoboFishInGoal(RoboFish fish, bool bIsLeftHalfCourt)
{
int tmpMm = (bIsLeftHalfCourt == true) ? Env.FieldInfo.LeftMm : Env.FieldInfo.RightMm;
int flag = (bIsLeftHalfCourt == true) ? 1 : -1;
bool bIsXInGoal = (bIsLeftHalfCourt == true)
? (fish.PositionMm.X <= tmpMm + flag * Env.FieldInfo.GoalDepthMm)
: (fish.PositionMm.X >= tmpMm + flag * Env.FieldInfo.GoalDepthMm);
return(bIsXInGoal && (fish.PositionMm.Z >= -Env.FieldInfo.GoalWidthMm / 2) &&
(fish.PositionMm.Z <= Env.FieldInfo.GoalWidthMm / 2));
}
/// <summary>
/// 指示是否已经发生过策略调用异常true是false否
/// 20110820废弃 策略代码中存在条件性除零/数组越界等异常时 不必要直接结束掉策略运行
/// </summary>
//bool _strategyExceptionFlag = false;
/// <summary>
/// 进行策略调用 获取指定id对应队伍的决策数据
/// </summary>
/// <param name="teamId">待获取决策数据的队伍id(从0开始)</param>
void GetLocalDecision(int teamId)
{
MyMission myMission = MyMission.Instance();
Decision[] decisions = null;
if (_serverControlBoard.TeamStrategyControls[teamId].StrategyInterface != null
/*&& _strategyExceptionFlag == false*/)
{// 进行null检查确保不出异常 LiYoubing 20110511
// 将当前仿真使命的通用Mission对象拷贝一份用于策略调用的参数传递
Mission mission = (Mission)MyMission.Instance().MissionRef.Clone();
for (int i = 0; i < mission.CommonPara.TeamCount; i++)
{// 循环对每条仿真机器鱼施加图像处理干扰 LiYoubing 20110617
for (int j = 0; j < mission.CommonPara.FishCntPerTeam; j++)
{
RoboFish f = mission.TeamsRef[i].Fishes[j];
Interference.ImageProcessInterference(ref f.PositionMm, ref f.BodyDirectionRad,
f.VelocityMmPs, f.AngularVelocityRadPs);
}
}
for (int i = 0; i < mission.EnvRef.Balls.Count; i++)
{// 循环对所有仿真水球施加图像处理干扰 LiYoubing 20110617
Ball b = mission.EnvRef.Balls[i];
float tmp = 0;
Interference.ImageProcessInterference(ref b.PositionMm, ref tmp, b.VelocityMmPs, b.AngularVelocityRadPs);
}
// 交换半场后交换策略处理 LiYoubing 20110711
// 交换半场后TeamsRef[0]/[1]代表交换前右/左半场的队伍因此应该分别调用第1/0号控件所加载的策略
int strategyId = myMission.ParasRef.IsExchangedHalfCourt ? (teamId + 1) % 2 : teamId;
try
{
decisions = _serverControlBoard.TeamStrategyControls[strategyId].StrategyInterface.GetDecision(
mission, teamId);
}
catch
{
//_strategyExceptionFlag = true;
MessageBox.Show("Remoting object timeout.\nThe instance of class Strategy has been released."
+ "\nYour simulated robofish will not be controlled.",
"Confirming", MessageBoxButtons.OK, MessageBoxIcon.Warning);
}
//decision = _serverControlBoard.TeamStrategyControls[teamId].StrategyInterface.GetDecision(
// mission, teamId);
//decision = _serverControlBoard.TeamStrategyControls[teamId].StrategyInterface.GetDecision(
// myMission.MissionRef, teamId);
}
if (decisions == null)
{
return;
}
for (int j = 0; j < myMission.TeamsRef[teamId].Fishes.Count; j++)
{
myMission.DecisionRef[teamId, j] = decisions[j];
}
}
/// <summary>
/// 顶球点和鱼的位置冲突，无法直线到达，防止无限顶球
/// </summary>
/// <param name="decision">决策变量</param>
/// <param name="fish">鱼的只读属性</param>
/// <param name="ball">球的坐标</param>
/// <param name="point">顶球点</param>
/// <returns>是否进行了调整</returns>
public static xna.Vector3 go_aside(ref Decision decision, RoboFish fish, xna.Vector3 ball, xna.Vector3 point)
{
//获取顶球点的对称点
xna.Vector3 point2 = new xna.Vector3(ball.X + (ball.X - point.X), 0, ball.Z + (ball.Z - point.Z));
if (GetDistance(fish.PolygonVertices[0], point) > 18.7f)
{
/* xna.Vector3 vector_point = new xna.Vector3(point.X - ball.X, 0, point.Z - ball.Z);//从球到顶球点的向量
* float rad_point = GetRadByVector(vector_point);
* float angle = rad_point - fish.BodyDirectionRad;
* if (angle > Math.PI) angle -= (float)(2 * Math.PI);
* else if (angle < -Math.PI) angle += (float)(2 * Math.PI);
* if (Math.Abs(angle) < (float)(0.2 * Math.PI))
* {
* //获得球的边缘上另外两点，以判断鱼头位置而调整方向
* //计算鱼头和顶球点的中点坐标
* xna.Vector3 mid = new xna.Vector3( 0.5f*(fish.PolygonVertices[0].X + point.X), 0, 0.5f * (fish.PolygonVertices[0].Z + point.Z));
* //计算鱼头和顶球点中垂线上一临时点M
* xna.Vector3 M = new xna.Vector3((float)(5.0 * mid.X - 4.0 * ball.X), 0, (float)(5.0 * mid.Z - 4.0 * ball.Z));
* //decision.VCode = 14;
* return M;//表示需要调整
* }*/
//计算鱼头和顶球点的中点坐标
xna.Vector3 mid = new xna.Vector3(0.5f * (fish.PolygonVertices[0].X + point.X), 0, 0.5f * (fish.PolygonVertices[0].Z + point.Z));
//计算鱼头和顶球点中垂线上一临时点M
if (mid.X == ball.X && mid.Z == ball.Z) //当顶球点和鱼头连线过球心时
{
xna.Vector3 shuiping = new xna.Vector3(1400 - ball.X, 0, 0); //从球心往右的水平线向量
xna.Vector3 despoint = new xna.Vector3(point.X - fish.PolygonVertices[0].X, 0, point.Z - fish.PolygonVertices[0].Z); //从鱼头到顶球点的向量
float rad_point = GetRadByVector(despoint);
float rad = 0f;
if (rad_point > 0.5 * Math.PI || rad_point < -0.5 * Math.PI)
{
rad = (float)Math.Abs((Math.PI - rad_point));
}
else
{
rad = (float)Math.Abs(rad_point);
}
xna.Vector3 M = new xna.Vector3((float)(ball.X - 3 * 58 * Math.Sin(rad)), 0, (float)(ball.Z + 3 * 58 * Math.Cos(rad)));
return(M);
}
else
{
xna.Vector3 M = new xna.Vector3((float)(5.0 * mid.X - 4.0 * ball.X), 0, (float)(5.0 * mid.Z - 4.0 * ball.Z));
return(M);
}
}
return(point);
}
public void SetFishInfo()
{
MyMission missionref = MyMission.Instance();
dgv.Location = new Point(10, 18);
dgv.Size = new Size(710, 230);
dgv.BackgroundColor = Color.White;
groupBox1.Controls.Add(dgv);
int fishes = 0;
for (int i = 0; i < missionref.TeamsRef.Count; i++)
{
for (int j = 0; j < missionref.TeamsRef[i].Fishes.Count; j++)
{
RoboFish f = missionref.TeamsRef[i].Fishes[j];
// 第1列第fishes + j个单元格的值为仿真机器鱼的X坐标值
dgv[0, fishes + j].Value = (int)(f.PositionMm.X);
// 第2列第fishes + j个单元格的值为仿真机器鱼的Z坐标值
dgv[1, fishes + j].Value = (int)(f.PositionMm.Z);
// 第3列第fishes + j个单元格的值为仿真机器鱼的鱼体方向
dgv[2, fishes + j].Value = string.Format("{0:0.0000}", f.BodyDirectionRad);
// 第4列第fishes + j个单元格的值为仿真机器鱼的运动速度
dgv[3, fishes + j].Value = string.Format("{0:0.0000}", f.VelocityMmPs);
// 第5列第fishes + j个单元格的值为仿真机器鱼运动的速度方向
dgv[4, fishes + j].Value = string.Format("{0:0.0000}", f.VelocityDirectionRad);
// 第6列第fishes + j个单元格的值为机器鱼的角速度
dgv[5, fishes + j].Value = string.Format("{0:0.0000}", f.AngularVelocityRadPs);
// 第7列第fishes + j个单元格的值为当前时刻机器鱼运动的速度档位
dgv[6, fishes + j].Value = f.Tactic.VCode;
// 第8列第fishes + j个单元格的值为当前时刻机器鱼运动的方向档位
dgv[7, fishes + j].Value = f.Tactic.TCode;
// 第9列第fishes + j个单元格的值为前一时刻机器鱼运动的目标决策速度档位
dgv[8, fishes + j].Value = f.TargetTactic.VCode;
// 第10列第fishes + j个单元格的值为前一时刻机器鱼运动的目标决策方向档位
dgv[9, fishes + j].Value = f.TargetTactic.TCode;
}
fishes += missionref.TeamsRef[i].Fishes.Count;
}
}
/// <summary>
/// 将仿真机器鱼的运动学参数写入Excel文档
/// </summary>
/// <param name="num"></param>
public void FishInfoWriteToExcel()
{
MyMission myMission = MyMission.Instance();
int fishes = 0;
for (int i = 0; i < myMission.TeamsRef.Count; i++)
{
for (int j = 0; j < myMission.TeamsRef[i].Fishes.Count; j++)
{
RoboFish f = myMission.TeamsRef[i].Fishes[j];
object[] objData = new Object[12]; // 仿真机器鱼运动参数记录数组
MSExcel.Worksheet sheet = (MSExcel.Worksheet)_excelDoc.Sheets[fishes + j + 1];
int iTmp = sheet.UsedRange.Cells.Rows.Count + 1; // 获取当前行数 + 1
objData[0] = iTmp - 2;
objData[1] = string.Format("{0:00}:{1:00}:{2:00}", DateTime.Now.Hour, DateTime.Now.Minute, DateTime.Now.Second);
// 第3/4列第fishes + j个单元格的值分别为仿真机器鱼的X/Z坐标
objData[2] = (int)(myMission.TeamsRef[i].Fishes[j].PositionMm.X);
objData[3] = (int)(myMission.TeamsRef[i].Fishes[j].PositionMm.Z);
//第5列第fishes + j个单元格的值为机器鱼的鱼体方向
objData[4] = string.Format("{0:0.0000}", myMission.TeamsRef[i].Fishes[j].BodyDirectionRad);
//第6列第fishes + j个单元格的值为机器鱼的速度
objData[5] = string.Format("{0:0.0000}", myMission.TeamsRef[i].Fishes[j].VelocityMmPs);
//第7列第fishes + j个单元格的值为机器鱼的速度方向
objData[6] = string.Format("{0:0.0000}", myMission.TeamsRef[i].Fishes[j].VelocityDirectionRad);
//第8列第fishes + j个单元格的值为机器鱼的角速度
objData[7] = string.Format("{0:0.0000}", myMission.TeamsRef[i].Fishes[j].AngularVelocityRadPs);
//第9/10列第fishes + j个单元格的值为机器鱼的速度档位/方向档位
objData[8] = myMission.TeamsRef[i].Fishes[j].Tactic.VCode;
objData[9] = myMission.TeamsRef[i].Fishes[j].Tactic.TCode;
//第11/12列第fishes + j个单元格的值为机器鱼的目标决策速度档位/方向档位
objData[10] = myMission.TeamsRef[i].Fishes[j].TargetTactic.VCode;
objData[11] = myMission.TeamsRef[i].Fishes[j].TargetTactic.TCode;
// get target range (cells of one row) for writing the info record above
MSExcel.Range range = sheet.get_Range(string.Format("A{0}", iTmp), string.Format("L{0}", iTmp));
// write the info record above to the target range
range.Value2 = objData;
}
fishes += myMission.TeamsRef[i].Fishes.Count;
}
}
//完整控制版的简略版，仅仅返回速度档位
public static int GetxzdVcode(RoboFish fish, xna.Vector3 destpoint)
{
float rad = Getxzdangle(fish, destpoint);
float t = Math.Abs(rad / getTtable(0));//最大转弯幅度预计时间
float dis = GetDistance(fish.PolygonVertices[0], destpoint);
for (int i = 0; i <= 14; i++)
{
if (dis / t <= getVtable(i))
{
return(i);
}
}
return(14);
}
/// <summary>
/// 更换队伍队员时点击按钮触发事件
/// </summary>
/// <param name="sender">事件对象</param>
/// <param name="e">事件参数</param>
public void CmbPlayers_SelectedIndexChanged(object sender, EventArgs e)
{
if (CmbPlayers.SelectedIndex == -1)
{
return;
}
RoboFish f = MyMission.Instance().TeamsRef[TeamId].Fishes[CmbPlayers.SelectedIndex];
//int flag = (MyMission.Instance().TeamsRef[TeamId].Para.MyHalfCourt == HalfCourt.RIGHT) ? -1 : 1;
BtnFrontColor.BackColor = f.ColorFish;
BtnBackColor.BackColor = f.ColorId;
TxtPositionMmX.Text = ((int)f.PositionMm.X).ToString();
TxtPositionMmZ.Text = ((int)f.PositionMm.Z).ToString();
TxtDirectionDeg.Text = ((int)xna.MathHelper.ToDegrees(f.BodyDirectionRad)).ToString();
}
public static void turn(ref Decision decision, RoboFish fish, float aim_direction)
{
float angle = aim_direction - fish.BodyDirectionRad;
if (angle < -2 * Math.PI)
{
angle += (float)(2 * Math.PI);
}
else if (angle > 2 * Math.PI)
{
angle -= (float)(2 * Math.PI);
}
decision.TCode = GetxzdTcode(angle);
decision.VCode = 1;
}
/// <summary>
/// 顶球算法
/// </summary>
/// <param name="decision">鱼的决策对象</param>
/// <param name="fish">鱼的属性只读对象</param>
/// <param name="ball">球的坐标</param>
/// <param name="dest">目标坐标</param>
/// <param name="angleThreshold">角度阈值，在此阈值内，进行顶球，超出则寻找顶球点</param>
/// <param name="Vcode1">速度最大值（越小越准）</param>
/// <param name="r">球的半径，大部分比赛默认58</param>
/// <param name="mission">比赛仿真对象</param>
/// <param name="times">标志变量</param>
public static void Dribble(ref Decision decision, RoboFish fish, xna.Vector3 ball, xna.Vector3 dest, float angleThreshold, int Vcode1, int r, Mission mission, ref int times)
{
xna.Vector3 point = getPoint(ball, dest, 58);
xna.Vector3 vector = new xna.Vector3(dest.X - ball.X, 0, dest.Z - ball.Z);
float rad = GetRadByVector(vector);
if (Math.Abs(fish.BodyDirectionRad - rad) < angleThreshold / 180 * Math.PI && GetDistance(fish.PolygonVertices[0], ball) <= 65)//在顶球区域内，进行顶球
{
approachToPoint(ref decision, fish, dest, Vcode1, 8, 6);
}
else //不在顶球区域内，寻找顶球点
{
StrategyHelper.Helpers.PoseToPose(ref decision, fish, point, rad, 2, 30, mission.CommonPara.MsPerCycle, ref times);
//approachToPoint(ref decision, fish, point, 14, 4, 2);
}
}
private void BtnBackColor_Click(object sender, EventArgs e)
{// 后端色标颜色修改为仿真机器鱼编号颜色
ColorDialog colorDlg = new ColorDialog();
colorDlg.ShowHelp = true;
colorDlg.Color = BtnBackColor.BackColor;
// 确认选择则更新按钮上显示的颜色和相应仿真机器鱼的编号颜色
if (colorDlg.ShowDialog() == DialogResult.OK)
{
BtnBackColor.BackColor = colorDlg.Color;
RoboFish f = MyMission.Instance().TeamsRef[TeamId].Fishes[CmbPlayers.SelectedIndex];
f.ColorId = BtnBackColor.BackColor;
}
}
public static xna.Vector3 CalCirclePoint(RoboFish fightFish, xna.Vector3 centralPoint)
{
int radius = 450;
double destX = centralPoint.X;
double destZ = centralPoint.Z;
xna.Vector3 fishTowardsPoint = centralPoint - fightFish.PositionMm;
double angle = MathHelper.ToRadians(Helpers.GetAngleDegree(fishTowardsPoint));
if (angle >= 0)
{
if (angle > Math.PI / 2)
{
angle = Math.PI - angle;
destX -= radius * Math.Cos(angle);
destZ += radius * Math.Sin(angle);
}
else
{
destX += radius * Math.Cos(angle);
destZ += radius * Math.Sin(angle);
}
}
else
{
if (angle < -Math.PI / 2)
{
angle = Math.PI - Math.Abs(angle);
destX -= radius * Math.Cos(angle);
destZ -= radius * Math.Sin(angle);
}
else
{
angle = Math.Abs(angle);
destX += radius * Math.Sin(angle);
destZ -= radius * Math.Cos(angle);
}
}
xna.Vector3 destPoint = new xna.Vector3((float)destX, 0, (float)destZ);
//StreamWriter log = new StreamWriter("C:\\Users\\wujun\\Desktop\\URWPGSim2D\\URWPGSim2D\\Strategy\\log.txt", true);
//log.Write(destPoint.X);
//log.Write(' ');
//log.Write(destPoint.Z);
//log.WriteLine("end");
//log.Close();
return(destPoint);
}
public static void approachToPoint2(ref Decision decision, RoboFish fish, xna.Vector3 destpoint)
{
float rad = Getxzdangle(fish, destpoint);
decision.TCode = GetxzdTcode(rad);
float t = Math.Abs(rad / getTtable(0));//最大转弯幅度预计时间
float dis = GetDistance(fish.PolygonVertices[0], destpoint);
for (int i = 0; i <= 14; i++)
{
if (dis / t <= getVtable(i))
{
decision.VCode = i;
break;
}
}
}
/// <summary>
/// 获取当前仿真使命（比赛项目）当前队伍所有仿真机器鱼的决策数据构成的数组
/// </summary>
/// <param name="mission">服务端当前运行着的仿真使命Mission对象</param>
/// <param name="teamId">当前队伍在服务端运行着的仿真使命中所处的编号
/// 用于作为索引访问Mission对象的TeamsRef队伍列表中代表当前队伍的元素</param>
/// <returns>当前队伍所有仿真机器鱼的决策数据构成的Decision数组对象</returns>
public Decision[] GetDecision(Mission mission, int teamId)
{
// 决策类当前对象第一次调用GetDecision时Decision数组引用为null
if (decisions == null)
{// 根据决策类当前对象对应的仿真使命参与队伍仿真机器鱼的数量分配决策数组空间
decisions = new Decision[mission.CommonPara.FishCntPerTeam];
}
RoboFish fish1 = mission.TeamsRef[teamId].Fishes[0];
Vector3 ball1 = mission.EnvRef.Balls[0].PositionMm;
RoboFish fish2 = mission.TeamsRef[teamId].Fishes[1];
Vector3 ball2 = mission.EnvRef.Balls[1].PositionMm;
OneFishGetScore(fish1, 1, ball1, ref decisions[0]);
OneFishGetScore(fish2, 2, ball2, ref decisions[1]);
//Fish1.Dingqiu(mission, teamId, ref decisions);
return(decisions);
}
/// <summary>
///
/// </summary>
/// <param name="fish">动作鱼</param>
/// <param name="ball">目标球</param>
/// <param name="dest">目标点</param>
/// <param name="dis">偏置距离，一般是球心后100左右</param>
/// <param name="radius">范围半径一般为60</param>
/// <returns></returns>
public static bool JudgeArea(RoboFish fish, xna.Vector3 ball, xna.Vector3 dest, int setDegree, int dis, int radius)
{
float fX = fish.PositionMm.X;
float fZ = fish.PositionMm.Z;
Vector3 area = new Vector3();
area = SetDestPtMm(dest, ball, setDegree, dis);
float xm = area.X;
float zm = area.Z;
if ((fX - xm) * (fX - xm) + (fZ - zm) * (fZ - zm) <= radius * radius)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 检查仿真机器鱼的位置参数合法性 若超出场地则调整到场地内
/// </summary>
/// <param name="obj">待检查位置参数合法性的方形障碍物对象</param>
public static void ParametersCheckingRoboFish(ref RoboFish obj)
{
Field f = Field.Instance();
// 执行重绘动作更新当前仿真机器鱼的PolygonVertices值
MyMission.Instance().IMissionRef.Draw();
int tmp = 0;
int minX = (int)UrwpgSimHelper.Min(obj.PolygonVertices[0].X, obj.PolygonVertices[1].X,
obj.PolygonVertices[2].X, obj.PolygonVertices[3].X, ref tmp);
if (minX < Field.Instance().LeftMm)
{ // 出了左边界
obj.PositionMm.X += Field.Instance().LeftMm - minX; // 右移超出的距离
}
else
{// 没出左边界
int maxX = (int)UrwpgSimHelper.Max(obj.PolygonVertices[0].X, obj.PolygonVertices[1].X,
obj.PolygonVertices[2].X, obj.PolygonVertices[3].X, ref tmp);
if (maxX > Field.Instance().RightMm)
{ // 出了右边界
obj.PositionMm.X -= maxX - Field.Instance().RightMm; // 左移超出的距离
}
}
int minZ = (int)UrwpgSimHelper.Min(obj.PolygonVertices[0].Z, obj.PolygonVertices[1].Z,
obj.PolygonVertices[2].Z, obj.PolygonVertices[3].Z, ref tmp);
if (minZ < Field.Instance().TopMm)
{ // 出了上边界
obj.PositionMm.Z += Field.Instance().TopMm - minZ; // 下移超出的距离
}
else
{// 没出上边界
int maxZ = (int)UrwpgSimHelper.Max(obj.PolygonVertices[0].Z, obj.PolygonVertices[1].Z,
obj.PolygonVertices[2].Z, obj.PolygonVertices[3].Z, ref tmp);
if (maxZ > Field.Instance().BottomMm)
{ // 出了下边界
obj.PositionMm.Z -= maxZ - Field.Instance().BottomMm; // 上移超出的距离
}
}
}
public static double angel(RoboFish fish, xna.Vector3 destPtMm)
{
xna.Vector3 srcPtMm = fish.PositionMm;
// 起始点到目标点的距离（目标距离）
double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)StrategyHelper.Helpers.GetAngleDegree(destPtMm - fish.PositionMm));
// 中间方向与鱼体方向的差值（目标角度）
double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
// 将目标角度规范化到(-PI,PI]
// 规范化之后目标角度为正，表示目标方向在鱼体方向右边
// 规范化之后目标角度为负，表示目标方向在鱼体方向左边
if (deltaTheta > Math.PI)
{ // 中间方向为正鱼体方向为负才可能目标角度大于PI
deltaTheta -= 2 * Math.PI; // 规范化到(-PI,0)
}
else if (deltaTheta < -Math.PI)
{ // 中间方向为负鱼体方向为正才可能目标角度小于-PI
deltaTheta += 2 * Math.PI; // 规范化到(0,PI)
}
return(deltaTheta);
}
/// <summary>
/// 鱼体简单快速运动封装
/// </summary>
/// <param name="decision">鱼的决策对象</param>
/// <param name="fish">鱼的参数只读对象</param>
/// <param name="aim_point">目标位置</param>
/// <param name="Vcode1">巡航速度最大值,可以取14，实际档位由程序决定</param>
/// <param name="Vcode2">减速第一阶段，默认为8</param>
/// <param name="Vcode3">减速第二阶段，默认为6</param>
static public void approachToPoint(ref Decision decision, RoboFish fish, xna.Vector3 aim_point, int Vcode1, int Vcode2, int Vcode3)
{
float angle = Getxzdangle(fish, aim_point);
int Tcode = GetxzdTcode(angle);
decision.TCode = Tcode;
float distance = GetDistance(fish.PolygonVertices[0], aim_point);
if (distance > 200)
{
int autoVcode = GetxzdVcode(fish, aim_point);
decision.VCode = Vcode1 < autoVcode ? Vcode1 : autoVcode;//较大距离时巡航速度
}
else if (distance >= 100)
{
decision.VCode = Vcode2;//第一阶段减速
}
else
{
decision.VCode = Vcode3;//第二阶段减速
}
}
/// <summary>
/// 获取当前仿真使命（比赛项目）当前队伍所有仿真机器鱼的决策数据构成的数组
/// </summary>
/// <param name="mission">服务端当前运行着的仿真使命Mission对象</param>
/// <param name="teamId">当前队伍在服务端运行着的仿真使命中所处的编号
/// 用于作为索引访问Mission对象的TeamsRef队伍列表中代表当前队伍的元素</param>
/// <returns>当前队伍所有仿真机器鱼的决策数据构成的Decision数组对象</returns>
public Decision[] GetDecision(Mission mission, int teamId)
{
// 决策类当前对象第一次调用GetDecision时Decision数组引用为null
if (decisions == null)
{// 根据决策类当前对象对应的仿真使命参与队伍仿真机器鱼的数量分配决策数组空间
decisions = new Decision[mission.CommonPara.FishCntPerTeam];
}
mission.CommonPara.MsPerCycle = 100;
#region 检查鱼是否被抓
//int by2 = Convert.ToInt32(mission.HtMissionVariables["IsYellowFish2Caught"]);
//int by3 = Convert.ToInt32(mission.HtMissionVariables["IsYellowFish3Caught"]);
//int by4 = Convert.ToInt32(mission.HtMissionVariables["IsYellowFish4Caught"]);
//int br2 = Convert.ToInt32(mission.HtMissionVariables["IsRedFish2Caught"]);
//int br3 = Convert.ToInt32(mission.HtMissionVariables["IsRedFish3Caught"]);
//int br4 = Convert.ToInt32(mission.HtMissionVariables["IsRedFish4Caught"]);
#endregion
#region 一堆鱼
RoboFish fightFish = mission.TeamsRef[(teamId + 1) % 2].Fishes[0];
RoboFish protectFish = mission.TeamsRef[teamId].Fishes[0];
RoboFish fish2 = mission.TeamsRef[teamId].Fishes[1];
RoboFish fish3 = mission.TeamsRef[teamId].Fishes[2];
RoboFish fish4 = mission.TeamsRef[teamId].Fishes[3];
#endregion
#region 障碍物中心点
xna.Vector3 blockUp = mission.EnvRef.ObstaclesRect[0].PositionMm;
xna.Vector3 blockMiddle = mission.EnvRef.ObstaclesRect[1].PositionMm;
xna.Vector3 blockDown = mission.EnvRef.ObstaclesRect[2].PositionMm;
#endregion
#region 圆形算法躲避
JudgeFish(ref decisions, fish2, CalCirclePoint(fightFish, blockUp), 2);
Helpers.Dribble(ref decisions[1], fish2, CalCirclePoint(fightFish, blockUp), CorrectRad(fightFish.BodyDirectionRad + (float)Math.PI), 20f, 15f, 100f, 8, 6, 15, 100, true);
JudgeFish(ref decisions, fish3, CalCirclePoint(fightFish, blockMiddle), 3);
Helpers.Dribble(ref decisions[2], fish3, CalCirclePoint(fightFish, blockMiddle), CorrectRad(fightFish.BodyDirectionRad + (float)Math.PI), 20f, 15f, 100f, 8, 6, 15, 100, true);
JudgeFish(ref decisions, fish4, CalCirclePoint(fightFish, blockDown), 4);
Helpers.Dribble(ref decisions[3], fish4, CalCirclePoint(fightFish, blockDown), CorrectRad(fightFish.BodyDirectionRad + (float)Math.PI), 20f, 15f, 100f, 8, 6, 15, 100, true);
#endregion
return(decisions);
}
//#region dribble算法封装
///// <summary>
///// dribble算法封装
///// </summary>
///// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
///// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
///// <param name="destPtMm">目标点。</param>
///// <param name="destDirRad">目标方向。</param>
///// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
///// 角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
///// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
///// 角度差大于此阈值，则机器鱼调整游动方向。</param>
///// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
///// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
///// /// <param name="VCode1">直游档位1（默认6档）。</param>
///// /// <param name="VCode2">直游档位2（默认4档）。</param>
///// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”,建议取5。</param>
///// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
///// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
///// <param name="dest">目标终点</param>
///// <returns></returns>
//public static bool dribble(ref Decision decision, RoboFish fish, xna.Vector3 ball, float destDirRad,
// float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag,
// xna.Vector3 dest_point)
//{
// xna.Vector3 aim_vector = new xna.Vector3(dest_point.X - ball.X, 0, dest_point.Z - ball.Z);
// float aim_rad = GetRadByVector(aim_vector);
// if (GetDistance(fish.PolygonVertices[0], ball) <= 60)//判断是否在顶球范围内
// {
// StrategyHelper.Helpers.Dribble(ref decision, fish, ball, aim_rad, angleTheta1, angleTheta2, disThreshold, VCode1, VCode2, cycles, msPerCycle, true);
// }
// else
// {
// if (flag0 == 3)
// {
// xna.Vector3 dest_point = new xna.Vector3(-600, 0, 200);//顶球终点
// xna.Vector3 aim_position = Method.getPoint(balls[0], dest_point, 58);//预顶球点
// xna.Vector3 aim_vector = new xna.Vector3(dest_point.X - balls[0].X, 0, dest_point.Z - balls[0].Z);
// float aim_rad = Method.GetRadByVector(aim_vector);//在顶球点的目标位姿
// //StrategyHelper.Helpers.PoseToPose(ref decisions[0], fishs[0], aim_position, aim_rad, 30f, 0f, mission.CommonPara.MsPerCycle, ref times[0], true);
// Method.approachToPoint(ref decisions[0], fishs[0], aim_position, 14, 8, 6);
// if (Method.ifOnVector(fishs[0].PolygonVertices[0], balls[0], dest_point))
// {
// times[0] = 0;
// flag0 = 4;
// }
// }
// if (flag0 == 4)
// {
// xna.Vector3 dest_point = new xna.Vector3(-600, 0, 200);//顶球终点
// xna.Vector3 aim_position = Method.getPoint(balls[0], dest_point, 58);//顶球点
// ////Method.approachToPoint(ref decisions[0], fishs[0], aim_position, 6, 5, 4);
// xna.Vector3 aim_vector = new xna.Vector3(dest_point.X - balls[0].X, 0, dest_point.Z - balls[0].Z);
// float aim_rad = Method.GetRadByVector(aim_vector);//在顶球点的目标位姿
// //Method.turn(ref decisions[0], fishs[0], aim_rad);
// StrategyHelper.Helpers.PoseToPose(ref decisions[0], fishs[0], aim_position, aim_rad, 5f, 0f, mission.CommonPara.MsPerCycle, ref times[0]);
// if (Math.Abs(fishs[0].BodyDirectionRad - aim_rad) < 0.05 * Math.PI)
// {
// times[0] = 0;
// flag0 = 5;
// }
// }
//}
//#endregion
#region 顶球终点为一条线的算法 获得线段上某点作为当前顶球终点
/// <summary>
/// 获得线段上某点作为当前顶球终点
/// </summary>
/// <param name="fish">机器鱼只读属性对象</param>
/// <param name="ball">球的坐标</param>
/// <param name="point1">线段一端点坐标</param>
/// <param name="point2">线段另一端点坐标</param>
/// <returns>返回当前鱼、球延长线与线段上交点 如不在线段上，则返回线段中点</returns>
public static xna.Vector3 getDest(RoboFish fish, xna.Vector3 ball, xna.Vector3 point1, xna.Vector3 point2)
{
float k1 = (ball.Z - fish.PositionMm.X) / (ball.X - fish.PositionMm.X);
float b1 = ball.Z - k1 * ball.X;
float k2 = (point1.Z - point2.Z) / (point1.X - point2.X);
float b2 = point1.Z - k2 * point1.X;
xna.Vector3 res = new xna.Vector3();
res.X = (b2 - b1) / (k1 - k2);
res.Z = (k1 * b2 - k2 * b1) / (k1 - k2);
res.Y = 0;
if ((res.X > point1.X && res.X < point2.X) || (res.X < point1.X && res.X > point2.X) || (res.Z > point1.Z && res.Z < point2.Z) || (res.Z < point1.Z && res.Z > point2.Z))
{
return(res);
}
else
{
res.X = (point1.X + point2.X) / 2;
res.Z = (point1.Z + point2.Z) / 2;
return(res);
}
}
private void BtnConfirm_Click(object sender, EventArgs e)
{
RoboFish f = MyMission.Instance().TeamsRef[TeamId].Fishes[CmbPlayers.SelectedIndex];
//int flag = (MyMission.Instance().TeamsRef[TeamId].Para.MyHalfCourt == HalfCourt.RIGHT) ? -1 : 1;
f.ColorFish = BtnFrontColor.BackColor;
f.ColorId = BtnBackColor.BackColor;
f.PrePositionMm = f.PositionMm; // 保存移动前的坐标
f.PositionMm.X = (int)Convert.ToSingle(TxtPositionMmX.Text);
f.PositionMm.Z = (int)Convert.ToSingle(TxtPositionMmZ.Text);
f.BodyDirectionRad = xna.MathHelper.ToRadians(Convert.ToSingle(TxtDirectionDeg.Text));
f.VelocityDirectionRad = f.BodyDirectionRad;
#region 设置的坐标数据合法性检查
UrwpgSimHelper.ParametersCheckingRoboFish(ref f);
// 无论是否进行过数据修正都将保存的值往界面控件上重写一次
TxtPositionMmX.Text = f.PositionMm.X.ToString();
TxtPositionMmZ.Text = f.PositionMm.Z.ToString();
#endregion
}
/// <summary>
/// 顶球点和鱼的位置冲突，无法直线到达，防止无限顶球
/// </summary>
/// <param name="decision">决策变量</param>
/// <param name="fish">鱼的只读属性</param>
/// <param name="ball">球的坐标</param>
/// <param name="point">顶球点</param>
/// <returns>是否进行了调整</returns>
public static bool go_aside(ref Decision decision, RoboFish fish, xna.Vector3 ball, xna.Vector3 point)
{
//获取顶球点的对称点
xna.Vector3 point2 = new xna.Vector3(ball.X + (ball.X - point.X), 0, ball.Z + (ball.Z - point.Z));
if (GetDistance(fish.PolygonVertices[0], point2) < 70 && GetDistance(fish.PolygonVertices[0], ball) < 80)
{
xna.Vector3 vector_point = new xna.Vector3(point.X - ball.X, 0, point.Z - ball.Z);//从球到顶球点的向量
xna.Vector3 vector_fish = new xna.Vector3(fish.PolygonVertices[0].X - ball.X, 0, fish.PolygonVertices[0].Z - ball.Z);
float rad_point = GetRadByVector(vector_point);
float rad_fish = GetRadByVector(vector_fish);
float angle = rad_point - rad_fish;
if (angle > Math.PI)
{
angle -= (float)(2 * Math.PI);
}
else if (angle < -Math.PI)
{
angle += (float)(2 * Math.PI);
}
if (angle > 0)
{
decision.TCode = 0;
}
else
{
decision.TCode = 14;
}
decision.VCode = 14;
//判断鱼头位置而调整方向
return(true);//表示需要调整
}
return(false);
}
/// <summary>
/// 仿真机器鱼位姿到位姿镇定算法（位置坐标和方向弧度值） Modified by Zhangbo20111020
/// 场地坐标系定义为：X向右，Y向下，负X轴顺时针转回负X轴角度范围为(-PI,PI)的坐标系
/// </summary>
/// <param name="decision">决策变量 输出参数 会被修改</param>
/// <param name="fish">目标仿真机器鱼（其PositionMm/PolygonVertices[0]和BodyDirectionRad参数为起始位姿）</param>
/// <param name="destPtMm">目标位置坐标（目标点）</param>
/// <param name="destDirRad">目标方向弧度值（目标方向）</param>
/// <param name="angThreshold">关键调节参数（中间方向与鱼体方向度数差值绝对值）上限，默认30度</param>
/// <param name="disThreshold">关键调节参数（临时目标点与最终目标点距离）阈值</param>
/// <param name="msPerCycle">每个仿真周期的毫秒数，默认取100</param>
public static void PoseToPose(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm, float destDirRad,
float angThreshold, float disThreshold, int msPerCycle, ref int times)
{
// 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
xna.Vector3 srcPtMm = fish.PositionMm;
// 起始点到目标点的距离（目标距离）
double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
+ Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));
// 沿目标方向的反方向偏离目标点阈值距离的临时目标点（临时目标点）
xna.Vector3 tmpPtMm = new xna.Vector3((float)(destPtMm.X - disThreshold * Math.Cos(destDirRad)),
0, (float)(destPtMm.Z - disThreshold * Math.Sin(destDirRad)));
double disSrcPtMmToTmpPtMm = Math.Sqrt(Math.Pow(tmpPtMm.X - srcPtMm.X, 2.0)
+ Math.Pow(tmpPtMm.Z - srcPtMm.Z, 2.0));
// 镇定阶段标志 1为远距离第一阶段2为近距离第二阶段
int phrase = 2;
if (disSrcPtMmToTmpPtMm > 0.2 * disThreshold && times == 0)
{// 起始点到临时目标点的距离大于阈值的20%，则认为尚未镇定到临时目标点，需要把目标点修正成临时目标点
destPtMm = tmpPtMm;
phrase = 1;
}
// 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));
// 中间方向与鱼体方向的差值（目标角度）
double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
// 将目标角度规范化到(-PI,PI]
// 规范化之后目标角度为正，表示目标方向在鱼体方向右边
// 规范化之后目标角度为负，表示目标方向在鱼体方向左边
if (deltaTheta > Math.PI)
{ // 中间方向为正鱼体方向为负才可能目标角度大于PI
deltaTheta -= 2 * Math.PI; // 规范化到(-PI,0)
}
else if (deltaTheta < -Math.PI)
{ // 中间方向为负鱼体方向为正才可能目标角度小于-PI
deltaTheta += 2 * Math.PI; // 规范化到(0,PI)
}
if (Math.Abs(deltaTheta) > angThreshold * Math.PI / 180.0)
{// 目标角度绝对值超过某一阈值（默认30度）速度档位置次低进行小半径转弯。防止控制率过大。
//decision.VCode = 1;
//decision.TCode = (deltaTheta <= 0) ? 1 : 13;
decision.VCode = 2;
decision.TCode = (deltaTheta <= 0) ? 0 : 14;
}
else
{
if (phrase == 1)
{// 第一阶段（在阈值区域之外）镇定算法
times = 0;
//decision.VCode = (disSrcPtMmToTmpPtMm < 0.5 * disThreshold) ? 4 : 10;
//decision.VCode = 8;
decision.VCode = 14;
decision.TCode = 7;
float lamdadot = ((destPtMm.X - srcPtMm.X) * (fish.VelocityMmPs * (float)Math.Sin(fish.BodyDirectionRad))
- (-destPtMm.Z + srcPtMm.Z) * (fish.VelocityMmPs * (float)Math.Cos(fish.BodyDirectionRad)))
/ ((float)Math.Pow(destPtMm.X - srcPtMm.X, 2.0) + (float)Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));
double targetAngularV = 50 * lamdadot;
if (deltaTheta <= 0)
{ // 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
while ((decision.TCode > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[decision.TCode] > targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
decision.TCode--;
}
}
else
{ // 目标角度为正目标方向在鱼体方向右边需要给右转档位
while ((decision.TCode < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[decision.TCode] < targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
decision.TCode++;
}
}
}
else
{// 第二阶段（进入阈值区域）镇定算法
times++;
float thetae = destDirRad - fish.BodyDirectionRad;
const float K1 = 0.6f;
const float K2 = 12.0f;
const float K3 = 18.0f;
xna.Vector3 srcPtMmLocal = new xna.Vector3(0, 0, 0);
UrwpgSimHelper.CoordinateTransformation(destDirRad, destPtMm, ref srcPtMmLocal, srcPtMm);
float u1 = -K1 * srcPtMmLocal.X * (float)Math.Pow(Math.Sin(times * msPerCycle / 1000.0f), 2.0);
float u2 = u1 * K2 * srcPtMmLocal.Z + u1 * K3 * (float)Math.Tan(thetae);
double targetVelocity = u1 / Math.Cos(thetae);
double targetAngularV = u2 * Math.Pow(Math.Cos(thetae), 2.0);
//if (disSrcPtMmToDestPtMm < 150.0f && Math.Abs(deltaTheta) < 10.0f * Math.PI / 180.0f)
if (disSrcPtMmToDestPtMm < 140.0f && Math.Abs(deltaTheta) < 10.0f * Math.PI / 180.0f)
{
decision.VCode = 0;
decision.TCode = 7;
}
else
{
decision.VCode = 2;
while ((decision.VCode < 14) && (DataBasedOnExperiment.VCodeAndVelocityTable[decision.VCode] < targetVelocity))
{// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
decision.VCode++;
}
decision.TCode = 7;
if (deltaTheta <= 0)
{ // 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
while ((decision.TCode > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[decision.TCode] > targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
decision.TCode--;
}
}
else
{ // 目标角度为正目标方向在鱼体方向右边需要给右转档位
while ((decision.TCode < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[decision.TCode] < targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
decision.TCode++;
}
}
}
}
}
}
public static void SmileFace(ref Mission mission, int teamId, ref Decision[] decisions)
{
#region 声明变量
int msPerCycle = mission.CommonPara.MsPerCycle;//仿真周期毫秒数
#region 一堆鱼
RoboFish fish1 = mission.TeamsRef[teamId].Fishes[0];
RoboFish fish2 = mission.TeamsRef[teamId].Fishes[1];
RoboFish fish3 = mission.TeamsRef[teamId].Fishes[2];
RoboFish fish4 = mission.TeamsRef[teamId].Fishes[3];
RoboFish fish5 = mission.TeamsRef[teamId].Fishes[4];
RoboFish fish6 = mission.TeamsRef[teamId].Fishes[5];
RoboFish fish7 = mission.TeamsRef[teamId].Fishes[6];
RoboFish fish8 = mission.TeamsRef[teamId].Fishes[7];
RoboFish fish9 = mission.TeamsRef[teamId].Fishes[8];
RoboFish fish10 = mission.TeamsRef[teamId].Fishes[9];
#endregion
#endregion
#region 构成笑脸的目标点
xna.Vector3 smileface2 = new xna.Vector3(-480, 0, -330);
xna.Vector3 smileface3 = new xna.Vector3(-246, 0, 774);
xna.Vector3 smileface4 = new xna.Vector3(816, 0, -588);
xna.Vector3 smileface5 = new xna.Vector3(1194, 0, -378);
xna.Vector3 smileface6 = new xna.Vector3(-900, 0, -528);
xna.Vector3 smileface7 = new xna.Vector3(-563, 0, 337);
xna.Vector3 smileface8 = new xna.Vector3(438, 0, 768);
xna.Vector3 smileface10 = new xna.Vector3(720, 0, 318);
xna.Vector3 center = new xna.Vector3(0, 0, 0);
#endregion
#region 构成与黄鱼互动的目标角度
float SFD2 = (float)0.7854;
float SFD3 = (float)-2.6180;
float SFD4 = (float)-1.0472;
float SFD5 = (float)0.7854;
float SFD6 = (float)-1.0472;
float SFD7 = (float)-1.8148;
float SFD8 = (float)0.5236;
float SFD10 = (float)-1.2217;
#endregion
#region 一堆鱼移动到目标点和目标角度
FishToPoint(ref decisions[1], fish2, smileface2, SFD2, 2, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[2], fish3, smileface3, SFD3, 3, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[3], fish4, smileface4, SFD4, 4, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[4], fish5, smileface5, SFD5, 5, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[5], fish6, smileface6, SFD6, 6, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[6], fish7, smileface7, SFD7, 7, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[7], fish8, smileface8, SFD8, 8, ref timeForPoseToPose, smileFaceflag);
FishToPoint(ref decisions[9], fish10, smileface10, SFD10, 10, ref timeForPoseToPose, smileFaceflag);
//FishToPoint(ref decisions[1], fish2, fish1.PolygonVertices[4], fish1.BodyDirectionRad - (float)1.309, 2, ref timeForPoseToPose, playflag);
Helpers.Dribble(ref decisions[8], fish9, fish1.PolygonVertices[3], CorrectRad(fish1.BodyDirectionRad - (float)1.0472), 30f, 20f, 100f, 14, 12, 15, 100, false);
//float dir3 = xna.MathHelper.ToRadians(Helpers.GetAngleDegree(center - fish1.PositionMm));
//Helpers.Dribble(ref decisions[1], fish2, fish1.PolygonVertices[0], dir3, 15, 30, 150, 14, 13, 15, 100, true);
#endregion
#region 到指定位置，进入下一函数
if (AllEqual(smileFaceflag, 2, 3, 10))
{
complete = true;
}
if (complete)
{
timeflag++;
if (timeflag >= 40)//等待4s
{
for (int i = 0; i < 11; i++)
{
timeForPoseToPose[i] = 0;
}
timeflag = 0;
flag++;
complete = false;
}
}
#endregion
}
/// <summary>
/// dirbble带球算法 Modified By Zhangbo 2011.10.22
/// </summary>
/// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
/// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
/// <param name="destPtMm">目标点。</param>
/// <param name="destDirRad">目标方向。</param>
/// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
/// 角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
/// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
/// 角度差大于此阈值，则机器鱼调整游动方向。</param>
/// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
/// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
/// /// <param name="VCode1">直游档位1（默认6档）。</param>
/// /// <param name="VCode2">直游档位2（默认4档）。</param>
/// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
/// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
/// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
public static void Dribble(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm, float destDirRad,
float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag)
{
// 调节所用周期数及每周期毫秒数转换得到秒数
double seconds1 = 15 * msPerCycle / 1000.0;
double seconds2 = cycles * msPerCycle / 1000.0;
// 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
// 起始点到目标点的距离（目标距离）
double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
+ Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));
// 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));
if (disSrcPtMmToDestPtMm < 30)
{// 起始点到目标点距离小于阈值（默认58毫米）将中间方向调为目标方向
dirFishToDestPtRad = destDirRad;
}
// 中间方向与鱼体方向的差值（目标角度）
double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
// 将目标角度规范化到(-PI,PI]
// 规范化之后目标角度为正表示目标方向在鱼体方向右边
// 规范化之后目标角度为负表示目标方向在鱼体方向左边
if (deltaTheta > Math.PI)
{ // 中间方向为正鱼体方向为负才可能目标角度大于PI
deltaTheta -= 2 * Math.PI; // 规范化到(-PI,0)
}
else if (deltaTheta < -Math.PI)
{ // 中间方向为负鱼体方向为正才可能目标角度小于-PI
deltaTheta += 2 * Math.PI; // 规范化到(0,PI)
}
// 最大角速度取左转和右转最大角速度绝对值的均值
float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0])
+ Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2;
// 以最大角速度转过目标角度所需的预计时间（角度预计时间）
double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
// 以角度预计时间游过目标距离所需平均速度值（目标速度）
double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;
int code = 1; // 目标（速度）档位初值置1
while ((code < 10) && (DataBasedOnExperiment.VCodeAndVelocityTable[code] < targetVelocity))
{// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
code++;
}
decision.VCode = code;
if (Math.Abs(deltaTheta) > angleTheta2 * Math.PI / 180.0)
{// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
decision.VCode = 2;
}
else if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
{// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
if (disSrcPtMmToDestPtMm > disThreshold)
{
decision.VCode = VCode1;
}
else
{
decision.VCode = VCode2;
}
}
// 以最大速度游过目标距离所需的预计时间（距离预计时间）
double estimatedTimeByDistance = disSrcPtMmToDestPtMm / DataBasedOnExperiment.VCodeAndVelocityTable[14];
if (estimatedTimeByDistance > seconds1)
{// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
estimatedTimeByDistance = seconds1;
}
// 以距离预计时间游过目标角度所需平均角速度（目标角速度）
double targetAngularV = deltaTheta / estimatedTimeByDistance;
code = 7;
if (deltaTheta <= 0)
{ // 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
while ((code > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] > targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
code--;
}
if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
{// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
// 则给相反方向次大转弯档位
code = 12;
}
}
else
{ // 目标角度为正目标方向在鱼体方向右边需要给右转档位
while ((code < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] < targetAngularV))
{ // 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
code++;
}
if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
{// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
// 则给相反方向次大转弯档位
code = 2;
}
}
decision.TCode = code;
}
public void OneFishGetScore(RoboFish fish, int leftOrRight, Vector3 ball, ref Decision decision)//leftOrRight参数：1为left，2为right
{
Vector3 fishLocation = fish.PositionMm;
float fishDirection = fish.BodyDirectionRad;
Vector3 fishTailLocation = (fish.PolygonVertices[3] + fish.PolygonVertices[4]) / 2;
Vector3 fishMiddleLocation = (fish.PolygonVertices[0] + fishTailLocation) / 2;
Vector3 targetPoint;
float targetDirection;
if (leftOrRight == 1)//自己球门在左边
{
Vector3 upPoint = new Vector3(-1000, 0, -500);
Vector3 bottomPoint = new Vector3(-1000, 0, 500);
Vector3 upTempPoint = new Vector3(-1350, 0, -750);
Vector3 bottomTempPoint = new Vector3(-1350, 0, 750);
if (ball.X > -1000) //球在门外
{
if (ball.Z > 0) //距离上面的点近一点
{
targetDirection = CalAngle(ball, upTempPoint);
}
else
{
targetDirection = CalAngle(ball, bottomTempPoint);
}
targetPoint = CalPointOnBall(ball, targetDirection);
if (GetVectorDistance(ball, fishLocation) > 150)//快速游到目标点
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5f, 8f, 400, 14, 13, 15, 100, true);
}
else
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2, 3, 200, 14, 13, 15, 100, true);
}
}
else if (ball.X <= -1250) //最左边区域
{
if (ball.Z < -500) //左上角
{
targetDirection = (float)3.1415;
targetPoint = new Vector3(-1500, 0, ball.Z - 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 15;
decision.VCode = 3;
}
}
else if (ball.Z > 500)//左下角
{
targetDirection = (float)3.1415;
targetPoint = new Vector3(-1500, 0, ball.Z + 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
else//左边中间区域（除球门内）
{
if (fishDirection < 0)
{
targetDirection = CalAngle(ball, upPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
else
{
targetDirection = CalAngle(ball, bottomPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
}
}
else //if (ball.X <= -1000 && ball.X > -1250)
{
if (ball.Z < -500)//卡在球门上侧的情况
{
targetDirection = (float)-1.5708;
targetPoint = new Vector3(ball.X + 80, 0, -1000);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 15;
decision.VCode = 3;
}
}
else if (ball.Z > 500)//卡在球门下侧的情况
{
targetDirection = (float)1.5708;
targetPoint = new Vector3(ball.X + 80, 0, 1000);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
else//球门区域内
{
//差死角位置
if (ball.Z > 400)
{
targetDirection = CalAngle(ball, upPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
return;
}
else if (ball.Z < -400)
{
targetDirection = CalAngle(ball, bottomPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
if (fishMiddleLocation.Z > ball.Z) //鱼在球下面
{
targetDirection = 0;
targetPoint = new Vector3(-1010, 0, ball.Z + 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 14;
decision.VCode = 3;
}
}
else//鱼在球上面
{
targetDirection = 0;
targetPoint = new Vector3(-1010, 0, ball.Z - 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
}
}
}
else if (leftOrRight == 2)
{
Vector3 upPoint = new Vector3(1000, 0, -500);
Vector3 bottomPoint = new Vector3(1000, 0, 500);
Vector3 upTempPoint = new Vector3(1350, 0, -750);
Vector3 bottomTempPoint = new Vector3(1350, 0, 750);
if (ball.X < 1000) //球在门外
{
if (ball.Z > 0) //距离上面的点近一点
{
targetDirection = CalAngle(ball, upTempPoint);
}
else
{
targetDirection = CalAngle(ball, bottomTempPoint);
}
targetPoint = CalPointOnBall(ball, targetDirection);
if (GetVectorDistance(ball, fishLocation) > 150)//快速游到目标点
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5f, 8f, 400, 14, 13, 15, 100, true);
}
else
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2, 3, 200, 14, 13, 15, 100, true);
}
}
else if (ball.X >= 1250) //最右边区域
{
if (ball.Z < -500) //右上角
{
targetDirection = 0;
targetPoint = new Vector3(1500, 0, ball.Z - 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
else if (ball.Z > 500)//右下角
{
targetDirection = 0;
targetPoint = new Vector3(1500, 0, ball.Z + 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 15;
decision.VCode = 3;
}
}
else//左边中间区域（除球门内）
{
if (fishDirection < 0)
{
targetDirection = CalAngle(ball, upPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
else
{
targetDirection = CalAngle(ball, bottomPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
}
}
else //if (ball.X <= -1000 && ball.X > -1250)
{
if (ball.Z < -500)//卡在球门上侧的情况
{
targetDirection = (float)-1.5708;
targetPoint = new Vector3(ball.X - 80, 0, -1000);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
else if (ball.Z > 500)//卡在球门下侧的情况
{
targetDirection = (float)1.5708;
targetPoint = new Vector3(ball.X - 80, 0, 1000);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 15;
decision.VCode = 3;
}
}
else//球门区域内
{
//差死角位置
if (ball.Z > 400)
{
targetDirection = CalAngle(ball, upPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
return;
}
else if (ball.Z < -400)
{
targetDirection = CalAngle(ball, bottomPoint);
targetPoint = CalPointOnBall(ball, targetDirection);
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 5, 10, 200, 10, 8, 15, 100, true);
}
if (fishMiddleLocation.Z > ball.Z) //鱼在球下面
{
targetDirection = (float)3.1415;
targetPoint = new Vector3(1010, 0, ball.Z + 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 0;
decision.VCode = 3;
}
}
else//鱼在球上面
{
targetDirection = (float)3.1415;
targetPoint = new Vector3(1010, 0, ball.Z - 80);
if (GetVectorDistance(fishLocation, ball) > 180 || IsDirectionRight(fishDirection, targetDirection) != 0)
{
Helpers.Dribble(ref decision, fish, targetPoint, targetDirection, 2f, 5f, 100, 5, 3, 15, 100, true);
}
else
{
decision.TCode = 14;
decision.VCode = 3;
}
}
}
}
}
}
/// <summary>
/// 返回鱼到达定点需要转的角度
/// </summary>
/// <param name="fish">机器鱼对象</param>
/// <param name="aimPosition">目标点坐标</param>
/// <returns>鱼到达定点需要转的角度</returns>
public static float _Getxzdangle(RoboFish fish, xna.Vector3 aimPosition)
{
float cur_x = fish.PositionMm.X;
float cur_z = fish.PositionMm.Z;
float dest_x = aimPosition.X;
float dest_z = aimPosition.Z;
float fish_rad = fish.BodyDirectionRad;
//鱼体方 向mission.TeamsRef[teamId].Fishes[i].BodyDirectionRad
float curangle;
float xzdangle = fish_rad;
curangle = (float)(Math.Abs(Math.Atan((cur_x - dest_x) / (cur_z - dest_z))));
if ((cur_x > dest_x) && (cur_z > dest_z))
{//以球为中心，当鱼在球的右下方
if (fish_rad < 0 && fish_rad > -Math.PI / 2)
{
xzdangle = -(float)(Math.PI / 2 + curangle + fish_rad);
}
else if (fish_rad > (-Math.PI) && fish_rad < -(Math.PI / 2))
{
xzdangle = (float)(-Math.PI / 2 - fish_rad - curangle);
}
else if (1.5 * Math.PI - fish_rad - curangle < fish_rad + 0.5 * Math.PI)
{
xzdangle = (float)(Math.PI * 1.5 - fish_rad - curangle);
}
else
{
xzdangle = (float)(fish_rad + 0.5 * Math.PI);
}
}
else if ((cur_x > dest_x) && (cur_z < dest_z))
{//以球为中心，当鱼在球的右上方
if (fish_rad < ((Math.PI / 2 + curangle)) && (-(Math.PI / 2 - curangle)) < fish_rad)
{
xzdangle = (float)(Math.PI / 2 + curangle - fish_rad);
}
else if ((-(Math.PI / 2 - curangle) > fish_rad) && fish_rad > -(Math.PI))
{
xzdangle = (float)(Math.PI * 2 + fish_rad - curangle);
xzdangle = -xzdangle;
}
else if (fish_rad > ((Math.PI / 2 + curangle)) && fish_rad < (Math.PI))
{
xzdangle = (float)(fish_rad - Math.PI / 2 - curangle);
xzdangle = -xzdangle;
}
}
else if ((cur_x < dest_x) && (cur_z < dest_z))
{//以球为中心，当鱼在球的左上方
if (fish_rad >= 0 && fish_rad < Math.PI)
{
xzdangle = (float)(curangle - fish_rad);
}
else if (fish_rad > 0.5 * Math.PI && fish_rad < Math.PI)
{
xzdangle = (float)(fish_rad - curangle);
}
else
{
if (-fish_rad + curangle > 2 * Math.PI + fish_rad - curangle)
{
xzdangle = -(float)(2 * Math.PI + fish_rad - curangle);
}
else
{
xzdangle = (float)(-fish_rad + curangle);
}
}
}
else if ((cur_x < dest_x) && (cur_z > dest_z))
{//以球为中心，当鱼在球的左下方
if (fish_rad >= 0 && fish_rad <= Math.PI)
{
if (curangle + fish_rad < Math.PI * 2 - curangle - fish_rad)
{
xzdangle = -(float)(curangle + fish_rad);
}
else
{
xzdangle = (float)(Math.PI * 2 - curangle - fish_rad);
}
}
else
{
if (fish_rad > -Math.PI && fish_rad < -0.5 * Math.PI)
{
xzdangle = (float)-(fish_rad + curangle);
}
else
{
xzdangle = (float)(fish_rad + curangle);
}
}
}
return(xzdangle);
}