THE MATH OF AUTO AIMING SCRIPT
# 1 TRANSFORMATION MATRIX LAYOUT
The game is based on DirectX library then it use Left-Handled coordinate system and row-major matrices format:
<center> </center>
Usually i use my school maths knowledge, like OpenGL, where Matrices are in column-major notation :
<center> </center>
The conversion between two layouts is simply the transpose. Take care about matrix multiplication because you need to invert the order of matrices
<center> </center>
A geometric representation of affine transformation matrix is a new coordinate system, the main transformations are Rotation, Scale and Translation. To fuse all transformation in one matrix require three matrix multiplications, the order is important.
In the example I do first Scale , second Rotation and last Translation, in the second image I do first Translate , second Rotation and last Scale.
<center> </center>
The first way is the commonly used because it's more intuitive, notice that using math notation the order is TRS but in DirectX notation is SRT , more intuitive.
<center> </center>
<center> </center>
To convert each point from untransformed object to transformed object you have to do a simple Vertex-Matrix multiplication, in DirectX notation :
π β π = πβ²
πβ² β πβ1 = π
<center>   </center>
If you want to do in math notation the formula is π β π = πβ²
These transformation are done by function:
Pβ = VRageMath.Vector3.Transform(P, ref M);
To optimize I prefer use always function what contain βrefβ because struct will not be copied.
# 2 CHANGE COORDINATE SYSTEM
If you want to change the coordinate system of a point you have simply do a vertex transformation, example if P is in M1 system, to convert in M2 system you have to do
ππ1 β π2β1 = ππ2 eq2.1
Change the coordinate system of a direction is more simple because we remove Translation and Scale operations. We can use only Rotation part of matrix, if matrix is orthogonal the inverse of Rotation is equal to Transpose of Rotation
<center></center>
Inverse of Rotation can be calculate using: Rot.TransposeRotationInPlace();
When there are a concatenation of transformation you can simply multiply each transformations. Example if M2 transform a point from M1 and M3 from M2
ππ1 = ππ2 β π2
ππ2 = ππ3 β π3
ππ1 = ππ3 β π3 β π2
π·π΄π β (π΄π β π΄π)βπ = π·π΄π
π·π΄π β π΄βππ β π΄βππ = π·π΄π
# 3 COORDINATES SYSTEM
To show the coordinate system in game you can create four GPS points:
<center></center>
O = {0, 0, 0}
X = {10, 0, 0}
Y = {0, 10, 0} Z = {0, 0, 10}
From VRageMath the definition is:
VRageMath.Vector3 |coordinates
-----------|---------------
Up | {0, +1, 0}
Down | {0, -1, 0}
Right |{+1, 0, 0}
Left |{-1, 0, 0}
Forward | {0, 0, -1}
Backward | {0, 0, +1}
I define four principal coordinates system:
Global GPS = Matrix Identity.
Ship = Matrix of floating points, change every time ship move on space.
Grid = Matrix of integers, generated only first time you create a new ship.
Block = Matrix of integers, generated only first time you create the block in the ship.
# 3.1 FIND COORDINATE SYSTEM OF SHIP
The game use a lot of this concatenations, the first is from GPS coordinate to Ship coordinate. The only way to build this matrix is use the common properties of generic block find in the game lib:
VRageMath.Vector3D PGPS = Sandbox.ModAPI.IMyEntity.GetPosition();
That return the position of blockβs center in the global world coordinates. I suggest you to use 1x1 block to avoid issues.
The game DONβT APPLY MIRROR OPERATIONS to blocks, in fact you never see mirrored block, example Refineries block have the correct orientation also when you build with symmetric tool. To get the correct matrix I use only two of Right, Up, Backward vectors, and calculate third with Cross product.
For a Left-Hand Coordinate System you can use one of these and remember that order of Cross
Product is important
<center> </center>
π = π Γ π
π = π Γ π
π = π Γ π
```
void GetCoordinateSystem(ref Vector3 Backward, ref Vector3 Up, ref Matrix Rotation) {
Vector3 Right = Vector3.Cross(Up, Backward);
Right.Normalize();
Backward.Normalize();
Up.Normalize();
Rotation.Right = Right;
Rotation.Up = Up;
Rotation.Backward = Backward; }
```
So I need tree block called VECTOR_BACKWARD ; VECTOR_UP ; VECTOR_ORIGIN, the Right vector can be derived using Cross(Up, BACKWARD)
```
Vector3 Backward = blockbackward.GetPosition() - blockcenter.GetPosition();
Vector3 Up = blockup.GetPosition() - blockcenter.GetPosition();
GetCoordinateSystem(ref Backward, ref Up, ref ShipRotation);
```
So if the Ship Matrix equal to Identity the ship are in this orientation :
<center> </center>
# 3.2 FIND COORDINATE SYSTEM OF GRID
<center> </center>
The second coordinate system is relative to Grid (local respect the ship) . This coordinate system is aligned to axis and use integer values because is a voxel system, in fact Vector3I use integers and can be implicit convert to Vector3
VRageMath.Vector3I PGRID = Sandbox.ModAPI.IMyCubeBlock.Position;
With a simple test (grid rotation is Identity Matrix) I notice that Grid coordinate are not aligned with coloured gizmo that you can see in the image:
If I write the simple code:
```
PosX = GridTerminalSystem.GetBlockWithName("+X");
NegX = GridTerminalSystem.GetBlockWithName("-X");
PosY = GridTerminalSystem.GetBlockWithName("+Y");
NegY = GridTerminalSystem.GetBlockWithName("-Y");
PosZ = GridTerminalSystem.GetBlockWithName("+Z");
NegZ = GridTerminalSystem.GetBlockWithName("-Z");
Origin = GridTerminalSystem.GetBlockWithName("Origin");
Vector3I px = PosX.Position - Origin.Position;
Vector3I nx = NegX.Position - Origin.Position;
Vector3I py = PosY.Position - Origin.Position;
Vector3I ny = NegY.Position - Origin.Position;
Vector3I pz = PosZ.Position - Origin.Position; Vector3I nz = NegZ.Position - Origin.Position;
text = string.Format("+X {0}\n-X {1}\n+Y {2}\n-Y {3}\n+Z {4}\n-Z
{5}\n",px.ToString(),nx.ToString(),py.ToString(),ny.ToString(),pz.ToString( ),nz.ToString());
```
Block In Game | Vector3I |Direction
---------------|---------------------|---------
+X (red) | {+1,0,0} |
-X |{-1,0,0} |
+Y (green) | {0,0,-1} |
-Y | {0,0,+1} |
+Z (blue) | {0,+1,0}|
-Z |{0,-1,0} |
So the correct calculation for Grid-Coordinates:
```
Matrix GridRotation = IDENTITY; GridRotation.Right = FixCoordiate(px); GridRotation.Up = FixCoordiate(py); GridRotation.Backward = FixCoordiate(pz);
Vector3 FixCoordiate(Vector3 ingame) { return new Vector3(ingame.GetDim(0),-ingame.GetDim(2),ingame.GetDim(1)); }
```
Example: if you change the grid orientation you can see that β+Xβ block is aligned with green line so in the matrix the Right Axis is {0,1,0} and so onβ¦
3.3 FIND COORDINATE SYSTEM OF GYROSCOPE
The third coordinate system is of Gyroscope (relative to grid). Same of grid, use voxel system. The problem is that I need to normalize MyOrientation class to default xyz coordinates of grid:
Base6Directions.Direction | Index | Vector3
---------------------------------|--------------------|--------
Forward | 0 | +Y
Backward | 1 |-Y
Left |2 |-X
Right | 3 |+X
Up |4 |+Z
Down | 5 | -Z
To Access to tree vectors without crash:
```
void GetCubeBlockMatrix(IMyCubeBlock block, ref Rotation) {
MyBlockOrientation orientation = block.Orientation;
Base6Directions.Direction L = orientation.TransformDirection(Base6Directions.Direction.Left);
Base6Directions.Direction U = orientation.TransformDirection(Base6Directions.Direction.Up);
Base6Directions.Direction F = orientation.TransformDirection(Base6Directions.Direction.Forward); Rotation.Right = Table[(int)L];
Rotation.Up = Table[(int)U];
Rotation.Backward = Table[(int)F];
}
```
Rotation is Identity if :
RightAxis = Direction.Left
UpAxis = Direction.Up
BackwardAxis = Direction.Forward
In the image you see the gyroscope in Identity Rotation (aligned with Grid-Coordinate Gizmo)
# 4 FIND ELEVATION AND AZIMUTH
We need to know the rotation that gyroscope have to do to orient Backward vector of Ship to βlookβ a point in the space. Look vector will be convert in ship coordinate system with equation 2.1
```
Vector3 Look = Target - Origin; ShipRotation.TransposeRotationInPlace(); Look = Vector3.Transform(Look , ref ShipRotation); Look.Normalize();
```
The rotations around a vector is calculated with Atan2 because I need also the sign of angle. The Elevation is X rotation and azimuth is Y rotation. For Twist Z rotation the zero is when Y of ship is aligned with Up vector but when Ship Backward and Z become equal, the twist become unstable. Using System.Math.Atan2(y, x) I show you for example all positive angles
<center> </center>
ππ = π΄π‘ππ2(ππ¦ ,ππ§) ππ = π΄π‘ππ2(βππ₯ ,ππ§) ππ = π΄π‘ππ2(ππ₯ ,ππ¦)
To respect the Left-Hand-Rule, I set a negative sign for angle Y and Z.
<center>  </center>
This code work when Gyroscope Rotation and Grid Rotation are Identity:
```
angles.SetDim(0 ,(float)Math.Atan2(Look.GetDim(1), Look.GetDim(2))); angles.SetDim(1 ,(float)Math.Atan2(-Look.GetDim(0), Look.GetDim(2))); angles.SetDim(2 , 0);
gyro.SetValueFloat("Yaw", angles.GetDim(2) * -6.0f / 3.14f); gyro.SetValueFloat("Pitch", angles.GetDim(0) * -6.0f / 3.14f); gyro.SetValueFloat("Roll", angles.GetDim(1) * -6.0f / 3.14f);
```
But what happens when Gyroscope isnβt in the default Identity Rotation?
Example: the target is (0,0,1) so the Gyroscope have to return Zero Y rotation, but you build a gyroscope rotated by Y+180Β° respect default.
The Atan2 return the angle of Look vector but in Gyroscope coordinate, in this case Zβ = -Z and Xβ = -X.
If the Look vector will be transformed in Gyroscope coordinate the formula will return a rotation of Y+180Β°, and itβs WRONG because the result must the same (Zero Y rotation). So the βangle-vectorβ not depend by Gyroscope rotation (I not investigate in details)
ππππππ = (ππ₯,ππ¦,ππ§)
But angle-vector must be transformed in Gyroscope coordinate
πππ‘ππ‘ππππ = ππππππ β πΊπ¦πππΆππππβ1
```
GyroRotation.TransposeRotationInPlace(); ShipToGyro = GridRotation * GyroRotation;
angles = Vector3.Transform(angles , ref ShipToGyro);
gyro.SetValueFloat("Yaw", angles.GetDim(0)); gyro.SetValueFloat("Pitch", angles.GetDim(1)); gyro.SetValueFloat("Roll", angles.GetDim(2));
```
This matrix can be precomputed at the beginning for each gyroscope you are using, because donβt change during script, change only if you build a new gyroscope or a new ship
# 5 THE LOOP
This script are used to calculate continually gyroscope velocity compensation relative to a Point in the space, canβt run only once. To do a sort of game loop I use this layout:
I build a TimerBlock, in the actionβs list I insert a βTrigger_Nowβ and βScript_Runβ
The script starts with:
```
int Updates = 0; bool initialized = false; void Main()
{
if (!initialized) GridBlockScan(); if ((Updates % 10) == 0)
{
gyroController.UpdateTarghet(Target);
}
if ((Updates % 50) == 0)
{
PrintDebugToPanel();
}
if (Updates++ > 1000)
{
Updates = 0;
}
}
```
The initialized flag is used to do preliminaries only once, example get the block of ship. In this case torpedo doesnβt change block during its life. With {Updates % 10 == 0} I can reduce the frequency of updates for some functions, example: I donβt want print the text to panel every time, only sometimes. The time elapsed from a Update and Next Update is define by gameβs physic dt (I suppose frame rate arenβt used to update the gameβs logics) and I suppose is about 10ms, but is not important know exactly this value, you only have to know that is a faaaaaaaaaster value.
Using βTimer_Startβ you can generate a maximum dt of 1 second but is too big for my purpose because torpedo need a fast calculation when speed in space.
6 TESTING
The minimum layout to test, ship and gyroscopes in random orientations. More gyroscopes you use, faster is the velocity.
TODO : remove βvector_upβ block because is not necessary ?
```
// the GPS coordinates
Vector3 Target = new Vector3(0,0,0);
string VECTOR_ORIGIN = "VECTOR_ORIGIN"; string VECTOR_BACKWARD = "VECTOR_BACKWARD"; string VECTOR_UP = "VECTOR_UP";
static Matrix IDENTITY = new Matrix(
1,0,0,0,
0,1,0,0,
0,0,1,0,
0,0,0,1);
static Vector3[] Table = new Vector3[]
{ new Vector3( 0, 1, 0), new Vector3( 0,-1, 0), new Vector3(-1, 0, 0), new Vector3( 1, 0, 0), new Vector3( 0, 0, 1), new Vector3( 0, 0,-1)
};
static float RadToVel = 6.0f / 3.14f; static float DampCoeff = 2.0f; string error = ""; string textfast = "";
string textslow = "";
int Updates = 0;
bool initialized = false;
List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
IMyTextPanel mypanel0, mypanel1;
// Z+ block
IMyTerminalBlock blockbackward;
// Y+ block
IMyTerminalBlock blockup;
// Origin block
IMyTerminalBlock blockcenter;
// rotation of ship respect global GPS coordinates, change every frames
Matrix ShipRotation = IDENTITY;
// rotation of grid respect to ship, it's fixed, have only integers values [-1,0,1]
Matrix GridRotation = IDENTITY;
List<IMyTerminalBlock> gyroscopes_obj = new List<IMyTerminalBlock>(); List<Matrix> gyroscopes_mat = new List<Matrix>();
void Main()
{
// frequent updates if ((Updates % 10) == 0)
{
if (initialized)
{
textfast = "";
UpdateTarghet(ref Target);
PrintDebugToPanel(textslow, textfast);
} else
{
PrintDebugToPanel(error, error);
}
}
// block scanning are called rarely, or only once if you need to optimize more if ((Updates % 100) == 0)
{
textslow = "";
initialized = GridBlockScan();
}
// reset loop and/or output something if (Updates++ > 1000)
{
Updates = 0;
}
} bool GridBlockScan()
{
IMyTerminalBlock item; blocks.Clear();
// get the panel to debug informations
GridTerminalSystem.GetBlocksOfType<IMyTextPanel>(blocks); if (blocks.Count>0) mypanel0 = blocks[0] as IMyTextPanel; else mypanel0 = null;
if (blocks.Count>1) mypanel1 = blocks[1] as IMyTextPanel; else mypanel1 = null;
// get the block used to understand orientations
blockbackward = GridTerminalSystem.GetBlockWithName(VECTOR_BACKWARD); if (blockbackward==null)
{ error = "Missing block in forward position with name : " + VECTOR_BACKWARD; return false;
}
blockup = GridTerminalSystem.GetBlockWithName(VECTOR_UP); if (blockup==null)
{ error = "Missing block in Up position with name : " + VECTOR_UP; return false;
} blockcenter = GridTerminalSystem.GetBlockWithName(VECTOR_ORIGIN); if (blockcenter==null)
{ error = "Missing block in Origin position with name : " + VECTOR_ORIGIN; return false;
}
ComputeGridCoordSystem(ref GridRotation);
if (mypanel0!=null) textslow+= "Grid:\n" + MatrixToString(ref GridRotation);
// get all gyroscopes used to control the orientation gyroscopes_obj.Clear();
GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyroscopes_obj); if (gyroscopes_obj.Count == 0)
{
error = "Missing gyroscopes"; return false;
} else {
gyroscopes_mat.Clear();
for (int i=0;i<gyroscopes_obj.Count;i++)
{
Matrix ShipToGyro = IDENTITY;
ComputeGyroCoordSystem(gyroscopes_obj[i], ref ShipToGyro); gyroscopes_mat.Add(ShipToGyro); //matrix are passed as copy
}
}
return true;
}
// Intensive calculation, update every delta-time void UpdateTarghet(ref Vector3 Target)
{
Vector3 Angles = new Vector3(0,0,0);
Vector3 gyrorotations = Angles;
GetShipAngles(ref Target, ref Angles);
for (int i=0;i<gyroscopes_obj.Count;i++)
{
IMyGyro gyro = gyroscopes_obj[i] as IMyGyro; Matrix ShipToGyro = gyroscopes_mat[i];
gyrorotations = Vector3.Transform(Angles , ref ShipToGyro);
ApplyRotationAxis(gyro, ref gyrorotations);
if (mypanel1!=null) textfast += string.Format("Gyro{0} : {1}\n", i , Vector3ToString(gyrorotations));
}
}
// Get the 3 angles of target vector relative to ship rotation void GetShipAngles(ref Vector3 Target, ref Vector3 Angles) {
// Update the current Ship Matrix
Vector3 Origin = blockcenter.GetPosition();
Vector3 Backward = blockbackward.GetPosition() - Origin;
Vector3 Up = blockup.GetPosition() - Origin;
GetCoordinateSystem(ref Backward, ref Up, ref ShipRotation); ShipRotation.TransposeRotationInPlace();
// Get ship direction and conversion from World (GPS) coordinate to Ship's Coordinates Vector3 Look = Target - Origin;
Look = Vector3.Transform(Look , ref ShipRotation);
Look.Normalize();
// elevation = aX = Atan2(Y,Z)
Angles.SetDim(0 ,(float)Math.Atan2(Look.GetDim(1), Look.GetDim(2)));
// azimuth = aY = Atan2(X,Z) negative sign if not respect left-handle-rule with rotation direction
Angles.SetDim(1 ,(float)Math.Atan2(-Look.GetDim(0), Look.GetDim(2))); // twist = aZ = Atan2(Y,X)
//angles.SetDim(2 ,(float)Math.Atan2(Look.GetDim(0), Look.GetDim(1))); Angles.SetDim(2, 0.0f);
if (mypanel1!=null) textfast += "Angles: " + Vector3ToString(Angles) + "\n";
}
// Get grid orientation
void ComputeGridCoordSystem(ref Matrix Rotation) {
Vector3I origin = blockcenter.Position;
Vector3 B = AlignGridCoordToXYZ(blockbackward.Position - origin); Vector3 U = AlignGridCoordToXYZ(blockup.Position - origin);
if (mypanel0!=null) textslow = string.Format("B {0}\nU {1}", Vector3ToString(B),Vector3ToString(U)); GetCoordinateSystem(ref B, ref U, ref Rotation);
}
// Get precomputed transformation for gyroscope
void ComputeGyroCoordSystem(IMyTerminalBlock gyro, ref Matrix GyroRotation) {
// Get block orientation
GetCubeBlockMatrix(gyro, ref GyroRotation);
// Precomputed transformation
GyroRotation.TransposeRotationInPlace();
GyroRotation = GridRotation * GyroRotation; }
// update a orientation matrix system
void GetCoordinateSystem(ref Vector3 Backward, ref Vector3 Up, ref Matrix Rotation) {
Vector3 Right = Vector3.Cross(Up, Backward);
Right.Normalize();
Backward.Normalize();
Up.Normalize();
Rotation.Right = Right;
Rotation.Up = Up; Rotation.Backward = Backward;
}
// Get the Rotation Matrix of any blocks in Grid-Coordinates void GetCubeBlockMatrix(IMyCubeBlock block, ref Matrix Rotation) {
MyBlockOrientation orientation = block.Orientation;
Base6Directions.Direction L = orientation.TransformDirection(Base6Directions.Direction.Left);
Base6Directions.Direction U = orientation.TransformDirection(Base6Directions.Direction.Up);
Base6Directions.Direction F = orientation.TransformDirection(Base6Directions.Direction.Forward); Rotation.Right = Table[(int)L];
Rotation.Up = Table[(int)U];
Rotation.Backward = Table[(int)F];
}
// Set Gyroscope rotation relative to Gyroscope-Coordinates void ApplyRotationAxis(IMyGyro gyro, ref Vector3 rotation) {
//interpolate from [-PI,PI]rad to [-6,6]rpm of gyroscope // TODO : implement a coefficient value to dampen rotations
gyro.SetValueFloat("Yaw", ClampGyroVelocity(rotation.GetDim(1) * RadToVel * DampCoeff)); gyro.SetValueFloat("Pitch", ClampGyroVelocity(rotation.GetDim(0) * RadToVel * DampCoeff)); gyro.SetValueFloat("Roll", ClampGyroVelocity(-rotation.GetDim(2) * RadToVel * DampCoeff)); }
// Fix game coordinate to respect the "game grid gizmo" Vector3 AlignGridCoordToXYZ(Vector3 ingame)
{
float y = ingame.GetDim(1); ingame.SetDim(1, -ingame.GetDim(2));
ingame.SetDim(2, y); return ingame;
}
float ClampGyroVelocity(float vel)
{ if (vel < -6.0f) return -6.0f; else if (vel > 6.0f) return 6.0f; else return vel;
}
/////////////////////// DEBUGGER ///////////////////////
// print only rotation part
string MatrixToString(ref Matrix matrix)
{
string str = "";
str += Vector3ToString(matrix.Right) + "\n"; //row 1 str += Vector3ToString(matrix.Up) + "\n"; //row 2 str += Vector3ToString(matrix.Backward) + "\n"; //row 3 return str;
}
string Vector3ToString(Vector3 vector)
{
return string.Format("{0:0.00} {1:0.00} {2:0.00}", vector.GetDim(0), vector.GetDim(1), vector.GetDim(2));
} string Vector3IToString(Vector3I vector)
{
return vector.ToString();
}
void PrintDebugToPanel(string testoA, string testoB)
{
if (mypanel0 != null)
{ mypanel0.WritePublicText(testoA); mypanel0.ShowPublicTextOnScreen();
} if (mypanel1 != null)
{ mypanel1.WritePublicText(testoB); mypanel1.ShowPublicTextOnScreen(); }
}
```
hiveblocks