## 太空工程师编程笔记5 - 运动与姿态
### 飞行控制
无人机是游戏里相当有意思一部分，不论是采矿无人机，还是战斗无人机，或者仅仅是货运无人机，都是可以实现的。除去无人机外，程序还可以实现飞船的辅助飞行控制，比如说驱动重力引擎，让重力引擎和普通引擎一样可以随鼠标与方向键控制。
实现这些，都需要通过编程去调整运动和姿态。具体来说，就是感知当前状态并控制加速和旋转。
核心的操作就是控制推子与重力发生器、控制陀螺仪、以及各类坐标转换。
比如说，如果你要让无人机飞向某个坐标或达到某个速度，那么就要获取到当前运动状态，得到和目标的偏差，然后把偏差转换成自身的上下左右前后需要的变化，随后控制六个方向的引擎出力，最终修正当前运动状态至目标状态。当然，这个过程也需要控制陀螺仪以实现正确的旋转和指向。
要说最复杂的地方，大概是一些坐标转换和控制算法吧。块的控制其实很容易。
### 编程
#### 接管飞行控制-引擎
手动进行六向引擎控制，并检测驾驶块的运动指示进行运动。
~~~csharp
public Program()
{
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
public void Main(string argument, UpdateType updateSource)
{
//获取当前玩家所在驾驶舱
List<IMyShipController> cockpits=new List<IMyShipController>();
GridTerminalSystem.GetBlocksOfType<IMyShipController>(cockpits,Controller=>Controller.IsUnderControl);
if(cockpits.Count<1)return;
IMyShipController cockpit=cockpits[0];
Vector3D targetThrustPower=new Vector3D(cockpit.MoveIndicator.X*100f,cockpit.MoveIndicator.Y*100f,cockpit.MoveIndicator.Z*100f);
//自动减速(简单)
//转换为相对速度（坐标系转换）
MatrixD refLookAtMatrix =MatrixD.CreateLookAt(Vector3D.Zero,cockpit.WorldMatrix.Forward,cockpit.WorldMatrix.Up);
if(cockpit.DampenersOverride&&targetThrustPower==Vector3D.Zero)targetThrustPower=-Vector3D.TransformNormal(cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
cockpit.IsMainCockpit=false;
log("引擎控制接管中...\n"+targetThrustPower.ToString());
//获取所有引擎
List<IMyThrust> thrusts=new List<IMyThrust>();
GridTerminalSystem.GetBlocksOfType<IMyThrust>(thrusts);
if(thrusts.Count<1)return;
//确定引擎方向
foreach (var thrust in thrusts)
{
//判断引擎喷口对于驾驶舱的相对方向
switch (cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward))
{
//设定推力
case Base6Directions.Direction.Backward:
thrust.CustomName="Backward";
thrust.ThrustOverridePercentage=(targetThrustPower.Z>0)?(float)targetThrustPower.Z:0;
thrust.Enabled=(targetThrustPower.Z>0)?true:false;
break;
case Base6Directions.Direction.Forward:
thrust.CustomName="Forward";
thrust.ThrustOverridePercentage=(targetThrustPower.Z<0)?(float)-targetThrustPower.Z:0;
thrust.Enabled=(targetThrustPower.Z<0)?true:false;
break;
case Base6Directions.Direction.Right:
thrust.CustomName="Right";
thrust.ThrustOverridePercentage=(targetThrustPower.X>0)?(float)targetThrustPower.X:0;
thrust.Enabled=(targetThrustPower.X>0)?true:false;
break;
case Base6Directions.Direction.Left:
thrust.CustomName="Left";
thrust.ThrustOverridePercentage=(targetThrustPower.X<0)?(float)-targetThrustPower.X:0;
thrust.Enabled=(targetThrustPower.X<0)?true:false;
break;
case Base6Directions.Direction.Up:
thrust.CustomName="Up";
thrust.ThrustOverridePercentage=(targetThrustPower.Y>0)?(float)targetThrustPower.Y:0;
thrust.Enabled=(targetThrustPower.Y>0)?true:false;
break;
case Base6Directions.Direction.Down:
thrust.CustomName="Down";
thrust.ThrustOverridePercentage=(targetThrustPower.Y<0)?(float)-targetThrustPower.Y:0;
thrust.Enabled=(targetThrustPower.Y<0)?true:false;
break;
}
}
}
private void log(string log){
IMyTextPanel debug=(IMyTextPanel)GridTerminalSystem.GetBlockWithName("debug");
if(debug==null)Echo(log);
else {
debug.ShowPublicTextOnScreen();
debug.WritePublicText(log);
}
}
~~~
**执行结果**
程序接管并模拟实现了通常的推进器控制。
这里面主要要注意是，在游戏中，右上后是正方向，左下前是负方向，虽然这就是三维坐标系，但有时相当诡异。
#### 接管飞行控制-旋转
手动进行三向旋转控制，并根据驾驶舱和旋转速度进行调整。
不得不说，要真正搞明白陀螺仪的控制是需要一些数学基础，要熟悉Pitch/Yaw/Roll这种旋转坐标系，并且要熟悉坐标变换。不过变换的基本思路很简单，将驾驶舱的旋转参数变换飞船的旋转参数，再变换成陀螺仪的旋转参数。
~~~csharp
public Program()
{
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
public void Main(string argument, UpdateType updateSource)
{
//获取当前玩家所在驾驶舱
List<IMyShipController> cockpits=new List<IMyShipController>();
GridTerminalSystem.GetBlocksOfType<IMyShipController>(cockpits,Controller=>Controller.IsUnderControl);
if(cockpits.Count<1)return;
IMyShipController cockpit=cockpits[0];
cockpit.IsMainCockpit=false;
Vector3 targetPYR=new Vector3(cockpit.RotationIndicator,cockpit.RollIndicator);
log("陀螺仪控制接管中...\n"+targetPYR.ToString());
//转换为飞船旋转参数（坐标系转换）
Matrix cockpitTransform;
cockpit.Orientation.GetMatrix(out cockpitTransform);
Vector3 GridPYR=Vector3.Transform(targetPYR,Matrix.Transpose(cockpitTransform));
//获取所有引擎
List<IMyGyro> gyros=new List<IMyGyro>();
GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyros,block=>block.Enabled);
if(gyros.Count<1)return;
foreach (var gyro in gyros)
{
//转换为陀螺仪旋转参数
Matrix gyroTransform;
gyro.Orientation.GetMatrix(out gyroTransform);
Vector3 gyroPYR=Vector3.Transform(GridPYR,Matrix.Invert(gyroTransform));
gyro.Pitch=gyroPYR.X;
gyro.Yaw=gyroPYR.Y;
gyro.Roll=gyroPYR.Z;
}
}
private void log(string log){
IMyTextPanel debug=(IMyTextPanel)GridTerminalSystem.GetBlockWithName("debug");
if(debug==null)Echo(log);
else {
debug.ShowPublicTextOnScreen();
debug.WritePublicText(log);
}
}
~~~
**执行结果**
程序接管了并模拟实现了通常的旋转控制。
#### 含重力引擎的飞控程序（基本型）
重力引擎的操作和一般的推进器无二，只是没有被接入游戏内飞控而已。
写这个飞控用到了C#的拓展方法语法，目的是让代码更加可读。
~~~csharp
public Program()
{
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
public void Main(string argument, UpdateType updateSource)
{
//获取当前玩家所在驾驶舱
List<IMyShipController> cockpits=new List<IMyShipController>();
GridTerminalSystem.GetBlocksOfType<IMyShipController>(cockpits,Controller=>Controller.IsUnderControl);
if(cockpits.Count<1)return;
IMyShipController cockpit=cockpits[0];
//获取运动参数
cockpit.IsMainCockpit=false;
Vector3 targetPYR=cockpit.PYRIndicator();
Vector3 targetThrustPower=new Vector3D(cockpit.MoveIndicator.X*100f,cockpit.MoveIndicator.Y*100f,cockpit.MoveIndicator.Z*100f);
MatrixD refLookAtMatrix =MatrixD.CreateLookAt(Vector3D.Zero,cockpit.WorldMatrix.Forward,cockpit.WorldMatrix.Up);
//线速度减速（简单）
if(cockpit.DampenersOverride&&targetThrustPower==Vector3D.Zero)targetThrustPower=-Vector3D.TransformNormal(cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
//角速度减速（简单）
targetPYR+=Vector3D.TransformNormal(cockpit.GetShipVelocities().AngularVelocity,refLookAtMatrix)*0.5;
log("飞行控制接管中...\n"+targetPYR.ToString()+"\n"+Vector3D.TransformNormal(cockpit.GetShipVelocities().AngularVelocity,refLookAtMatrix));
//转换为飞船运动参数（坐标系转换）
Matrix cockpitTransform;
cockpit.Orientation.GetMatrix(out cockpitTransform);
Vector3 GridPYR=Vector3.Transform(targetPYR,cockpitTransform);
Vector3 GridOutput=Vector3.Transform(targetThrustPower,cockpitTransform);
//获取并设置推进器和陀螺仪
List<IMyGyro> gyros=new List<IMyGyro>();
GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyros,block=>block.Enabled);
gyros.setPYR(GridPYR);
List<IMyThrust> thrusts=new List<IMyThrust>();
GridTerminalSystem.GetBlocksOfType<IMyThrust>(thrusts);
thrusts.setOutput(GridOutput);
//重力引擎
List<IMyGravityGenerator> gravities=new List<IMyGravityGenerator>();
GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(gravities);
gravities.setOutput(GridOutput);
}
private void log(string log){
IMyTextPanel debug=(IMyTextPanel)GridTerminalSystem.GetBlockWithName("debug");
if(debug==null)Echo(log);
else {
debug.ShowPublicTextOnScreen();
debug.WritePublicText(log);
}
}
}
public static class Utils{
public static Vector3 PYRIndicator(this IMyShipController controller){
return new Vector3(controller.RotationIndicator,controller.RollIndicator);
}
public static void setPYR(this List<IMyGyro> gyros,Vector3 GridPYR){
foreach (var gyro in gyros)
{
//转换为陀螺仪旋转参数
Matrix gyroTransform;
gyro.Orientation.GetMatrix(out gyroTransform);
Vector3 gyroPYR=Vector3.Transform(GridPYR,Matrix.Invert(gyroTransform));
gyro.Pitch=gyro.Pitch*0.5f+gyroPYR.X;
gyro.Yaw=gyro.Yaw*0.5f+gyroPYR.Y;
gyro.Roll=gyro.Roll*0.5f+gyroPYR.Z;
}
}
public static void setOutput(this List<IMyThrust> thursts,Vector3 GridOutput){
foreach (var thrust in thursts)
{
//转换为陀螺仪旋转参数
Matrix gyroTransform;
thrust.Orientation.GetMatrix(out gyroTransform);
Vector3 thrustOutput=Vector3.Transform(GridOutput,Matrix.Invert(gyroTransform));
thrust.ThrustOverridePercentage=(thrustOutput.Z>0)?(float)thrustOutput.Z:0;
thrust.Enabled=(thrustOutput.Z>0)?true:false;
}
}
public static void setOutput(this List<IMyGravityGenerator> gravities,Vector3 GridOutput){
foreach (var gravity in gravities)
{
//转换为陀螺仪旋转参数
Matrix gyroTransform;
gravity.Orientation.GetMatrix(out gyroTransform);
Vector3 thrustOutput=Vector3.Transform(GridOutput,Matrix.Invert(gyroTransform));
gravity.GravityAcceleration=-(float)thrustOutput.Y;
gravity.Enabled=(Math.Abs(thrustOutput.Y)>0.01f)?true:false;
}
}
~~~
**执行结果**
重力引擎被接管成为飞行控制的一部分，可以随玩家操作而动。
引擎并不负责人工质量块的开关，可以通过开关人工质量块来开关引擎。
### 参考
#### 关于人工重力块
部分API不在Sandbox.Common.dll里，而在SpaceEngineers.Game.dll里
~~~csharp
public interface IMyGravityGeneratorBase : IMyFunctionalBlock, IMyTerminalBlock, IMyCubeBlock, IMyEntity
{
// Properties
[Obsolete("Use GravityAcceleration.")]
float Gravity { get; }
float GravityAcceleration { get; set; }
}
public interface IMyGravityGenerator : IMyGravityGeneratorBase, IMyFunctionalBlock, IMyTerminalBlock, IMyCubeBlock, IMyEntity
{
// Properties
[Obsolete("Use FieldSize.X")]
float FieldWidth { get; }
[Obsolete("Use FieldSize.Y")]
float FieldHeight { get; }
[Obsolete("Use FieldSize.Z")]
float FieldDepth { get; }
Vector3 FieldSize { get; set; }
}
public interface IMySpaceBall : IMyVirtualMass, IMyFunctionalBlock, IMyTerminalBlock, IMyCubeBlock, IMyEntity
{
// Properties
float Friction { get; set; }
float Restitution { get; set; }
[Obsolete("Use IMySpaceBall.Broadcasting")]
bool IsBroadcasting { get; }
bool Broadcasting { get; set; }
float VirtualMass { get; set; }
}
~~~
#### 关于陀螺仪
~~~csharp
public interface IMyGyro : IMyFunctionalBlock, IMyTerminalBlock, IMyCubeBlock, IMyEntity
{
// Properties
float GyroPower { get; set; }
bool GyroOverride { get; set; }
float Yaw { get; set; }
float Pitch { get; set; }
float Roll { get; set; }
}
~~~
#### 关于推进器
~~~csharp
public interface IMyThrust : IMyFunctionalBlock, IMyTerminalBlock, IMyCubeBlock, IMyEntity
{
// Properties
float ThrustOverride { get; set; }
float ThrustOverridePercentage { get; set; }
float MaxThrust { get; }
float MaxEffectiveThrust { get; }
float CurrentThrust { get; }
Vector3I GridThrustDirection { get; }
}
~~~
- 序言
- 写在前面的话
- 太空工程师
- MEA小组
- 一、入门
- 1.1 基础概念
- 1.2 编程工具
- 1.3 变量
- 1.4 函数 Function
- 1.5 基本语法
- 1.5.1 运算符
- 1.5.2 if
- 1.5.3 for
- 1.5.4 其他语法
- 1.3 类 Class
- 二、编程块
- 2.1 方块的概念
- 2.2 List<T>结构
- 2.3 获取方块
- 2.4 方块的使用
- 三、Ship 类
- 3.1 简介
- Ship v0.5
- 代码
- 手册（待更新）
- 例子（待更新）
- Ship v1.0
- 代码
- 例子
- 文档
- 实例化
- 内置变量
- 内置方法
- Target类
- 四、运动控制算法在SE中的应用
- 4.1 运动控制介绍
- 4.2 过程控制
- 4.3 震荡和动态误差
- 4.4 误差累加方案
- 4.5 PID算法
- 4.6 对PID算法的一点点简化
- 4.7 一阶惯性系统的PID算法优化的研究
- 五、MEA方块类
- 5.0 核心代码目录
- v1.0核心代码
- v1.1 核心代码
- v2.0 核心代码
- 5.1 类的概念
- 5.2 MEA的方块类（Block）
- 5.3 方块类文档
- 5.4 方块类2.0 全教程
- 5.4.1 安装和使用
- 5.4.2 方块类（Block）
- 5.4.3 显示屏类（DisplayScreen）
- 5.4.4 LCD类（LCD）
- 5.4.5 主控座椅类（Cockpit）
- 六、疯猴的编程笔记
- 第一个程序
- 获取和控制其他块
- 物流与生产
- 界面与通信
- 运动与姿态
- 侦测与导航
- 七、SteamZhou的笔记
- 有趣而花里胡哨的IDEA
- 八、质子对撞炮的笔记
- 属性 Property
- 接口 interface