如题求助!想实现以下功能：控制飞船上所以在编组“turrets”的炮塔和自定义炮塔，将所有炮塔的弹道交于一点，可以通过指令“forward”和“back”控制焦点的远近，每当运行“forward”或“back”时，焦点向前或向后移动500m，当运行“lock”指令后，控制编组为“camera”的摄像头自动扫描当前方向，在扫描到敌方飞船后将炮塔的焦点对焦到敌方飞船上。

代码部分:
// ---- 获取方块设置----
const string TurretGroupName = "turrets"; //炮塔编组名
const string CameraGroupName = "camera"; //摄像头编组名
const string DesignatorName = "designator"; //指示器炮塔名
const double ProjectileVelocity = 400; //弹丸速度
const double DefaultFocusDistance = 1000; //默认焦点距离
// ---- 全局变量 ----
List<IMyLargeTurretBase> turrets = new List<IMyLargeTurretBase>(); //炮塔列表
List<IMyCameraBlock> cameras = new List<IMyCameraBlock>(); //摄像头列表
IMyLargeTurretBase designator; //指示器炮塔
double focusDistance; //焦点距离
bool locked; //是否锁定目标
bool init = false; //是否初始化
void Main(string argument)
{
if (!init) //初始化
{
GetBlocks(); //获取方块
focusDistance = DefaultFocusDistance; //设置默认焦点距离
locked = false; //设置锁定状态为否
init = true;
}
switch (argument.ToLower()) //根据指令执行操作
{
case "forward": //向前移动焦点
focusDistance += 500;
locked = false;
break;
case "back": //向后移动焦点
focusDistance -= 500;
locked = false;
break;
case "lock": //锁定目标
ScanTarget(); //扫描目标
locked = true;
break;
default: //无效指令
Echo("Invalid argument");
break;
}
AimTurrets(); //瞄准炮塔
Echo($"Focus distance: {focusDistance} m"); //显示焦点距离
Echo($"Locked: {locked}"); //显示锁定状态
}
void GetBlocks() //获取方块
{
IMyGridTerminalSystem gridTerminalSystem = Me.GridTerminalSystem; //获取网格终端系统接口
gridTerminalSystem.GetBlockGroupWithName(TurretGroupName)?.GetBlocksOfType(turrets); //获取炮塔编组中的所有炮塔
gridTerminalSystem.GetBlockGroupWithName(CameraGroupName)?.GetBlocksOfType(cameras); //获取摄像头编组中的所有摄像头
designator = gridTerminalSystem.GetBlockWithName(DesignatorName) as IMyLargeTurretBase; //获取指示器炮塔
if (turrets.Count == 0) Echo("No turrets found"); //如果没有找到炮塔，显示警告信息
if (cameras.Count == 0) Echo("No cameras found"); //如果没有找到摄像头，显示警告信息
if (designator == null) Echo("No designator found"); //如果没有找到指示器炮塔，显示警告信息
}
void ScanTarget() //扫描目标
{
foreach (var camera in cameras) //遍历所有摄像头
{
if (camera.CanScan(focusDistance)) //如果摄像头可以扫描到焦点距离
{
var target = camera.Raycast(focusDistance); //进行射线扫描
if (target.Type == MyDetectedEntityType.LargeGrid || target.Type == MyDetectedEntityType.SmallGrid) //如果扫描到敌方飞船
{
focusDistance = target.Distance; //更新焦点距离为目标距离
break; //跳出循环
}
}
}
}
void AimTurrets() //瞄准炮塔
{
Vector3D focusPoint; //焦点位置
if (locked && designator.HasTarget) //如果锁定目标并且指示器炮塔有目标
{
focusPoint = designator.GetTargetedEntity().Position; //获取指示器炮塔的目标位置作为焦点位置
}
else //否则
{
focusPoint = Me.GetPosition() + Me.WorldMatrix.Forward * focusDistance; //获取编程块前方的焦点距离处的位置作为焦点位置
}
foreach (var turret in turrets) //遍历所有炮塔
{
var elevationRotor = turret.GetElevationRotor(); //获取仰角转子
var azimuthRotor = turret.GetAzimuthRotor(); //获取方位角转子
if (elevationRotor != null && azimuthRotor != null) //如果转子都存在
{
var turretDirection = turret.WorldMatrix.Forward; //获取炮塔的朝向
var turretPosition = turret.GetPosition(); //获取炮塔的位置
var targetDirection = Vector3D.Normalize(focusPoint - turretPosition); //获取目标方向
var angleToTarget = Vector3D.AngleBetween(turretDirection, targetDirection); //获取炮塔朝向和目标方向的夹角
if (angleToTarget < MathHelper.ToRadians(0.1)) continue; //如果夹角小于0.1度，跳过本次循环
var elevationAxis = azimuthRotor.WorldMatrix.Up; //获取仰角转子的旋转轴
var azimuthAxis = elevationRotor.WorldMatrix.Up; //获取方位角转子的旋转轴
var elevationAngle = GetAngleBetween(targetDirection, turretDirection, elevationAxis); //获取仰角转子需要旋转的角度
var azimuthAngle = GetAngleBetween(targetDirection, turretDirection, azimuthAxis); //获取方位角转子需要旋转的角度
elevationRotor.TargetVelocityRPM = (float)elevationAngle * 60 / (float)Math.PI; //设置仰角转子的目标速度
azimuthRotor.TargetVelocityRPM = (float)azimuthAngle * 60 / (float)Math.PI; //设置方位角转子的目标速度
}
}
}
double GetAngleBetween(Vector3D a, Vector3D b, Vector3D axis) //计算两个向量在指定轴上的夹角，返回弧度值，范围为-pi到pi
{
a = Vector3D.Reject(a, axis); //将a向量投影到垂直于轴的平面上
b = Vector3D.Reject(b, axis); //将b向量投影到垂直于轴的平面上
if (Vector3D.IsZero(a) || Vector3D.IsZero(b)) return 0; //如果有任何一个向量为零向量，返回0
a = Vector3D.Normalize(a); //将a向量归一化
b = Vector3D.Normalize(b); //将b向量归一化
double dot = MathHelper.Clamp(Vector3D.Dot(a, b), -1, 1); //计算a和b的点积，并限制在-1到1之间，防止出现NaN
double cross = Vector3D.Dot(Vector3D.Cross(a, b), axis); //计算a和b的叉积，并点乘轴向量，得到叉积在轴上的分量
double sign = Math.Sign(cross); //根据叉积的符号判断夹角的正负，如果叉积为零，返回0
double angle = sign * Math.Acos(dot); //根据点积和符号计算夹角，返回弧度值
return angle; //返回夹角
}