Welcome to Keen Software House Forums! Log in or Sign up to interact with the KSH community.
  1. You are currently browsing our forum as a guest. Create your own forum account to access all forum functionality.

[Library] Grid to World coordinates

Discussion in 'Programming Released Codes' started by JoeTheDestroyer, Feb 6, 2015.

Thread Status:
This last post in this thread was made more than 31 days old.
  1. JoeTheDestroyer Junior Engineer

    Edit, Jul 7, 2015:

    Due to additions to the API, this library is now obsolete and shouldn't be used in new projects. Instead, you can find below two small functions that accomplish the same thing.


    I thought I would share the world-grid coordinate conversion library I wrote as part of the auto-miner I'm working on. If anybody can make use of it feel free...

    Hopefully the comments in the code and examples make the usage clear enough.

    Also, it seems a little big but there are a lot of helper functions that can be removed if not needed. Any of the transform*, reverse* and get* functions plus all the properties are optional.

    Edit, Feb 22,2015:
    Changed interface to use all double types for increased precision.

    //Name: Grid To World Coordinates Lib
    //Version: 2
    //Changes from v1: Changed interface to use all doubles for better precision
    //Author: Joe Barker
    //License: Public Domain
    /// <summary>
    /// This class finds the orientation/offset of a grid in world space.
    /// </summary>
    public class GridToWorldCoordinates
        ///<summary>Matrix transforming from grid-space to world-space</summary>
        private MatrixD trans;
        ///<summary>Matrix transforming from world-space to grid-space</summary>
        private MatrixD invTrans;
        ///<summary>3 selected blocks to use as reference points</summary>
        private IMyCubeBlock[] refPts;
        ///<summary>Local half of the rotation matrix, precalculated because it doesn't change.</summary>
        private MatrixD refBasis;
        ///<summary>Size (in meters) of a cube on this grid.</summary>
        private double cubeSize;
        ///<summary>Build a coordinate system for the provided grid</summary>
        public GridToWorldCoordinates(IMyGridTerminalSystem GridTerminalSystem, IMyCubeGrid grid)
            trans=new MatrixD(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);//Matrix.Identity;
            //Find all blocks on the target grid
            var l = new List<IMyTerminalBlock>();
            GridTerminalSystem.GetBlocksOfType<IMyTerminalBlock>(l,delegate(IMyTerminalBlock blk){
                return blk.CubeGrid==grid;
            if(refPts==null) return;
        ///<summary>True if valid conversion found, false if no 3 non-collinear reference points found</summary>
        public bool isValid()
        {   return refPts!=null; }
        ///<summary>Update conversion for the new position,orientation of the grid.</summary>
        public void update()
            //Find an orthonormal basis from the reference points
            //in _world_ coordinates
            Vector3D q1=refPts[0].GetPosition();
            Vector3D q2=refPts[1].GetPosition();
            Vector3D q3=refPts[2].GetPosition();
            //Work from directions
            Vector3D u1=Vector3D.Normalize(q2-q1);
            Vector3D u2=Vector3D.Normalize(q3-q1);
            //Orthoganalize and normalize
            //Vector3D uo1=u1;
            Vector3D uo3=Vector3D.Normalize(u1.Cross(u2));
            Vector3D uo2=Vector3D.Normalize(uo3.Cross(u1));
            trans=new MatrixD(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
            //Rotate from grid coord to world coord
            //Find origin
            Vector3D p1=center(refPts[0])*cubeSize;
            Vector3D origin=q1-Vector3D.Transform(p1,trans);
        ///<summary>Transform grid-coordinates to world-coordinates</summary>
        public Vector3D transform(Vector3D gridCoord)
        { return Vector3D.Transform(cubeSize*gridCoord,ref trans); }
        ///<summary>Transform world-coordinates to grid-coordinates</summary>
        public Vector3D reverse(Vector3D worldCoord)
        { return Vector3D.Transform(worldCoord,ref invTrans)/cubeSize; }
        ///<summary>Transform direction in grid-space to world-space</summary>
        public Vector3D transformDir(Vector3D gridDir)
        { return Vector3D.Transform(gridDir,trans.GetOrientation()); }
        ///<summary>Transform direction in world-space to grid-space</summary>
        public Vector3D reverseDir(Vector3D worldDir)
        { return Vector3D.Transform(worldDir,invTrans.GetOrientation()); }
        ///<summary>World space location for grid-coord (0,0,0)</summary>
        public Vector3D Origin
        { get { return Vector3D.Transform(new Vector3D(0,0,0),ref trans); } }
        ///<summary>Forward (-Z) world-space direction for the grid</summary>
        public Vector3D Forward
        { get { return -trans.GetDirectionVector(Base6Directions.Direction.Backward); } }
        ///<summary>Backward (+Z) world-space direction for the grid</summary>
        public Vector3D Backward
        { get { return trans.GetDirectionVector(Base6Directions.Direction.Backward); } }
        ///<summary>Left (-X) world-space direction for the grid</summary>
        public Vector3D Left
        { get { return -trans.GetDirectionVector(Base6Directions.Direction.Right); } }
        ///<summary>Right (+X) world-space direction for the grid</summary>
        public Vector3D Right
        { get { return trans.GetDirectionVector(Base6Directions.Direction.Right); } }
        ///<summary>Up (+Y) world-space direction for the grid</summary>
        public Vector3D Up
        { get { return trans.GetDirectionVector(Base6Directions.Direction.Up); } }
        ///<summary>Down (-Y) world-space direction for the grid</summary>
        public Vector3D Down
        { get { return -trans.GetDirectionVector(Base6Directions.Direction.Up); } }
        ///<summary>Get the world-space direction for the given block's Forward vector</summary>
        ///<remark>For example, a thruster thrusts in the forward direction and a cockpit faces in the forward direction.</remark>
        public Vector3D getForward(IMyCubeBlock blk)
            Matrix bmat;
            blk.Orientation.GetMatrix(out bmat);
            return transformDir(bmat.Forward);
        ///<summary>Get the world-space direction for the given block's Left vector</summary>
        public Vector3D getLeft(IMyCubeBlock blk)
            Matrix bmat;
            blk.Orientation.GetMatrix(out bmat);
            return transformDir(bmat.Left);
        ///<summary>Get the world-space direction for the given block's Up vector</summary>
        public Vector3D getUp(IMyCubeBlock blk)
        {   // Orientation.Up doesn't work (returns .Forward), so do this instead
            Matrix bmat;
            blk.Orientation.GetMatrix(out bmat);
            return transformDir(bmat.Forward.Cross(bmat.Left));
        ///<summary>Return the grid-space center of the given block.</summary>
        ///<remark>blk.Position isn't always the exact center of the block, this is.</remark>
        public static Vector3D center(IMyCubeBlock blk)
        { return (new Vector3D(blk.Min+blk.Max))/2.0f; }
        private void generateSolver()
            //Find an orthonormal basis from the reference points
            //in grid coordinates
            Vector3D p1=center(refPts[0]);
            Vector3D p2=center(refPts[1]);
            Vector3D p3=center(refPts[2]);
            //Work from directions
            Vector3D v1=Vector3D.Normalize(p2-p1);
            Vector3D v2=Vector3D.Normalize(p3-p1);
            //Orthogonalize and normalize
            //Vector3D vo1=v1;
            Vector3D vo3=Vector3D.Normalize(v1.Cross(v2));
            Vector3D vo2=Vector3D.Normalize(vo3.Cross(v1));
            refBasis=new MatrixD(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
        private static IMyCubeBlock[] refFromList(List<IMyTerminalBlock> l)
        {   //Find 3 non-co-linear points
            for(int i=0;i<l.Count;++i)
                Vector3D pi=center(l[i]);
                for(int j=i+1;j<l.Count;++j)
                    Vector3D pj=center(l[j]);
                    Vector3D vij=Vector3D.Normalize(pj-pi);
                    for(int k=j+1;k<l.Count;++k)
                        Vector3D pk=center(l[k]);
                        Vector3D vik=Vector3D.Normalize(pk-pi);
                            return new IMyCubeBlock[]{ l[i], l[j], l[k] };
            return null;
    // Example
    IMyTimerBlock timer;
    IMyTerminalBlock cockpit;
    IMyTerminalBlock output;
    GridToWorldCoordinates g2w;
    void Main()
            //Find first timer on grid
            var l=new List<IMyTerminalBlock>();
            if(l.Count<1) return;
            timer=l[0] as IMyTimerBlock;
            //Fint first cockpit (to use for directions)
            //Find first beacon on grid (for output)
            g2w=new GridToWorldCoordinates(GridTerminalSystem,timer.CubeGrid);
            g2w.getForward(cockpit),g2w.getUp(cockpit),g2w.getLeft(cockpit) ) );
    // Testing
    void Main()
        var l=new List<IMyTerminalBlock>();
        IMyTerminalBlock output=l[0];
        GridToWorldCoordinates g2w=new GridToWorldCoordinates(GridTerminalSystem,output.CubeGrid);
        var blocks=GridTerminalSystem.Blocks;
        string o="";
        for(int i=0;i<blocks.Count;++i)
            Vector3D wp=blocks[i].GetPosition();
            Vector3D c=GridToWorldCoordinates.center(blocks[i]);
            Vector3D cFwp=g2w.reverse(wp);
            Vector3D wpFc=g2w.transform(c);
            o+=String.Format("{0} ({1}):\n\t.Position={2}\n\tcenter={3}\n\tcent-from-w={4} (err={5})\n\t.GetPosition()={6}\n\tpos-from-g={7} (err={8})\n\tForward={9}\n\tUp={10}\n\tLeft={11}\n",
                blocks[i].CustomName, blocks[i].DefinitionDisplayNameText, blocks[i].Position,
    Last edited: Jul 20, 2015
  2. Belthize Apprentice Engineer

    Very useful and very timely. I was recently thinking about this kind of thing. Definitely tagging for future reference.
  3. Immersive Apprentice Engineer

    Can you use any block for reference, or do they require a specific layout/alignment?
  4. JoeTheDestroyer Junior Engineer

    Any three (terminal) blocks in general position should be fine. Mathematically, this means any three blocks that aren't in a straight line.

    Practically, you could run into numerical precision problems on the extreme ends, so I limit it here:
    To only choose blocks that have atleast a 37 degree and no more than a 143 degree bend between them. (You could move the limit from 0.8 to closer to 1 if you're willing to risk less precision.)

    On a normal size ship, it should be very likely to meet this constraint. (Even just considering thrusters, it is very likely.)

    It may be a problem on a very small (few blocks) ship like a torpedo.
  5. JoeTheDestroyer Junior Engineer

    I have updated the library to use all double types for better precision. I found some new functions to make this possible (whether Keen added them or I just missed them the first time, I don't know.)
  6. fusurugi Junior Engineer

    it would be nice if the script could select blocks neighboring the pblock it's executed on on it's own.
  7. JoeTheDestroyer Junior Engineer

    Unfortunately, there is currently no way to find "the pblock it's executed on." There are kludges that involve searching for a block with a specific name (or part thereof), but these have edge cases where they fail, plus many users have their own preferred naming schemes. Best not to include such things in a generic library.

    Also, I'm not sure I see the utility anyways. First, the user may be interested in a grid other than the one the programmable block is on (e.g, after a rotor or piston). Second, if a user is interested in a grid, they almost certainly have already (or will) lookup blocks on that grid. In which case, they can use those to provide the required CubeGrid reference anyways.

    Unless you mean the selection of the actual reference point blocks? In that case, they are selected automatically. No need for them to be neighboring or anything, as long as there are 3 blocks on the same grid that are not in a straight line.
  8. fusurugi Junior Engineer

    See, that's the issue i was having with this kind of script: I didn't know where or how I define the ref points.
  9. JoeTheDestroyer Junior Engineer

    Rather that not knowing where to place the ref points, I personally just didn't like having to place specific blocks for this purpose, cluttering up my designs. Hence this library.

    To be fair, though, defining specific reference points solves two problems. First, it makes the math easier (2 vector subtractions and normalizations plus a cross product vs. the basis creation and matrix math I have to do).

    Second, it defines what the user considers "forward", "up", etc. Because there is no direct relation between the x,y,z-axis on a ship's grid and what the user considers forward, etc for that ship to be, there is no way for me to automatically get that right for all situations. In most cases, I expect that simply finding the cockpit block of the ship and using getForward(),etc methods will suffice. In other cases, it will be up to the script writer (library user) and/or script user to find an appropriate block to use for reference.
  10. Gloin the Dark Apprentice Engineer

  11. JoeTheDestroyer Junior Engineer

  12. Newton Trainee Engineer

    I'm having trouble using this library, or at least figuring out its use.
    What does "getForward" method return? Is it a unit vector in world coordinate space that points in the object's forward direction?
    If thrusters apply force in "forward" direction (according to the comment in the library), then g2w.getForward(thruster) does not return the said vector according to my tests.

    I created a ship that used a sensor to track the player and calculated the direction vector to him:
    var target = VRageMath.Normalize(player.GetPosition() - g2w.Origin); //g2w is a GridToWorldCoordinates object
    This "target" vector was outputted to a beacon along with "g2w.getForward(thruster), and the vectors were not colinear when I was in front of the thruster. The vectors were colinear when I was to its right side.

    Lets say I want a direction vector in world coordinate space that points in direction in which thruster applies force on the ship. Is it possible to get it with this libaray?
  13. JoeTheDestroyer Junior Engineer


    I just tested and I have it backwards. A thruster points in the Forward direction, but thrusts in the opposite direction.

    I'll update my documentation at some point...

    Your problem is likely the use of Origin. This is the world space location of the CubeGrid origin, which is usually the position of the first block that was placed when building. Depending on how the ship was built, this could be in the middle, on an edge, or even completely outside a ship.

    To get the direction between the player and the thruster, you should do:
    var target = VRageMath.Normalize(player.GetPosition() - thruster.GetPosition()); //g2w is a GridToWorldCoordinates object
    In my test just now, this gives the result you were expecting.
  14. Warixx Trainee Engineer


    I believe this library might be outdated, since we have we the methods GridIntegerToWorld and WorldToGridInteger in IMyCubeGrid and the property Orientation in IMyCubeBlock.


    IMyTerminalBlock someBlock = GridTerminalSy...
    VRageMath.Vector3I someIntVector = new VRageMath.Vector3I( 3, 2, 1);
    VRageMath.Vector3D someDblVector;
    someDblVector = someBlock.CubeGrid.GridIntegerToWorld(someIntVector);
    someIntVector = someBlock.CubeGrid.WorldToGridInteger(someDblVector);
    VRageMath.Matrix orientation;
    (someBlock as IMyCubeBlock).Orientation.GetMatrix(out orientation);
    There is other stuff in VageMath.MyBlockOrientation, e.g .GetQuaternion or .Forward if you prefer these things over matrices.
  15. JoeTheDestroyer Junior Engineer

    This gives the orientation of a block with respect to the grid it is on, not the world space orientation. You'll note how it is used in the getForward() function under discussion.

    But yes, I could update to use those other functions rather than the round-about way that was needed before they were available. Though I hate to mess with code I know works...
  16. Warixx Trainee Engineer

    Last edited: Jun 9, 2015
  17. Newton Trainee Engineer

  18. Lynnux Junior Engineer

    I use this library in my Black Adder craft instead of two cameras with my rangefinder mod because this solution is very robust and therefore better suited for military ships.
    I use the heading information to set up a matrix for the radar display.
    Last edited: Jun 19, 2015
  19. Wicorel Senior Engineer

    I'm trying to use this library to do ship aiming/orientation.

    All the examples of orientate use up/down/left/right/etc blocks and get their positions to to their math.

    1) How do I use this library to avoid having to define specific blocks in specific positions and have a function that modifies gyros to aim the ship?

    2) I don't understand the difference between .Forward and .GetForward. .Forward is a 'direction'... so a vector? And .GetForward says it is a vector.. so I can't be understanding that right..

    They never had this stuff when I was in school :(...
  20. JoeTheDestroyer Junior Engineer

    This is what the library was intended for, I will describe more below.

    However, as was noted above, this library is outdated due to functions added to the API after it's creation. I posted these in another thread, but everything this library does can be accomplished w/ two simple functions now:
    MatrixD GetGrid2WorldTransform(IMyCubeGrid grid)
        Vector3D origin=grid.GridIntegerToWorld(new Vector3I(0,0,0));
        Vector3D plusY=grid.GridIntegerToWorld(new Vector3I(0,1,0))-origin;
        Vector3D plusZ=grid.GridIntegerToWorld(new Vector3I(0,0,1))-origin;
        return MatrixD.CreateScale(grid.GridSize)*MatrixD.CreateWorld(origin,-plusZ,plusY);
    MatrixD GetBlock2WorldTransform(IMyCubeBlock blk)
        Matrix blk2grid;
        blk.Orientation.GetMatrix(out blk2grid);
        return blk2grid*
               MatrixD.CreateTranslation(((Vector3D)new Vector3D(blk.Min+blk.Max))/2.0)*
    void Main(string argument)
        var l=new List<IMyTerminalBlock>();
        var lcd=l[0] as IMyTextPanel;
        for(int i=0;i<l.Count;++i)
            //Calculate error between our world matrix and the game's GetPosition()
            MatrixD g2w=GetGrid2WorldTransform(l[i].CubeGrid);
            Vector3D gridPos=(new Vector3D(l[i].Min+l[i].Max))/2.0; //( .Position is a problem for even size blocks)
            Vector3D calcPos=Vector3D.Transform(gridPos,ref g2w);
            double err=(l[i].GetPosition()-calcPos).Length();
            //Find the world "forward" vector for the block
            MatrixD b2w=GetBlock2WorldTransform(l[i]);
            Vector3D fwd=b2w.Forward;
            fwd.Normalize(); //(Need to normalize because the above matrices are scaled by grid size)
            lcd.WritePublicText(String.Format("{0}: Error={1}\n    fwd={2}\n",l[i].CustomName,err,fwd),true);
    In the example, look under "//Find the world "forward" vector for the block" for the part most relevant to you.

    This part is outside the scope of this library. There are some older threads (a couple months, I think) that discuss this topic in more detail. Basically, this library (and the above functions) only supplies one small part of the solution to that problem.

    They are both directions, and yes, vectors. (Positions for cubegrids and blocks are already available via IMyEntity.GetPosition().)

    The difference between those two (and similarly, the two functions above) is what they are relative to (i.e., their reference frame). In this case, they refer to the cubegrid-local (or just grid-local) coordinate system (.Forward, GetGrid2WorldTransform) and the block-local coordinate system (.GetForward, GetBlock2WorldTransform).

    The thing to keep in mind is that the grid-local coordinate system is rather arbitrary and it's "forward" likely has no relation to what you consider the forward for your ship to be. However, this coordinate system is convenient for autopilot scripts to work from because it is common between all blocks.

    The coordinate system that usually matches what you consider the forward for your ship is the cockpit's block-local coordinate system. (That is, the cockpit's "forward" vector is normally what you think of as the front of your ship.) A thruster points in its "forward" direction (and applies force in the opposite, "back" direction). Similarly, the "yaw" setting of a gyroscope will apply torque around its "up" vector (opposite from the flat base plate). (This is why, if you place gyroscopes at different orientations, and give them the same override values you won't get the rotation you were expecting.)
    Last edited: Jul 21, 2015
  21. Wicorel Senior Engineer

    Ok, vector math is melting my brain... ouch.

    Can somebody help me translate this code to not need set blocks? I could pass it a remote control block, or have it find one.

    Note: I didnt' write this code in the first place. It's from NAV Autopilot by arrnor. I have modified the script for my miner to add some features.

    Note2: I know that yawAngle is not really an angle...
  22. JoeTheDestroyer Junior Engineer

    var l=new List<IMyTerminalBlock>();
    MatrixD refOrientation=GetBlock2WorldTransform(l[0]);
    Vector3D center=l[0].GetPosition();
    Vector3D back=center+1.0*Vector3D.Normalize(refOrientation.Back));
    Vector3D up=center+1.0*Vector3D.Normalize(refOrientation.Up));
    Vector3D right=center+1.0*Vector3D.Normalize(refOrientation.Right));
    Assuming calculateDistance() does what I think it does, that should do it.

    Also, it looks like the code is somewhat dependent on how far apart the reference blocks are, so you might have to adjust the "1.0" to match.
  23. Wicorel Senior Engineer

    oops. didn't include calculateDistance, sorry.. It probably does what you think it does.

    Here it is, just in case:

    So these are the virtual locations of those blocks, right?
  24. JoeTheDestroyer Junior Engineer

    Rather than virtual, I would call them imaginary, but yeah.

    Using the remote control (or other cockpit-type block) as reference (and "center"), "back" will be the (world) location of an imaginary block 1 meter behind "center". Similarly for "up" and "right".

    It would be better to work w/ the directions directly, but this is probably the easiest way to modify the code w/o risking problems elsewhere.
  25. Wicorel Senior Engineer


    Here's the code as I modified and fixed:

    MatrixD refOrientation=GetBlock2WorldTransform(gpsCenter);
  26. Lynnux Junior Engineer

    Small improvement (according to the comments MatrixD.Rescale is faster than MatrixD.CreateScale):

    MatrixD GetGrid2WorldTransform(IMyCubeGrid grid)
        Vector3D origin=grid.GridIntegerToWorld(new Vector3I(0,0,0));
        Vector3D plusY=grid.GridIntegerToWorld(new Vector3I(0,1,0))-origin;
        Vector3D minusZ=grid.GridIntegerToWorld(new Vector3I(0,0,-1))-origin;
        return MatrixD.Rescale(MatrixD.CreateWorld(origin,minusZ,plusY),grid.GridSize);
  27. TangoFoxtrot Apprentice Engineer

    Have you ever rewritten this to use the new functions?
    I've been stealing borrowing reverse engineering the matrix rotation bit for use in a turret, but I'm trying to use the new functions instead for some bits, and it's not going so well.
  28. Lynnux Junior Engineer

    @TangoFoxtrot Yes, I'm using the new functions now. I've uploaded an update of the Black Adder with the Radar Display and Catlanding (new) two days ago. Take a look to the SW in the "BA Control Unit" block, maybe it can help you.
    Are you using GetAzimuthAndElevation(Vector3...) by coincidence ? Then you have normalize the vector before passing it to this function, it's bugged.
  29. TangoFoxtrot Apprentice Engineer

    I can't seem to open the Adder into my world. Mind posting the new script?
    No, I've not been using that to get the elevation/azimuth. I've been taking the rotated coordinates and making a set of spherical coordinates out of them, which gets me azimuth, inclination, and range.
    Elevation I get by subtracting 90 degrees from the inclination.
    It seems to work.

    This is what I'm currently using to rotate the relative position of the target:
    Vector3D RelativePos = new Vector3D (RelativeX, RelativeY, RelativeZ) ;
    MatrixD TurretOrient =  MatrixD.Invert(MatrixD.CreateFromDir(CopiedGetUp(Base),CopiedGetForward(Base)));
    Vector3D TargetLock = Vector3D.TransformNormal(RelativePos, TurretOrient);
    This started as a straight copy of your radar position rotation math. Strangely, switching the two vectors made it work, more or less-It tracks right as long as the target is in the 'forward' hemisphere of the turret, and has the right inclination and the opposite azimuth when the target is in the 'backward' hemisphere.
    It's a major improvement over anything else I've tried thus far, but I wish I knew why it's tracking the opposite azimuth like that.
    EDIT: apparently x and D is :D
    Last edited: Aug 14, 2015
  30. Lynnux Junior Engineer

    It's Matrix.CreateFromDir(Forward,Up), I think that's why. You've switched the parameters.

    Matrix CockpitOrient: reference direction
    Vector TargetDiff: vector of the target position (in the world) relative to the position of the cockpit (in the world)
    TransformNormal: now we take the rotation of the cockpit in the world into account, rotating the vector as if the cockpit would be the reference orientation of the world

    MatrixD CockpitOrient = MatrixD.Invert(MatrixD.CreateFromDir(CockpitBase.Forward,CockpitBase.Up));
    Vector3D TargetDiff = TargetPos - Cockpit.GetPosition();
    Vector3D pos = Vector3D.TransformNormal(TargetDiff, CockpitOrient);
    double yaw = MathHelper.ToDegrees(Math.Atan2(-pos.GetDim(0), -pos.GetDim(2)));
    double pitch = MathHelper.ToDegrees(-Math.Acos(pos.GetDim(1) / pos.Length())+ MathHelper.PiOver2);
    Concerning the Black Adder: Vas' Battery Rebalance mod is reporting a critical error now but I can still load it. Maybe downloading all the required mods for the first time takes a lot of time. Thanks for reporting.
    Last edited: Aug 14, 2015
Thread Status:
This last post in this thread was made more than 31 days old.