基于URWPGSim2D仿真平台水下机器鱼斯诺克的源代码

using System; using System.Collections.Generic; using System.Linq; using System.Text; using URWPGSim2D.Core; using xna = Microsoft.Xna.Framework; using URWPGSim2D.Common; using URWPGSim2D.StrategyLoader; namespace URWPGSim2D.Strategy { public class Strategy : MarshalByRefObject, IStrategy { #region reserved code never be changed or removed /// <summary> /// override the InitializeLifetimeService to return null instead of a valid ILease implementation /// to ensure this type of remote object never dies /// </summary> /// <returns>null</returns> public override object InitializeLifetimeService() { //return base.InitializeLifetimeService(); return null; // makes the object live indefinitely } #endregion /// <summary> /// 决策类当前对象对应的仿真使命参与队伍的决策数组引用 第一次调用GetDecision时分配空间 /// </summary> private Decision[] decisions = null; private xna.Vector3 ZuoShangQiuDong = new xna.Vector3(-2250, 0, -1500); private xna.Vector3 ZuoXiaQiuDong = new xna.Vector3(-2250, 0, 1500); private xna.Vector3 ZhongShangQiuDong = new xna.Vector3(0, 0, -1500); private xna.Vector3 ZhongXiaQiuDong = new xna.Vector3(0, 0, 1500); private xna.Vector3 YouShangQiuDong = new xna.Vector3(2250, 0, -1500); private xna.Vector3 YouXiaQiuDong = new xna.Vector3(2250, 0, 1500); private xna.Vector3 ZuiJiaDingQiuDian = new xna.Vector3(); private xna.Vector3 YuZhiDianZuoBiao = new xna.Vector3(); private xna.Vector3 ZuiJiaQiuDong = new xna.Vector3(); private xna.Vector3 MuBiaoDian = new xna.Vector3(); private xna.Vector3 ZuiJiaJinQiuDian = new xna.Vector3(); private int i = 0; #region 求最佳顶球点 public xna.Vector3 ZuiJiaDingQiuDianHanShu(Mission mission, xna.Vector3 MuBiaoDian, int i) { Ball Qiu = mission.EnvRef.Balls[i]; xna.Vector3 QiuZuoBiao = Qiu.PositionMm; xna.Vector3 zuijiadingqiudian = FanXiangYuZhiDianZuoBiaoHanShu(QiuZuoBiao, MuBiaoDian, 58); return zuijiadingqiudian; } #endregion #region AB向量弧度 public float XiangLiangHuDu(xna.Vector3 dianA, xna.Vector3 dianB) { xna.Vector3 XiangLiangAB = dianB - dianA; float XiangLiangABJiaoDu = GetAngleDegree(XiangLiangAB); float XiangLiangABHuDu = xna.MathHelper.ToRadians(XiangLiangABJiaoDu); return XiangLiangABHuDu; } #endregion #region 沿目标方向的反方向偏离起点阈值距离的点 public xna.Vector3 FanXiangYuZhiDianZuoBiaoHanShu(xna.Vector3 QiDian, xna.Vector3 ZhongDian, float YuZhi) { float QiDianDaoZhongDianHuDu = XiangLiangHuDu(QiDian, ZhongDian); xna.Vector3 yuzhidianzuobaio = new xna.Vector3((float)(QiDian.X - YuZhi * Math.Cos(QiDianDaoZhongDianHuDu)), 0, (float)(QiDian.Z - YuZhi * Math.Sin(QiDianDaoZhongDianHuDu))); return yuzhidianzuobaio; } #endregion #region 最佳进球点函数 public xna.Vector3 ZuiJiaJinQiuDianHanShu(Mission mission, int teamId, xna.Vector3 QiuDong, int i) { RoboFish Yu = mission.TeamsRef[teamId].Fishes[0];//鱼对象 xna.Vector3 ZuiJiaJinQiuDian = new xna.Vector3(); Ball Qiu = mission.EnvRef.Balls[i];//球对象 float QiuDaoQiuMenXiangLiangHuDu = XiangLiangHuDu(Qiu.PositionMm, QiuDong); double ZuiJiaJinQiuDianDaoQiuMenJuLi = 75 / (Math.Abs(Math.Cos(QiuDaoQiuMenXiangLiangHuDu))); ZuiJiaJinQiuDian = ZhengXiangYuZhiDianZuoBiaoHanShu(QiuDong, Qiu.PositionMm, (float)ZuiJiaJinQiuDianDaoQiuMenJuLi); return ZuiJiaJinQiuDian; } #endregion #region 进洞函数 public void JinDongHanShu(Mission mission, int teamId, int i) { RoboFish Yu = mission.TeamsRef[teamId].Fishes[0];//鱼对象 xna.Vector3 MuBiaoDian = mission.EnvRef.Balls[i].PositionMm; // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向） double ZhongJianFangXiang = xna.MathHelper.ToRadians((float)GetAngleDegree(MuBiaoDian - Yu.PositionMm)); // 中间方向与鱼体方向的差值（目标角度） double ChaJiao = ZhongJianFangXiang - Yu.BodyDirectionRad; // 将目标角度规范化到(-PI,PI] // 规范化之后目标角度为正，表示目标方向在鱼体方向右边 // 规范化之后目标角度为负，表示目标方向在鱼体方向左边 if (ChaJiao > Math.PI) {// 中间方向为正鱼体方向为负才可能目标角度大于PI ChaJiao -= 2 * Math.PI; // 规范化到(-PI,0) } else if (ChaJiao < -Math.PI) {// 中间方向为负鱼体方向为正才可能目标角度小于-PI ChaJiao += 2 * Math.PI; // 规范化到(0,PI) } float angThreshold = 5; if (Math.Abs(ChaJiao) > angThreshold * Math.PI / 180.0) {// 目标角度绝对值超过某一阈值（默认30度）速度档位置次低进行小半径转弯。防止控制率过大。 decisions[0].VCode = 3; decisions[0].TCode = (ChaJiao <= 0) ? 1 : 13; } else { decisions[0].VCode = 5; decisions[0].TCode = 7; } } #endregion #region 带球函数 public void DaiQiu(Mission mission, int teamId, xna.Vector3 MuBiaoDianZuoBiao, float MuBiaoFangXiangHuDu, float JiaoDuChaYuZhi1, float JiaoDuChaYuZhi2, float JuLiYuZhi, int SuDu1, int SuDu2, int cycles, int msPerCycle, bool YuZhongXin_YuTou, int ZuiDaSuDuDangWei) { RoboFish Yu = mission.TeamsRef[teamId].Fishes[0];//鱼对象 // 调节所用周期数及每周期毫秒数转换得到秒数 double seconds1 = 15 * msPerCycle / 1000.0; double seconds2 = cycles * msPerCycle / 1000.0; // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点） xna.Vector3 YuZuoBiao = (YuZhongXin_YuTou == true) ? Yu.PositionMm : Yu.PolygonVertices[0]; // 起始点到目标点的距离（目标距离） double MuBiaoJuLi = Math.Sqrt(Math.Pow(MuBiaoDianZuoBiao.X - YuZuoBiao.X, 2.0) + Math.Pow(MuBiaoDianZuoBiao.Z - YuZuoBiao.Z, 2.0)); // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向） double YuDaoMuBiaoDianXiangLiangHuDu = xna.MathHelper.ToRadians((float)GetAngleDegree(MuBiaoDianZuoBiao - Yu.PositionMm)); if (MuBiaoJuLi < 30) {// 起始点到目标点距离小于阈值（默认58毫米）将中间方向调为目标方向 YuDaoMuBiaoDianXiangLiangHuDu = MuBiaoFangXiangHuDu; } // 中间方向与鱼体方向的差值（目标角度） double MuBiaoJiaoDu = YuDaoMuBiaoDianXiangLiangHuDu - Yu.BodyDirectionRad; // 将目标角度规范化到(-PI,PI] // 规范化之后目标角度为正表示目标方向在鱼体方向右边 // 规范化之后目标角度为负表示目标方向在鱼体方向左边 if (MuBiaoJiaoDu > Math.PI) {// 中间方向为正鱼体方向为负才可能目标角度大于PI MuBiaoJiaoDu -= 2 * Math.PI; // 规范化到(-PI,0) } else if (MuBiaoJiaoDu < -Math.PI) {// 中间方向为负鱼体方向为正才可能目标角度小于-PI MuBiaoJiaoDu += 2 * Math.PI; // 规范化到(0,PI) } // 最大角速度取左转和右转最大角速度绝对值的均值 float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0]) + Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2; // 以最大角速度转过目标角度所需的预计时间（角度预计时间） double estimatedT