/// <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); }
/// <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; // TODO use an epsilon value instead of 0, because of the precision error of floating point multiplication. while (u.Dot(v) == 0 && vIndex < blocks.Count) { v = blocks[++vIndex].Position - localCoord; } if (u.Dot(v) == 0) { throw new Exception("All blocks are linear dependent => It's not possible to calculate a transformation matrix."); } 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); }
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>(); 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(); }