public static void CalculateRotation(ref VRageMath.Vector3 a, ref VRageMath.Vector3 b, out VRageMath.Matrix rotation)
{
if (!VRageMath.Vector3.IsUnit(ref a))
{
a.Normalize();
}
if (!VRageMath.Vector3.IsUnit(ref b))
{
b.Normalize();
}
VRageMath.Vector3 v = a.Cross(b);
float s = v.Length(); // sine
float c = a.Dot(b); // cosine
VRageMath.Matrix cross = Utils.CreateSkewSymmetricMatrix(v);
rotation = Identity + cross + cross * cross * (1 - c) / s;
}
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();
}