void Main()
{
// initialize
var blocks = new List <IMyTerminalBlock>();
GridTerminalSystem.GetBlocksOfType <IMyShipController>(blocks, FilterShipController);
if (blocks.Count == 0)
{
throw new Exception("Did not find any cockpit.");
}
controller = blocks[0] as IMyShipController;
debug.Append("use ").Append(controller.CustomName).Append(':').AppendLine();
perpBlocks = Utils.FindPerpendicularTo(controller);
for (int i = 0; i < perpBlocks.Count; ++i)
{
var block = perpBlocks[i];
debug.Append(block.Position).AppendLine();
}
VRageMath.Vector3 cur = perpBlocks[1].Position - perpBlocks[0].Position;
VRageMath.Vector3 next = perpBlocks[2].Position - perpBlocks[0].Position;
debug.Append(VRageMath.Vector3.ArePerpendicular(ref cur, ref next)).AppendLine();
worldCoord = new VRageMath.Vector3D(0, 0, 0);
bool orthogonal = perpBlocks.Count == 3;
VRageMath.Matrix toWorld = orthogonal ? Utils.toWorld(perpBlocks) : Utils.toWorld(GridTerminalSystem.Blocks);
VRageMath.Vector3 r = toWorld.Right;
VRageMath.Vector3 u = toWorld.Up;
VRageMath.Vector3 b = toWorld.Backward;
debug.Append(r.Dot(u)).AppendLine();
debug.Append(u.Dot(b)).AppendLine();
debug.Append(b.Dot(r)).AppendLine();
debug.Append(VRageMath.Vector3.ArePerpendicular(ref r, ref u)).AppendLine();
debug.Append(VRageMath.Vector3.ArePerpendicular(ref u, ref b)).AppendLine();
debug.Append(VRageMath.Vector3.ArePerpendicular(ref b, ref r)).AppendLine();
blocks = new List <IMyTerminalBlock>();
GridTerminalSystem.GetBlocksOfType <IMyThrust>(blocks);
GridTerminalSystem.GetBlocksOfType <IMyGyro>(blocks);
debug.Append("worldCoord = ").Append(VRageMath.Vector3I.Round(worldCoord)).AppendLine();
debug.Append("controller.GetPosition() = ").Append(VRageMath.Vector3I.Round(controller.GetPosition())).AppendLine();
debug.Append("controller.Position = ").Append(controller.Position).AppendLine();
debug.Append("transfrom controller.Position = ").Append(VRageMath.Vector3I.Round(VRageMath.Vector3.Transform(controller.Position, toWorld))).AppendLine();
debug.Append("transfrom controller.GetPosition() = ").Append(VRageMath.Vector3I.Round(VRageMath.Vector3.Transform(controller.GetPosition(), VRageMath.Matrix.Invert(toWorld)))).AppendLine();
debug.Append("transfrom zero = ").Append(VRageMath.Vector3I.Round(VRageMath.Vector3.Transform(worldCoord, VRageMath.Matrix.Invert(toWorld)))).AppendLine();
VRageMath.Vector3 worldDir = worldCoord - controller.GetPosition();
float distance = worldDir.LengthSquared() > 0 ? worldDir.Normalize() : 0;
debug.Append("distance = ").Append(distance).AppendLine();
debug.Append("direction = ").Append(worldDir).AppendLine();
VRageMath.Matrix worldController = new VRageMath.Matrix();
controller.Orientation.GetMatrix(out worldController);
worldController = worldController * VRageMath.Matrix.CreateTranslation(controller.Position) * toWorld;
debug.Append("worldController = ").AppendLine();
debug.Append(worldController.Right).AppendLine();
debug.Append(worldController.Up).AppendLine();
debug.Append(worldController.Backward).AppendLine();
debug.Append(worldController.Translation).AppendLine();
VRageMath.Vector3 a = worldController.Forward;
VRageMath.Matrix rotation = Utils.CalculateRotation(ref worldDir, ref a);
debug.Append((double)Math.Abs(rotation.Right.Dot(rotation.Up))).AppendLine();
debug.Append((double)Math.Abs(rotation.Right.Dot(rotation.Backward))).AppendLine();
debug.Append((double)Math.Abs(rotation.Up.Dot(rotation.Backward))).AppendLine();
debug.Append("rotation transl+persp = ").Append(rotation.HasNoTranslationOrPerspective()).AppendLine();
debug.Append("rotation rotation = ").Append(rotation.IsRotation()).AppendLine();
debug.Append(rotation.Right).AppendLine();
debug.Append(rotation.Up).AppendLine();
debug.Append(rotation.Backward).AppendLine();
debug.Append(rotation.Translation).AppendLine();
VRageMath.Vector3 xyz = new VRageMath.Vector3();
VRageMath.Matrix.GetEulerAnglesXYZ(ref rotation, out xyz);
debug.Append("X = ").Append(xyz.GetDim(0)).AppendLine();
debug.Append("Y = ").Append(xyz.GetDim(1)).AppendLine();
debug.Append("Z = ").Append(xyz.GetDim(2)).AppendLine();
Debug(debug.ToString());
debug.Clear();
}
