/// <summary>
/// Calculates a transformation matrix to transform grid coordinates to world coordinates.
/// </summary>
/// <param name="blocks"></param>
/// <returns></returns>
public static VRageMath.Matrix toWorld(List <IMyCubeBlock> blocks)
{
if (blocks == null)
{
throw new Exception("The block list is null");
}
if (blocks.Count < 3)
{
throw new Exception("Need at least 3 blocks.");
}
IMyCubeBlock origin = blocks[0];
VRageMath.Vector3 localCoord = origin.Position;
// first basis vector
VRageMath.Vector3 u = blocks[1].Position - localCoord;
// second basis vector
int vIndex = 2;
VRageMath.Vector3 v = blocks[vIndex].Position - localCoord;
while (u.Dot(v) * u.Dot(v) == u.LengthSquared() * v.LengthSquared() && vIndex < blocks.Count)
{
v = blocks[++vIndex].Position - localCoord;
}
if (u.Dot(v) * u.Dot(v) == u.LengthSquared() + v.LengthSquared())
{
throw new Exception("All blocks are linear dependent => It's not possible to calculate a transformation matrix.");
}
debug.Append("choose: ").Append(u).Append(v).AppendLine();
VRageMath.Matrix localBasis = VRageMath.Matrix.CreateWorld(localCoord, u, v);
VRageMath.Vector3 worldCoord = origin.GetPosition();
// world basis depending on the local bases (same coordinates)
VRageMath.Vector3 ug = blocks[1].GetPosition() - worldCoord;
VRageMath.Vector3 vg = blocks[vIndex].GetPosition() - worldCoord;
VRageMath.Matrix worldBasis = VRageMath.Matrix.CreateWorld(worldCoord, ug, vg);
VRageMath.Matrix inverseLocalBasis;
// if local basis is orthogonal, take the transposed matrix, because then
// the transposed and the inverse matrix are the same and it's obviously
// easier to get the transposed matrix.
if (VRageMath.Vector3.ArePerpendicular(ref u, ref v))
{
inverseLocalBasis = VRageMath.Matrix.Transpose(localBasis);
}
else
{
inverseLocalBasis = VRageMath.Matrix.Invert(localBasis);
}
return(inverseLocalBasis * worldBasis);
}
void Main()
{
// initialize
var blocks = new List <IMyTerminalBlock>();
if (counter == 0)
{
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);
ship = new ShipController(controller);
worldCoord = controller.GetPosition();
debug.Append("POSITION = ").Append(worldCoord).AppendLine();
Debug(debug.ToString());
debug.Clear();
counter++;
return;
}
worldCoord = new VRageMath.Vector3D(0, 0, 0);
bool orthogonal = perpBlocks.Count == 3;
VRageMath.Matrix toWorld = orthogonal ? Utils.toWorld(GridTerminalSystem.Blocks) : Utils.toWorld(perpBlocks);
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.Position = ").Append(VRageMath.Vector3I.Round(VRageMath.Vector3.Transform(controller.GetPosition(), 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 * 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();
debug.Append("origin worldController = ").Append(VRageMath.Vector3I.Round(VRageMath.Vector3.Transform(new VRageMath.Vector3(), worldController))).AppendLine();
//VRageMath.Vector3 n = orthogonal ? worldController.Right : worldController.Up.Cross(worldController.Backward);
//VRageMath.Vector3 projDir = worldDir - worldDir.Dot(n) / n.Dot(n) * n;
//if (projDir.LengthSquared() > 0)
// projDir.Normalize();
//VRageMath.Vector3 eY = worldController.Up;
//eY.Normalize();
//VRageMath.Vector3 eZ = worldController.Backward;
//eZ.Normalize();
//float cosinePhiY = eY.Dot(projDir);
//float cosinePhiZ = eZ.Dot(projDir);
//float pitch = (float)(cosinePhiY > 0 ? -Math.Acos(cosinePhiZ) : Math.Acos(cosinePhiZ));
////VRageMath.Matrix.AlignRotationToAxes();
//debug.Append("pitch = ").Append(pitch).AppendLine();
debug.Append("worldController.IsRotation() = ").Append(worldController.IsRotation());
VRageMath.Matrix toAlign = VRageMath.Matrix.CreateFromDir(worldDir, worldController.Up);
VRageMath.Matrix align = VRageMath.Matrix.AlignRotationToAxes(ref toAlign, ref worldController);
VRageMath.Vector3 xyz = new VRageMath.Vector3();
VRageMath.Matrix.GetEulerAnglesXYZ(ref align, out xyz);
xyz = 0.1f * xyz;
debug.Append(xyz).AppendLine();
ship.UpdateBlocks(blocks);
ship.Stop();
ship.Rotate(Identity.Left, xyz.GetDim(0));
ship.Rotate(Identity.Down, xyz.GetDim(1));
ship.Rotate(Identity.Forward, xyz.GetDim(2));
Debug(debug.ToString());
debug.Clear();
}
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();
}
